******************************************************************************
*                                                                            *
*       Module:         WallFollower                                         *
*       Purpose:        CS-474 Assignment 3                                  *
*       Programmer:     Paul Hudak                                           *
*                                                                            *
******************************************************************************

> module WallFollower where

> import Frob
> import RobotSim

Zhanyong Wan's World

> wansWorld = maybeReadWorldFromPath "as33"

> maybeReadWorldFromPath p = do
>    w <- readWorldFromPath p
>    case w of
>       Right w' -> return w'
>       Left s   -> return []

Data type for turning left or right:

> data End = NoWall | FrontWall

Range detectors:

> leftD, rightD, frontD, backD :: RangeDetection i => Behavior i Length
> 
> leftD  = lift1 (!! 1) rangeReadings
> rightD = lift1 (!! 3) rangeReadings
> frontD = lift1 (!! 0) rangeReadings
> backD  = lift1 (!! 2) rangeReadings

Main simulator run:

> main = do w <- wansWorld
>           runSimRunT w () wallFollowerT

Main control parameters / constants:

> rs          :: Behavior a Speed
> wd,ld,d1,d2 :: Behavior a Length
> 
> rs = 0.3 -- nominal robot speed
> wd = 0.3 -- distance from wall
> ld = 1.0 -- distance at which robot is "lost"
> d1 = 0.0 -- distance robot moves forward before left turn
> d2 = 0.6 -- distance robot moves forward after  left turn

Main wall-follower (including inside and outside corners):

> wallFollowerT ::
>   (RobotProperties i, Odometry i, RangeDetection i, WheelControl o) =>
>    RobotTask s i o e
> 
> wallFollowerT = do
>   end <- followLeftWallT rs wd ld
>   case end of
>     NoWall    -> do moveRelative d1
>                     stopT
>                     turnLeft
>                     findWallT
>                     wallFollowerT
>     FrontWall -> do turnRight
>                     wallFollowerT

Go forward until wall is on left:

> findWallT :: (RangeDetection i, WheelControl o) =>
>   RobotTask s i o ()
> findWallT =
>   forwardT rs
>     `tillT_` (isTrueE (leftD <* ld))

Left wall-follower only:

At speed s, keep distance d from left wall; slow down and stop at
distance d if approaching wall in front; keep going but terminate task
if left wall distance is > r

> followLeftWallT :: (RobotProperties i, RangeDetection i, WheelControl o) => 
>   Behavior i Speed -> Behavior i Length -> Behavior i Length
>   -> RobotTask s i o End
> 
> followLeftWallT s d r =
>   let ds = frontD - d  - 0.2
>       hs = (leftD - d) * 0.3
>       omega = lim 0.2 hs + derivativeB leftD
>       vel   = lim 0.3 ds
>   in forwardRotateT vel omega
>      |.| ifT (isTrueE (leftD >* r)) (return NoWall)
>      |.| ifT (isTrueE (ds  <* 0.1)) (return FrontWall)
> 
> lim m y = max (-m) (min m y)
> 
> ifT event task = do noCommandT `tillT_` event
>                     task

Go forward while rotating:

> forwardRotateT :: (RobotProperties i, WheelControl o) =>
>   Behavior i Speed -> Behavior i AngSpeed -> RobotTask s i o e
> forwardRotateT v omega = liftT $
>     setWheelSpeeds (pairZ (v - dv) (v + dv)) where
>         dv   = robotDiameter * omega / 2

Move forward a distant d and stop:

> moveRelative :: (Odometry i, WheelControl o) => 
>   Behavior i Length -> RobotTask s i o ()
> 
> moveRelative d =
>   do p0 <- snapshotNowT position
>      let rs = magnitude (position .-. lift0 p0) + d
>      forwardT (max 0.3 rs)
>        `tillT_` isTrueE (abs rs <* 0.1)

Turn left and right using turnRelativeT:

> turnLeft,turnRight :: (RobotProperties i, WheelControl o, Odometry i) =>
>   RobotTask s i o ()
> 
> turnLeft  = turnRelativeT (pi/2)
> turnRight = turnRelativeT (-pi/2)
> 
> turnRelativeT :: (RobotProperties i, Odometry i, WheelControl o) =>
>   Angle -> RobotTask s i o ()
> 
> turnRelativeT a =
>   do h0 <- snapshotNowT heading
>      let omega = lift1 normalizeHeading (lift0 (h0+a) - heading)
>      forwardRotateT 0 (lim 0.2 omega)
>        `tillT_` isTrueE (abs omega <* 0.1)

Go forward at the speed specified:

> forwardT :: WheelControl o => Behavior i Speed -> RobotTask s i o a
> forwardT v = wheelSpeedsT v v
> 
> wheelSpeedsT :: WheelControl o => 
>   Behavior i Speed -> Behavior i Speed -> RobotTask s i o e
> wheelSpeedsT vl vr = liftT $ setWheelSpeeds (pairZ vl vr)

Stops the robot (doesn't just set wheel speeds to zero!):

> stopT :: (WheelControl o, Odometry i) => RobotTask s i o ()
> stopT = 
>   do p0 <- snapshotNowT position
>      forwardT 0.0 `tillT_` 
>        isTrueE (derivativeB (magnitude (position .-. lift0 p0)) <* 0.05)