module System.Hardware.Arduino.Comm where
import Control.Monad (when, forever)
import Control.Concurrent (MVar, ThreadId, newChan, newMVar, newEmptyMVar, putMVar, writeChan, readChan, forkIO, modifyMVar_, tryTakeMVar, killThread)
import Control.Exception (tryJust, AsyncException(UserInterrupt), handle, SomeException)
import Control.Monad.State (runStateT, gets, liftIO, modify)
import Data.Bits (testBit, (.&.))
import Data.List (intercalate, isInfixOf)
import Data.Maybe (listToMaybe)
import Data.Word (Word8)
import System.Timeout (timeout)
import System.IO (stderr, hPutStrLn)
import qualified Data.ByteString as B (unpack, length)
import qualified Data.Map as M (empty, mapWithKey, insert, assocs, lookup)
import qualified Data.Set as S (empty)
import qualified System.Hardware.Serialport as S (withSerial, defaultSerialSettings, CommSpeed(CS57600), commSpeed, recv, send)
import System.Hardware.Arduino.Data
import System.Hardware.Arduino.Utils
import System.Hardware.Arduino.Protocol
withArduino :: Bool
-> FilePath
-> Arduino ()
-> IO ()
withArduino verbose fp program =
do debugger <- mkDebugPrinter verbose
debugger $ "Accessing arduino located at: " ++ show fp
listenerTid <- newEmptyMVar
let Arduino controller = do initOK <- initialize listenerTid
if initOK
then program
else error "Communication time-out (5s) expired."
handle (\(e::SomeException) -> do cleanUp listenerTid
let selfErr = "*** hArduino" `isInfixOf` show e
hPutStrLn stderr $ if selfErr
then dropWhile (== '\n') (show e)
else "*** hArduino:ERROR: " ++ show e
++ concatMap ("\n*** " ++) [ "Make sure your Arduino is connected to " ++ fp
, "And StandardFirmata is running on it!"
]) $
S.withSerial fp S.defaultSerialSettings{S.commSpeed = S.CS57600} $ \port -> do
let initBoardState = BoardState {
boardCapabilities = BoardCapabilities M.empty
, analogReportingPins = S.empty
, digitalReportingPins = S.empty
, pinStates = M.empty
, digitalWakeUpQueue = []
, lcds = M.empty
}
bs <- newMVar initBoardState
dc <- newChan
let initState = ArduinoState {
message = debugger
, bailOut = bailOut listenerTid
, port = port
, firmataID = "Unknown"
, capabilities = BoardCapabilities M.empty
, boardState = bs
, deviceChannel = dc
, listenerTid = listenerTid
}
res <- tryJust catchCtrlC $ runStateT controller initState
case res of
Left () -> putStrLn "hArduino: Caught Ctrl-C, quitting.."
_ -> return ()
cleanUp listenerTid
where catchCtrlC UserInterrupt = Just ()
catchCtrlC _ = Nothing
cleanUp tid = do mbltid <- tryTakeMVar tid
case mbltid of
Just t -> killThread t
_ -> return ()
bailOut tid m ms = do cleanUp tid
error $ "\n*** hArduino:ERROR: " ++ intercalate "\n*** " (m:ms)
send :: Request -> Arduino ()
send req = do debug $ "Sending: " ++ show req ++ " <" ++ unwords (map showByte (B.unpack p)) ++ ">"
serial <- gets port
sent <- liftIO $ S.send serial p
when (sent /= lp)
(debug $ "Send failed. Tried: " ++ show lp ++ "bytes, reported: " ++ show sent)
where p = package req
lp = B.length p
recv :: Arduino Response
recv = do ch <- gets deviceChannel
liftIO $ readChan ch
recvTimeOut :: Int -> Arduino (Maybe Response)
recvTimeOut n = do ch <- gets deviceChannel
liftIO $ timeout n (readChan ch)
setupListener :: Arduino ThreadId
setupListener = do
serial <- gets port
dbg <- gets message
chan <- gets deviceChannel
let getBytes n = do let go need sofar
| need <= 0 = return $ reverse sofar
| True = do b <- S.recv serial need
case B.length b of
0 -> go need sofar
l -> go (need l) (b : sofar)
chunks <- go n []
return $ concatMap B.unpack chunks
collectSysEx sofar = do [b] <- getBytes 1
if b == firmataCmdVal END_SYSEX
then return $ reverse sofar
else collectSysEx (b : sofar)
listener bs = do
[cmd] <- getBytes 1
resp <- case getFirmataCmd cmd of
Left unknown -> return $ Unimplemented (Just (show unknown)) []
Right START_SYSEX -> unpackageSysEx `fmap` collectSysEx []
Right nonSysEx -> unpackageNonSysEx getBytes nonSysEx
case resp of
Unimplemented{} -> dbg $ "Ignoring the received response: " ++ show resp
AnalogMessage mp l h -> modifyMVar_ bs $ \bst ->
do let BoardCapabilities caps = boardCapabilities bst
mbP = listToMaybe [mappedPin | (mappedPin, PinCapabilities{analogPinNumber = Just mp'}) <- M.assocs caps, pinNo mp == mp']
case mbP of
Nothing -> return bst
Just p -> do
let v = (128 * fromIntegral (h .&. 0x07) + fromIntegral (l .&. 0x7f)) :: Int
case pinValue `fmap` (p `M.lookup` pinStates bst) of
Just (Just (Right v'))
| abs (v v') < 10 -> return ()
_ -> dbg $ "Updating analog pin " ++ show p ++ " values with " ++ showByteList [l,h] ++ " (" ++ show v ++ ")"
return bst{ pinStates = M.insert p PinData{pinMode = ANALOG, pinValue = Just (Right v)} (pinStates bst) }
DigitalMessage p l h -> do dbg $ "Updating digital port " ++ show p ++ " values with " ++ showByteList [l,h]
modifyMVar_ bs $ \bst -> do
let upd o od | p /= pinPort o = od
| pinMode od `notElem` [INPUT] = od
| True = od{pinValue = Just (Left newVal)}
where idx = pinPortIndex o
newVal | idx <= 6 = l `testBit` fromIntegral idx
| True = h `testBit` fromIntegral (idx 7)
let wakeUpQ = digitalWakeUpQueue bst
bst' = bst{ pinStates = M.mapWithKey upd (pinStates bst)
, digitalWakeUpQueue = []
}
mapM_ (`putMVar` ()) wakeUpQ
return bst'
_ -> do dbg $ "Received " ++ show resp
writeChan chan resp
bs <- gets boardState
tid <- liftIO $ forkIO $ forever (listener bs)
debug $ "Started listener thread: " ++ show tid
return tid
initialize :: MVar ThreadId -> Arduino Bool
initialize ltid = do
tid <- setupListener
liftIO $ putMVar ltid tid
send SystemReset
mbTo <- handshake QueryFirmware (Just (5000000 :: Int))
(\r -> case r of {Firmware{} -> True; _ -> False})
(\(Firmware v1 v2 m) -> modify (\s -> s{firmataID = "Firmware v" ++ show v1 ++ "." ++ show v2 ++ "(" ++ m ++ ")"}))
case mbTo of
Nothing -> return False
Just () -> do
_ <- handshake CapabilityQuery Nothing
(\r -> case r of {Capabilities{} -> True; _ -> False})
(\(Capabilities c) -> modify (\s -> s{capabilities = c}))
_ <- handshake AnalogMappingQuery Nothing
(\r -> case r of {AnalogMapping{} -> True; _ -> False})
(\(AnalogMapping as) -> do BoardCapabilities m <- gets capabilities
let caps = BoardCapabilities (M.mapWithKey (mapAnalog as) m)
modify (\s -> s{capabilities = caps})
bs <- gets boardState
liftIO $ modifyMVar_ bs $ \bst -> return bst{boardCapabilities = caps})
caps <- gets capabilities
dbg <- gets message
liftIO $ dbg $ "Handshake complete. Board capabilities:\n" ++ show caps
return True
where handshake msg mbTOut isOK process = do
dbg <- gets message
send msg
let wait = do mbResp <- case mbTOut of
Nothing -> Just `fmap` recv
Just n -> recvTimeOut n
case mbResp of
Nothing -> return Nothing
Just resp -> if isOK resp
then Just `fmap` process resp
else do liftIO $ dbg $ "Skipping unexpected response: " ++ show resp
wait
wait
mapAnalog :: [Word8] -> IPin -> PinCapabilities -> PinCapabilities
mapAnalog as p c
| i < rl && m /= 0x7f
= c{analogPinNumber = Just m}
| True
= c
where rl = length as
i = fromIntegral (pinNo p)
m = as !! i