Permalink
Browse files

Moved capture code into seperate module. Defined basic types for comm…

…unication.
  • Loading branch information...
1 parent 82a0029 commit cd5e2989f14669eef2090e30179829ffa725fc63 @mitar committed Sep 11, 2010
Showing with 22 additions and 185 deletions.
  1. +0 −89 lib/Capture.hs
  2. +0 −38 lib/CaptureTypes.hs
  3. +0 −34 lib/CaptureUtils.hs
  4. +6 −6 lib/NXT.hs
  5. +14 −14 lib/NXTData.hs
  6. +1 −3 lib/NXTTypes.hs
  7. +1 −1 lib/blue.c
View
@@ -1,89 +0,0 @@
-module NXT.Capture (initCapture, terminateCapture, getLastRobotPosition, waitForRobotPosition, waitForRobotPosition') where
-
-import Control.Exception
-import Control.Monad.State
-import qualified Data.ByteString.Lazy.Char8 as C
-import Data.Time.Clock.POSIX
-import System.IO
-import System.IO.Error
-import System.Process
-
-import NXT.CaptureTypes
-import NXT.CaptureUtils
-import NXT.NXT
-import NXT.NXTTypes
-
--- TODO Move to configuration file
-enableCapture :: Bool
-enableCapture = True
-
-initCapture :: NXT ()
-initCapture = do
- if not enableCapture
- then return ()
- else do
- -- TODO Move to configuration file
- (_, Just captureOut, _, capturePid) <- io $ createProcess (proc "/home/natrix/files/capture" []){ cwd = Just "/home/natrix/files", std_out = CreatePipe, std_err = Inherit, close_fds = True }
- processRunning <- io $ getProcessExitCode capturePid
- case processRunning of
- Just _ -> fail "Capture process could not start"
- _ -> do
- io $ hSetBuffering captureOut NoBuffering
- let capacity = 10 * maxRobotDescLength
- modify (\s -> s { capture = Just (Capture captureOut capturePid (CaptureBuffer capacity C.empty)) })
-
-terminateCapture :: NXT ()
-terminateCapture = do
- c <- gets capture
- case c of
- Just (Capture _ capturePid _) -> do
- modify (\s -> s { capture = Nothing })
- io $ terminateProcess capturePid
- _ -> return ()
-
-getLastRobotPosition :: NXT (Maybe Robot)
-getLastRobotPosition = do
- c <- gets capture
- case c of
- Nothing -> fail "No capture process"
- Just (Capture captureOut capturePid captureBuffer) -> do
- processRunning <- io $ getProcessExitCode capturePid
- case processRunning of
- Just _ -> captureProcessExited
- _ -> do
- ret <- io $ tryJust (guard . isEOFError) $ slurpInput captureOut captureBuffer
- case ret of
- Left _ -> captureProcessExited -- EOF
- Right (positions, currentBuffer) -> do
- modify (\s -> s { capture = Just (Capture captureOut capturePid currentBuffer) })
- if null positions
- then return Nothing
- else do
- let Robot x y d ct pt _ = head . head $ positions -- positions are stored as lists in the reverse order so (head . head) is the last position from capture process
- currentTime <- io $ getPOSIXTime
- return $ Just $ Robot x y d ct pt currentTime
-
-waitForRobotPosition :: NXT ()
-waitForRobotPosition = do
- _ <- waitForRobotPosition' (-1)
- return ()
-
-waitForRobotPosition' :: Int -> NXT Bool
-waitForRobotPosition' timeout = do
- c <- gets capture
- case c of
- Nothing -> fail "No capture process"
- Just (Capture captureOut capturePid _) -> do
- processRunning <- io $ getProcessExitCode capturePid
- case processRunning of
- Just _ -> captureProcessExited
- _ -> do
- ret <- io $ tryJust (guard . isEOFError) $ hWaitForInput captureOut timeout
- case ret of
- Left _ -> captureProcessExited -- EOF
- Right inputAvailable -> return inputAvailable
-
-captureProcessExited :: NXT a
-captureProcessExited = do
- terminateCapture
- fail "Capture process exited"
View
@@ -1,38 +0,0 @@
-{-# LANGUAGE CPP #-}
-{-# OPTIONS_GHC -fno-warn-orphans #-}
-module NXT.CaptureTypes where
-
-import qualified Data.ByteString.Lazy.Char8 as C
-import Data.Fixed
-import Data.Time.Clock
-import Data.Time.Clock.POSIX
-import Numeric
-import System.IO
-import System.Process
-
--- TODO Change to record
-data Robot = Robot
- Double -- X coordinate of a robot
- Double -- Y coordinate of a robot
- Double -- direction of a robot in radians from positive X axis
- POSIXTime -- capture timestamp in microseconds (as determined by a clock of the remote process)
- POSIXTime -- processed timestamp in microseconds (as determined by a clock of the remote process)
- POSIXTime -- receive timestamp in microseconds (as determined by a clock of this process)
- deriving (Eq, Read, Show)
-
--- Approximate but larger than possible length of Robot data type text description
--- We assume that total length of POSIXTime is at most three times its precision, and 42 characters for data type format structure itself
-maxRobotDescLength :: Int
-#if __GLASGOW_HASKELL__ <= 610
-maxRobotDescLength = length "Robot" + 3 * (floatDigits (undefined :: Double)) + 3 * 3 * (length . show $ resolution (undefined :: E12)) + 42
-#else
-maxRobotDescLength = length "Robot" + 3 * (floatDigits (undefined :: Double)) + 3 * 3 * (length . show $ resolution (undefined :: Pico)) + 42
-#endif
-
-instance Read NominalDiffTime where -- POSIXTime is NominalDiffTime
- readsPrec _ = readFloat
-
-type Capacity = Int
-data CaptureBuffer = CaptureBuffer Capacity C.ByteString
-
-data Capture = Capture Handle ProcessHandle CaptureBuffer
View
@@ -1,34 +0,0 @@
-module NXT.CaptureUtils where
-
-import Control.Exception
-import qualified Data.ByteString.Lazy.Char8 as C
-import System.IO
-
-import NXT.CaptureTypes
-
-slurpInput :: Read a => Handle -> CaptureBuffer -> IO ([a], CaptureBuffer)
-slurpInput = slurpInput' []
- where slurpInput' datalist h buffer@(CaptureBuffer capacity before) = do
- ready <- hReady h
- if not ready
- then return (datalist, buffer)
- else do
- after <- C.hGetNonBlocking h (capacity - (fromIntegral $ C.length before))
- let new = before `C.append` after
- chars = C.unpack new
- (ds, rest) = readInput chars
- if null ds
- then if (fromIntegral $ C.length new) == capacity
- then error "Invalid data from input"
- else return (datalist, (CaptureBuffer capacity new))
- else do
- let rest' = assert ((fromIntegral $ C.length rest') <= capacity) $ C.pack rest
- slurpInput' (ds ++ datalist) h (CaptureBuffer capacity rest')
-
-readInput :: Read a => String -> ([a], String)
-readInput = readInput' []
- where readInput' datalist "" = (datalist, "")
- readInput' datalist string = case reads string of
- [(x, rest)] -> readInput' (x:datalist) rest
- [] -> (datalist, string) -- we probably do not have enough data to read data properly
- _ -> error "Ambiguous parse from input"
View
@@ -29,28 +29,28 @@ import NXT.NXTTypes
-- Appendix 1 - Communication protocol
-- Appendix 2 - Direct commands
--- TODO All functions which requests ModuleInfo could populate module ID cache along the way
+-- TODO: All functions which requests ModuleInfo could populate module ID cache along the way
-- Foreign function call for C function which initialize serial port device on POSIX systems
foreign import ccall unsafe "initserial.h" initSerialPort :: Fd -> IO CInt
--- TODO Move to configuration file
+-- TODO: Move to configuration file
device :: FilePath -- serial port device file
--device = "/dev/tty.NatriX-DevB-1"
device = "/dev/rfcomm0"
--- TODO Move to configuration file
+-- TODO: Move to configuration file
debug :: Bool
debug = False
-io :: MonadIO m => IO a -> m a
+io :: (MonadIO m) => IO a -> m a
io = liftIO
-- Opens and intializes serial port, installs signal handler so that ctrl-c closes the program gracefully
initialize :: IO NXTState
initialize = do
let signals = foldl (flip addSignal) emptySignalSet [virtualTimerExpired]
- blockSignals signals -- we have to block signals from interrupting openFd system call
+ blockSignals signals -- we have to block signals from interrupting openFd system call (fixed in GHC versions after 6.12.1)
fd <- openFd device ReadWrite Nothing OpenFileFlags { append = False, noctty = True, exclusive = False, nonBlock = True, trunc = False }
unblockSignals signals
throwErrnoIfMinus1_ "initSerialPort" $ initSerialPort fd
@@ -59,7 +59,7 @@ initialize = do
mainthread <- myThreadId
_ <- installHandler softwareTermination (Catch (throwTo mainthread UserInterrupt)) Nothing
when debug $ hPutStrLn stderr "initialized"
- return $ NXTState h Nothing [] 0 0 Nothing
+ return $ NXTState h Nothing [] 0 0
-- Stops all NXT activities and closes the handler (and so serial port connection)
terminate :: NXTState -> IO ()
View
@@ -7,35 +7,35 @@ import Data.List
import Data.Word
-- Converts a list of bytes to an unsigned numeric value
-dataToInt :: Integral a => [Word8] -> a -- least significant byte first
+dataToInt :: (Integral a) => [Word8] -> a -- least significant byte first
dataToInt = foldr addByte 0x00
where addByte x y = y' * 0x100 + x'
where x' = fromIntegral x
y' = fromIntegral y
-fromUByte :: Integral a => [Word8] -> a -- one byte, unsigned
+fromUByte :: (Integral a) => [Word8] -> a -- one byte, unsigned
fromUByte ws@[_] = dataToInt ws
fromUByte _ = throw $ PatternMatchFail "fromUByte"
-fromUWord :: Integral a => [Word8] -> a -- two bytes, unsigned, least significant byte first
+fromUWord :: (Integral a) => [Word8] -> a -- two bytes, unsigned, least significant byte first
fromUWord ws@[_, _] = dataToInt ws
fromUWord _ = throw $ PatternMatchFail "fromUWord"
-fromULong :: Integral a => [Word8] -> a -- four bytes, unsigned, least significant byte first
+fromULong :: (Integral a) => [Word8] -> a -- four bytes, unsigned, least significant byte first
fromULong ws@[_, _, _, _] = dataToInt ws
fromULong _ = throw $ PatternMatchFail "fromULong"
-fromSByte :: Integral a => [Word8] -> a -- one byte, signed
+fromSByte :: (Integral a) => [Word8] -> a -- one byte, signed
fromSByte ws@[b] | b <= 0x7F = dataToInt ws
| otherwise = negate . (-) 0x100 . dataToInt $ ws
fromSByte _ = throw $ PatternMatchFail "fromSByte"
-fromSWord :: Integral a => [Word8] -> a -- two bytes, signed, least significant byte first
+fromSWord :: (Integral a) => [Word8] -> a -- two bytes, signed, least significant byte first
fromSWord ws@[_, b] | b <= 0x7F = dataToInt ws
| otherwise = negate . (-) 0x10000 . dataToInt $ ws
fromSWord _ = throw $ PatternMatchFail "fromSWord"
-fromSLong :: Integral a => [Word8] -> a -- four bytes, signed, least significant byte first
+fromSLong :: (Integral a) => [Word8] -> a -- four bytes, signed, least significant byte first
fromSLong ws@[_, _, _, b] | b <= 0x7F = dataToInt ws
| otherwise = negate . (-) 0x100000000 . dataToInt $ ws
fromSLong _ = throw $ PatternMatchFail "fromSLong"
@@ -49,36 +49,36 @@ dataToString = C.unpack . B.pack
-- Converts a numeric value to list of bytes
-- In a case of a negative number it produces an infinite list
-intToData :: Integral a => a -> [Word8] -- least significant byte first
+intToData :: (Integral a) => a -> [Word8] -- least significant byte first
intToData 0x00 = [0x00]
intToData x = unfoldr getByte x
where getByte 0x00 = Nothing
getByte y = Just (fromIntegral $ y `mod` 0x100, y `div` 0x100)
-toUByte :: Integral a => a -> [Word8] -- one byte, unsigned
+toUByte :: (Integral a) => a -> [Word8] -- one byte, unsigned
toUByte x | x >= 0x00 && x <= 0xFF = intToData x
| otherwise = throw . PatternMatchFail $ "toUByte: " ++ show x
-toUWord :: Integral a => a -> [Word8] -- two bytes, unsigned, least significant byte first
+toUWord :: (Integral a) => a -> [Word8] -- two bytes, unsigned, least significant byte first
toUWord x | x >= 0x00 && x <= 0xFFFF = take 2 . flip (++) (repeat 0x00) . intToData $ x
| otherwise = throw . PatternMatchFail $ "toUWord: " ++ show x
-toULong :: Integral a => a -> [Word8] -- four bytes, unsigned, least significant byte first
+toULong :: (Integral a) => a -> [Word8] -- four bytes, unsigned, least significant byte first
toULong x | x' >= 0x00 && x' <= 0xFFFFFFFF = take 4 . flip (++) (repeat 0x00) . intToData $ x'
| otherwise = throw . PatternMatchFail $ "toULong: " ++ show x
where x' = fromIntegral x :: Integer
-toSByte :: Integral a => a -> [Word8] -- one byte, signed
+toSByte :: (Integral a) => a -> [Word8] -- one byte, signed
toSByte x | x >= (-0x80) && x < 0x00 = intToData $ 0x100 + x
| x >= 0x00 && x <= 0x7F = intToData x
| otherwise = throw . PatternMatchFail $ "toSByte: " ++ show x
-toSWord :: Integral a => a -> [Word8] -- two bytes, signed, least significant byte first
+toSWord :: (Integral a) => a -> [Word8] -- two bytes, signed, least significant byte first
toSWord x | x >= (-0x8000) && x < 0x00 = take 2 . flip (++) (repeat 0x00) . intToData $ 0x10000 + x
| x >= 0x00 && x <= 0x7FFF = take 2 . flip (++) (repeat 0x00) . intToData $ x
| otherwise = throw . PatternMatchFail $ "toSWord: " ++ show x
-toSLong :: Integral a => a -> [Word8] -- four bytes, signed, least significant byte first
+toSLong :: (Integral a) => a -> [Word8] -- four bytes, signed, least significant byte first
toSLong x | x' >= (-0x80000000) && x' < 0x00 = take 4 . flip (++) (repeat 0x00) . intToData $ 0x100000000 + x'
| x' >= 0x00 && x' <= 0x7FFFFFFF = take 4 . flip (++) (repeat 0x00) . intToData $ x'
| otherwise = throw . PatternMatchFail $ "toSLong: " ++ show x
View
@@ -7,8 +7,6 @@ import Data.Time.Clock.POSIX
import Data.Word
import System.IO
-import NXT.CaptureTypes
-
-- Described in Lego Mindstorms NXT Bluetooth Developer Kit:
-- Appendix 1 - Communication protocol
-- Appendix 2 - Direct commands
@@ -17,7 +15,7 @@ import NXT.CaptureTypes
-- Appendix 7 - Ultrasonic sensor I2C communication protocol
type NXT = StateT NXTState IO -- NXT monad
-data NXTState = NXTState { nxthandle :: Handle, address :: (Maybe BTAddress), modules :: [(ModuleName, ModuleInfo)], sleeptime :: Duration, lastkeepalive :: POSIXTime, capture :: Maybe Capture } -- NXT monad has a handle of an opened serial port, some module infos, sleep time limit in seconds, last time keep alive has been sent and capture handles
+data NXTState = NXTState { nxthandle :: Handle, address :: (Maybe BTAddress), modules :: [(ModuleName, ModuleInfo)], sleeptime :: Duration, lastkeepalive :: POSIXTime } -- NXT monad has a handle of an opened serial port, some module infos, sleep time limit in seconds, last time keep alive has been sent
-- The format of version is major.minor: (printf "%d.%02d" major minor)
type Major = Int
View
@@ -4,7 +4,7 @@
#include <sys/ioctl.h>
#include <sys/socket.h>
-// TODO This works currently only on Linux
+// TODO: This works currently only on Linux
#ifdef LINUX
#include <bluetooth/bluetooth.h>

0 comments on commit cd5e298

Please sign in to comment.