Permalink
Browse files

Initial code.

  • Loading branch information...
mitar committed Aug 27, 2010
0 parents commit 006e0cb3526ba9d21f9f1032a7e8b7cb6f296a43
Showing with 1,607 additions and 0 deletions.
  1. +65 −0 lib/BluetoothUtils.hs
  2. +65 −0 lib/Capture.hs
  3. +30 −0 lib/CaptureTypes.hs
  4. +34 −0 lib/CaptureUtils.hs
  5. +734 −0 lib/NXT.hs
  6. +86 −0 lib/NXTCompass.hs
  7. +96 −0 lib/NXTData.hs
  8. +48 −0 lib/NXTErrors.hs
  9. +111 −0 lib/NXTTypes.hs
  10. +127 −0 lib/NXTUltrasonicSensor.hs
  11. +165 −0 lib/blue.c
  12. +11 −0 lib/blue.h
  13. +29 −0 lib/initserial.c
  14. +6 −0 lib/initserial.h
@@ -0,0 +1,65 @@
+{-# LANGUAGE ForeignFunctionInterface #-}
+
+module BluetoothUtils (bluetoothRSSI, bluetoothLinkQuality, bluetoothAddress) where
+
+import Control.Monad.State
+import Foreign
+import Foreign.C.String
+import Foreign.C.Types
+
+import NXT
+import NXTTypes
+
+-- Foreign function call for C function which returns RSSI Bluetooth value of a connection to a given Bluetooth address
+foreign import ccall unsafe "blue.h" rssi :: CString -> IO CInt
+
+-- Foreign function call for C function which returns link quality Bluetooth value of a connection to a given Bluetooth address
+foreign import ccall unsafe "blue.h" lq :: CString -> IO CInt
+
+-- As defined in blue.h
+blueError :: Int
+blueError = 1000
+blueNotConnected :: Int
+blueNotConnected = 1001
+blueNotSupported :: Int
+blueNotSupported = 1002
+
+bluetoothRSSI :: NXT Int
+bluetoothRSSI = do
+ addr <- bluetoothAddress
+ bluetoothRSSIAddr addr
+
+bluetoothRSSIAddr :: BTAddress -> NXT Int
+bluetoothRSSIAddr addr = do
+ ret <- io $ withCString addr rssi
+ let ret' = fromIntegral ret
+ case ret' of
+ _ | ret' == blueError -> error "Could not get connection's RSSI"
+ _ | ret' == blueNotConnected -> error "Connection not established"
+ _ | ret' == blueNotSupported -> error "Not supported on this system"
+ _ | otherwise -> return ret'
+
+bluetoothLinkQuality :: NXT Int
+bluetoothLinkQuality = do
+ addr <- bluetoothAddress
+ bluetoothLinkQualityAddr addr
+
+bluetoothLinkQualityAddr :: BTAddress -> NXT Int
+bluetoothLinkQualityAddr addr = do
+ ret <- io $ withCString addr lq
+ let ret' = fromIntegral ret
+ case ret' of
+ _ | ret' == blueError -> error "Could not get connection's link quality"
+ _ | ret' == blueNotConnected -> error "Connection not established"
+ _ | ret' == blueNotSupported -> error "Not supported on this system"
+ _ | otherwise -> return ret'
+
+bluetoothAddress :: NXT BTAddress
+bluetoothAddress = do
+ addr <- gets address
+ case addr of
+ Just a -> return a
+ Nothing -> do
+ getDeviceInfo
+ (Just a) <- gets address
+ return a
@@ -0,0 +1,65 @@
+module NXT.Capture (initCapture, terminateCapture, getLastRobotPosition) where
+
+import Control.Exception
+import Control.Monad.State
+import qualified Data.ByteString.Lazy.Char8 as C
+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 = False
+
+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 return $ Just $ head . head $ positions -- positions are stored as lists in the reverse order so (head . head) is the last position from capture process
+
+captureProcessExited :: NXT a
+captureProcessExited = do
+ terminateCapture
+ fail "Capture process exited"
@@ -0,0 +1,30 @@
+{-# 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
+
+data Robot = Robot Double Double Double POSIXTime POSIXTime deriving (Eq, Read, Show) -- X and Y coordinates of a robot, direction of a robot in radians from positive X axis, capture and processed timestamps in microseconds
+
+-- 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)) + 2 * 3 * (length . show $ resolution (undefined :: E12)) + 42
+#else
+maxRobotDescLength = length "Robot" + 3 * (floatDigits (undefined :: Double)) + 2 * 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
@@ -0,0 +1,34 @@
+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"
Oops, something went wrong.

0 comments on commit 006e0cb

Please sign in to comment.