Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Newer
Older
100644 115 lines (100 sloc) 4.241 kb
26921e7 @JPMoresmau initial commit
authored
1 -- | moves a robot using two tracks on engines B & C
2 -- the robot moves tills the tactile sensor is pressed, then it reverse a little, turns and start again
3 module Main where
4
5 import Robotics.NXT
6
7 import System.Environment (getArgs)
8 import Control.Concurrent (threadDelay,forkIO)
9 import Control.Monad.IO.Class (liftIO)
10 import Control.Monad (when, forever)
11
12 import Data.IORef
13 import System.IO
14
15 -- | the main method
16 main :: IO()
17 main = do
18 (device:_)<-getArgs
19 iorC<-newIORef True
20 forkIO (do
21 hSetBuffering stdin NoBuffering
22 hSetBuffering stdout NoBuffering -- does not Work on windows
23 putStrLn "press space to stop robot"
24 waitForStop iorC
25 putStrLn "stopping..."
26 return ()
27 )
28 withNXT device (do
29 reset
30 forever $ loop iorC
31 reset
32 liftIO $ threadDelay 1000000 -- wait before killing everything probably not needed after reset
33 )
34 --killThread tid
35
36 -- | waits for user to press space, this stops the robot
37 waitForStop :: IORef Bool-> IO()
38 waitForStop iorC=do
39 c<-getChar
40 if c == ' ' then
41 do atomicModifyIORef iorC (\ a -> (False, a))
42 return ()
43 else waitForStop iorC
44
45 -- | reset the NXT brick motors
46 reset :: NXT()
47 reset = mapM_ resetMotor [B,C]
48
49 -- | reset a motor
50 resetMotor :: OutputPort -- ^ the output port
51 -> NXT()
52 resetMotor p= mapM_ (resetMotorPosition p) [InternalPosition,AbsolutePosition,RelativePosition]
53
54 -- | the main loop for the robot
55 loop :: IORef Bool -- ^ the stop flag ioref
56 -> NXT()
57 loop iorC= do
58 move iorC [B,C] 75 [0,0] 0 -- move forever
59 setInputModeConfirm One Switch BooleanMode -- set the sensor on port One to switch mode
60 pollForScaled iorC One 0 -- wait for sensor to be triggered
61 stop [B,C] -- stop
62 move iorC [B,C] (-75) [0,0] 360 -- reverse
63 move iorC [B,C] (-75) [100,-100] 360 -- turn
64 stop [B,C] -- stop
65
66
67 -- | stop the motors on the given port
68 stop :: [OutputPort] -> NXT()
69 stop =mapM_ (\p->setOutputStateConfirm p 0 [Regulated,Brake] RegulationModeMotorSync 0 MotorRunStateIdle 0)
70
71 -- | move motors on the given ports till the limit has been reached or the stop signal sent
72 move :: IORef Bool -- ^ the stop signal ioref
73 -> [OutputPort] -- ^ the output port
74 -> OutputPower -- ^ the power to apply
75 -> [TurnRatio] -- ^ the turn ratio between engine
76 -> TachoLimit -- ^ the move limit
77 -> NXT()
78 move iorC ports power ratios limit=pollForStop iorC $ do
79 let port1= head ports
80 OutputState _ _ _ _ _ _ _ count _ _<-getOutputState port1
81 mapM_ (\(p,r)->setOutputStateConfirm p power [Regulated,MotorOn] RegulationModeMotorSync r MotorRunStateRunning limit) $ zip ports ratios
82 when (limit>0) (pollForCount iorC port1 (count+limit))
83
84 -- | wait for the given motor to have reached the limit
85 pollForCount :: IORef Bool -- ^ the stop signal ioref
86 -> OutputPort -- ^ the output port
87 -> TachoLimit -- ^ the limit
88 -> NXT()
89 pollForCount iorC port limit=pollForStop iorC $ do
90 OutputState _ _ _ _ _ state _ count _ _<-getOutputState port
91 when (state/=MotorRunStateIdle && count<limit) (do
92 liftIO $ threadDelay 500
93 pollForCount iorC port limit
94 )
95
96 -- | wait for the input value to reach the given value
97 pollForScaled :: IORef Bool -- ^ the stop signal ioref
98 -> InputPort -- ^ the input port
99 -> ScaledValue -- ^ the value to wait for
100 -> NXT()
101 pollForScaled iorC port v=pollForStop iorC $ do
102 InputValue _ _ _ _ _ _ _ scalV _<-getInputValues port
103 when (scalV==v) ( do
104 liftIO $ threadDelay 500
105 pollForScaled iorC port v
106 )
107
108 -- | only perform the given action if the user hasn't said stop
109 pollForStop :: IORef Bool -- ^ the stop signal ioref
110 -> NXT() -- ^ the action to perform
111 -> NXT()
112 pollForStop iorC f=do
113 c<-liftIO $ atomicModifyIORef iorC (\a->(a,a))
114 when c f
115
Something went wrong with that request. Please try again.