sphero (empty) → 0.1.0.0
raw patch · 7 files changed
+439/−0 lines, 7 filesdep +basedep +bytestringdep +cerealsetup-changed
Dependencies added: base, bytestring, cereal, containers, mtl, simple-bluetooth
Files
- LICENSE +30/−0
- Network/Protocol/Orbotix/Sphero.hs +287/−0
- Setup.hs +2/−0
- examples/FindSphero.hs +18/−0
- examples/coloured-roll.hs +53/−0
- examples/versioning.hs +13/−0
- sphero.cabal +36/−0
+ LICENSE view
@@ -0,0 +1,30 @@+Copyright (c) 2013, Stephen Blackheath + +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above + copyright notice, this list of conditions and the following + disclaimer in the documentation and/or other materials provided + with the distribution. + + * Neither the name of Stephen Blackheath nor the names of other + contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ Network/Protocol/Orbotix/Sphero.hs view
@@ -0,0 +1,287 @@+{-# LANGUAGE FlexibleInstances, DeriveGeneric, DeriveFunctor #-} +-- | Client library for Orbotix Sphero. +-- +-- See examples directory for an example. +module Network.Protocol.Orbotix.Sphero ( + Sphero, + runSphero, + forkSphero, + ping, + getVersioning, + Versioning(..), + color, + roll + ) where + +import Control.Applicative +import Control.Concurrent +import Control.Exception +import Control.Monad +import Control.Monad.Reader +import Data.Bits +import Data.ByteString (ByteString) +import qualified Data.ByteString as B +import qualified Data.ByteString.Char8 as C +import Data.IORef +import Data.Map (Map) +import qualified Data.Map as M +import Data.Maybe +import Data.Serialize +import Data.Typeable +import Data.Word +import GHC.Generics +import Network.Bluetooth +import Numeric + + +data Command = Ping + | GetVersioning + | Roll { roSpeed :: Word8, roHeading :: Int } + | Color { coRed :: Word8, coGreen :: Word8, coBlue :: Word8, coSave :: Bool } + deriving (Generic, Show) + +instance Serialize Command where + +data Reply = SimpleResponse Word8 ByteString + | PowerNotification ByteString + | Level1Diag ByteString + | SensorData ByteString + | ConfigBlock ByteString + | PreSleepWarning ByteString + | MacroMarkers ByteString + | CollisionDetection ByteString + | OrbPrint ByteString + | OrbErrorASCII ByteString + | OrbErrorBinary ByteString + deriving (Generic, Show) + +data Packet a = Packet Word8 a + deriving (Generic, Show) + +instance Serialize (Packet Command) where + put (Packet seq Ping) = addCK $ do + putWord8 0 + putWord8 1 + putWord8 seq + putWord8 1 + + put (Packet seq GetVersioning) = addCK $ do + putWord8 0 + putWord8 2 + putWord8 seq + putWord8 1 + + put (Packet seq (Roll sp hding)) = addCK $ do + let hding' = fromIntegral (hding `mod` 360) + putWord8 2 + putWord8 0x30 + putWord8 seq + putWord8 5 + putWord8 sp + putWord16be hding' + putWord8 0xff + + put (Packet seq (Color r g b s)) = addCK $ do + putWord8 2 + putWord8 0x20 + putWord8 seq + putWord8 5 + putWord8 r + putWord8 g + putWord8 b + putWord8 $ if s then 0xff else 0 + +instance Serialize Reply where + get = do + sop1 <- getWord8 + unless (sop1 == 0xff) $ fail $ "unexpected SOP1 byte 0x"++showHex sop1 "" + sop2 <- getWord8 + case sop2 of + 0xff -> do + mrsp <- getWord8 + seq <- getWord8 + len <- getWord8 + dta <- getByteString (fromIntegral (len - 1)) + ck <- getWord8 + -- to do: check ck + case mrsp of + 0x00 -> pure $ SimpleResponse seq dta + _ -> fail $ "Unknown packet received 0x"++showHex mrsp "" + 0xfe -> do + idCode <- getWord8 + dlen <- getWord16be + dta <- getByteString (fromIntegral (dlen - 1)) + ck <- getWord8 + -- to do: check ck + case idCode of + 0x01 -> pure $ PowerNotification dta + 0x02 -> pure $ Level1Diag dta + 0x03 -> pure $ SensorData dta + 0x04 -> pure $ ConfigBlock dta + 0x05 -> pure $ PreSleepWarning dta + 0x06 -> pure $ MacroMarkers dta + 0x07 -> pure $ CollisionDetection dta + 0x08 -> pure $ OrbPrint dta + 0x09 -> pure $ OrbErrorASCII dta + 0x0a -> pure $ OrbErrorBinary dta + _ -> fail $ "Unknown async id code 0x"++showHex idCode "" + where + getFirst = do + f <- getWord8 + if f == 0xff + then getFirst + else return f + +addCK :: Put -> Put +addCK p = do + let payload = runPut p + putWord8 0xff + putWord8 0xff + putByteString payload + let ck = sum (B.unpack payload) `xor` 0xff + putWord8 ck + +data SState = SState { + ssSocket :: RFCOMMSocket, + ssNextSeq :: MVar Word8, + ssNextThread :: MVar Int, + ssListeners :: MVar (Map Int (Chan Reply)), + ssThread :: Int + } + +sendCommand :: SState -> Command -> IO Word8 +sendCommand ss cmd = do + seq <- modifyMVar (ssNextSeq ss) $ \seq -> do + let bs = encode $ Packet seq cmd + sendAllRFCOMM (ssSocket ss) bs + return (seq+1, seq) + return seq + +cmd :: Command -> Sphero ByteString +cmd c = Sphero $ do + ss <- ask + liftIO $ do + ch <- newChan + modifyMVar (ssListeners ss) $ \lnrs -> do + return (M.insert (ssThread ss) ch lnrs, ()) + do + seq <- sendCommand ss c + let await = do + r <- readChan ch + case r of + SimpleResponse seq' body | seq == seq' -> pure body + _ -> await + await + `finally` do + modifyMVar (ssListeners ss) $ \lnrs -> do + return (M.delete (ssThread ss) lnrs, ()) + +data Versioning = Versioning { + veRECV :: Word8, + veMDL :: Word8, + veHW :: Word8, + veMSA :: (Word8, Word8), + veBL :: Word8, + veBAS :: Word8, + veMACRO :: Word8, + veAPI :: Maybe (Word8, Word8) + } + deriving Show + +getVersioning :: Sphero Versioning +getVersioning = do + bs <- cmd GetVersioning + case runGet go bs of + Left err -> fail $ "getVersioning failed: "++err + Right v -> return v + where + go = Versioning + <$> getWord8 + <*> getWord8 + <*> getWord8 + <*> ((,) <$> getWord8 <*> getWord8) + <*> getWord8 + <*> getWord8 + <*> getWord8 + <*> (do + r <- remaining + if r >= 2 + then Just <$> ((,) <$> getWord8 <*> getWord8) + else pure Nothing + ) + +newtype Sphero a = Sphero (ReaderT SState IO a) + deriving Functor + +instance Applicative Sphero where + pure a = Sphero (pure a) + Sphero f <*> Sphero a = Sphero (f <*> a) + +instance Monad Sphero where + return a = Sphero (return a) + Sphero ma >>= kmb = Sphero $ do + a <- ma + let Sphero mb = kmb a + mb + +instance MonadIO Sphero where + liftIO ma = Sphero $ liftIO ma + +runSphero :: RFCOMMSocket -> Sphero a -> IO a +runSphero s (Sphero m) = do + ss <- SState s <$> newMVar 0 + <*> newMVar 1 + <*> newMVar M.empty + <*> return 0 + t <- forkIO $ do + let go cont bs0 = do + bs <- if B.null bs0 + then recvRFCOMM s 1024 + else return bs0 + case cont bs of + Fail err _ -> fail $ "parse failed: "++err + Partial cont -> go cont B.empty + Done r bs -> do + chans <- M.elems <$> readMVar (ssListeners ss) + forM_ chans $ \ch -> writeChan ch r + go (runGetPartial get) bs + go (runGetPartial get) B.empty + ret <- runReaderT m ss + killThread t + return ret + +forkSphero :: Sphero () -> Sphero ThreadId +forkSphero (Sphero m) = Sphero $ do + ss <- ask + liftIO $ do + thr <- modifyMVar (ssNextThread ss) $ \thr -> return (thr+1, thr) + forkIO $ runReaderT m $ ss { ssThread = thr } + +ping :: Sphero () +ping = do + cmd Ping + return () + +color :: Word8 -- ^ Red + -> Word8 -- ^ Green + -> Word8 -- ^ Blue + -> Sphero () +color r g b = do + cmd $ Color r g b False + return () + +-- | Set the Sphero's colour and save it as the default. +colorSave :: Word8 -- ^ Red + -> Word8 -- ^ Green + -> Word8 -- ^ Blue + -> Sphero () +colorSave r g b = do + cmd $ Color r g b True + return () + +roll :: Word8 -- ^ Speed + -> Int -- ^ Heading + -> Sphero () +roll sp hding = do + cmd $ Roll sp hding + return ()
+ Setup.hs view
@@ -0,0 +1,2 @@+import Distribution.Simple +main = defaultMain
+ examples/FindSphero.hs view
@@ -0,0 +1,18 @@+module FindSphero where + +import qualified Data.ByteString as B +import Network.Bluetooth + +findSphero :: IO RFCOMMSocket +findSphero = do + mAdapter <- defaultAdapter + adapter <- case mAdapter of + Just adapter -> return adapter + Nothing -> fail $ "Can't find a local bluetooth adapter" + devs <- discover adapter + case filter isSphero devs of + (dev:_) -> openRFCOMM dev 1 + [] -> fail $ "Can't find a sphero device" + +isSphero :: Device -> Bool +isSphero (Device _ (BluetoothAddr addr)) = B.pack [0x66, 0x06, 0x00] `B.isSuffixOf` addr
+ examples/coloured-roll.hs view
@@ -0,0 +1,53 @@+{-# LANGUAGE FlexibleInstances, DeriveGeneric, DeriveFunctor #-} +import Network.Protocol.Orbotix.Sphero +import Network (withSocketsDo) +import Control.Monad +import Control.Monad.Trans +import Control.Concurrent (threadDelay) +import FindSphero + + +main :: IO () +main = withSocketsDo $ do + s <- findSphero + runSphero s $ do + forkSphero $ forever $ do + forever $ do + forM_ [0,5..359] $ \h -> do + let (r, g, b) = hsv_to_rgb (h :: Double) 1 1 + color (toW r) (toW g) (toW b) + liftIO $ threadDelay 20000 + let sp = 40 + forever $ do + roll sp 0 + liftIO $ threadDelay 2000000 + roll sp 90 + liftIO $ threadDelay 2000000 + roll sp 180 + liftIO $ threadDelay 2000000 + roll sp 270 + liftIO $ threadDelay 2000000 + where + toW a = floor $ a * 255 + +-- | hue is in degrees +hsv_to_rgb :: (Num a, Fractional a, RealFloat a) => a -> a -> a -> (a, a, a) +hsv_to_rgb h s v = case hi of + 0 -> (v,t,p) + 1 -> (q,v,p) + 2 -> (p,v,t) + 3 -> (p,q,v) + 4 -> (t,p,v) + _ -> (v,p,q) + where + hi = floor (h/60) `mod` 6 + f = mod1 (h/60) + p = v*(1-s) + q = v*(1-f*s) + t = v*(1-(1-f)*s) + + mod1 x | pf < 0 = pf+1 + | otherwise = pf + where + (_,pf) = properFraction x +
+ examples/versioning.hs view
@@ -0,0 +1,13 @@+{-# LANGUAGE FlexibleInstances, DeriveGeneric, DeriveFunctor #-} +import Network.Protocol.Orbotix.Sphero +import Network (withSocketsDo) +import Control.Monad.Trans (liftIO) +import FindSphero + + +main :: IO () +main = withSocketsDo $ do + s <- findSphero + runSphero s $ do + v <- getVersioning + liftIO $ print v
+ sphero.cabal view
@@ -0,0 +1,36 @@+-- Initial sphero.cabal generated by cabal init. For further +-- documentation, see http://haskell.org/cabal/users-guide/ + +name: sphero +version: 0.1.0.0 +synopsis: Orbotix Sphero client library +description: + My nephew visited me at Christmas time. + Guess what I wasted my time doing. :) + . + See examples directory for examples. +license: BSD3 +license-file: LICENSE +author: Stephen Blackheath +maintainer: docks.cattlemen.stephen@blacksapphire.com +-- copyright: +category: Network +build-type: Simple +extra-source-files: + examples/FindSphero.hs + examples/versioning.hs + examples/coloured-roll.hs +cabal-version: >=1.10 + +library + exposed-modules: Network.Protocol.Orbotix.Sphero + -- other-modules: + other-extensions: FlexibleInstances, DeriveGeneric, DeriveFunctor + build-depends: base >=4.5 && <4.7, + mtl >=2.1 && <2.2, + bytestring >=0.10 && <0.11, + containers >=0.5 && <0.6, + cereal >=0.4 && <0.5, + simple-bluetooth >=0.1 && <0.2 + -- hs-source-dirs: + default-language: Haskell2010