Tweaked Turtle3 behavior for IROS demo.

`Waypoint nav now oscillates due to interaction between the PID integrator and an added delay to control output.`
Anthony Cowley committed Sep 27, 2011
1 parent 8cd8737 commit 6face7b1d5ee7e016ff3f94b59c3f6b03f84edf1
Showing with 7 additions and 6 deletions.
1. +7 −6 Examples/Turtle/src/Turtle3.hs
 @@ -8,7 +8,7 @@ import Data.Complex import Ros.Logging import Ros.Node import Ros.Topic (repeatM, force, dropWhile, metamorphM, yieldM) -import Ros.TopicUtil (everyNew, interruptible, forkTopic, topicOn) +import Ros.TopicUtil (everyNew, interruptible, forkTopic, topicOn, subsample) import Ros.Util.PID (pidTimedIO) import AngleNum import Ros.Turtlesim.Pose @@ -29,7 +29,7 @@ destinations :: (Functor m, Monad m) => Topic m Point -> Topic m Pose -> Topic m Point destinations goals poses = metamorphM (start (p2v <\$> poses)) goals where start t g = yieldM g (go g t) - go g t g' = let keepGoing x = magnitude (g - x) > 1.5 + go g t g' = let keepGoing x = magnitude (g - x) > 0.5 in force (dropWhile keepGoing t) >>= yieldM g' . go g' p2v (Pose x y _ _ _) = x :+ y @@ -39,19 +39,20 @@ toGoal (pos,goal) = (magnitude v, thetaErr) where v = goal - (x pos :+ y pos) thetaErr = toDegrees \$ angle (phase v) - angle (theta pos) --- Run a PID loop on angular velocity to converge to a bearing for the goal. +-- Run a PID loop on angular velocity to converge to a bearing for the +-- goal. steering :: Topic IO (Float,Float) -> Topic IO Velocity -steering = topicOn snd (Velocity . fst) ((\$ 0) <\$> pidTimedIO 1 0 0) +steering = topicOn snd (Velocity . fst) ((\$ 0) <\$> pidTimedIO 0.01 4.0 0) -- Compute position and bearing error to goal, clamp linear and -- angular velocities, and generate a steering command. navigate :: Topic IO (Pose,Point) -> Topic IO Velocity -navigate = steering . fmap ((clamp *** clamp) . toGoal) +navigate = steering . fmap ((clamp *** id) . toGoal) where clamp x = signum x * min 2 (abs x) main = runNode "HaskellBTurtle" \$ do enableLogging (Just Warn) poses <- subscribe "/turtle1/pose" let goals = destinations (interruptible getTraj) poses - commands = navigate \$ everyNew poses goals + commands = subsample 15 \$ navigate \$ everyNew poses goals advertise "/turtle1/command_velocity" commands