module Robotics.NXT.Sensor.Ultrasonic (
usInit,
usGetMeasurement,
usGetAllMeasurements,
usSetMode,
usGetMode,
Mode(..),
usSetContinuousInterval,
usGetContinuousInterval,
usSetActualZero,
usGetActualZero,
usSetActualScaleFactor,
usGetActualScaleFactor,
usSetActualScaleDivisor,
usGetActualScaleDivisor,
usGetVersion,
usGetVendorID,
usGetDeviceID,
usGetFactoryZero,
usGetFactoryScaleFactor,
usGetFactoryScaleDivisor,
usGetMeasurementUnits,
Zero,
ScaleFactor,
ScaleDivisor,
ContinuousInterval,
MeasurementNumber
) where
import Control.Applicative
import Control.Exception
import Control.Monad
import Control.Monad.Trans
import Data.Maybe
import Robotics.NXT
import Robotics.NXT.Data
type Zero = Int
type ScaleFactor = Int
type ScaleDivisor = Int
type ContinuousInterval = Int
type MeasurementNumber = Int
data Mode =
Off
| SingleShot
| ContinuousMeasurement
| EventCapture
| WarmReset
deriving (Bounded, Enum, Eq, Ord, Read, Show)
deviceAddress :: DeviceAddress
deviceAddress = 0x02
usInit :: InputPort -> NXT ()
usInit input = do
setInputModeConfirm input Lowspeed9V RawMode
ready <- lowspeedGetStatus input
when (ready > 0) $ lowspeedRead input >> return ()
usSetMode input WarmReset
usReadByte :: Integral a => InputPort -> Command -> NXT a
usReadByte input command = do
lowspeedWrite input 1 [deviceAddress, command]
b <- lowspeedRead input
return $ fromUByte . take 1 $ b
usReadString :: InputPort -> Command -> RxDataLength -> NXT String
usReadString input command size = do
lowspeedWrite input size [deviceAddress, command]
s <- lowspeedRead input
return $ dataToString0 s
usGetVersion :: InputPort -> NXT String
usGetVersion input = usReadString input 0x00 8
usGetVendorID :: InputPort -> NXT String
usGetVendorID input = usReadString input 0x08 8
usGetDeviceID :: InputPort -> NXT String
usGetDeviceID input = usReadString input 0x10 8
usGetFactoryZero :: InputPort -> NXT Zero
usGetFactoryZero input = usReadByte input 0x11
usGetFactoryScaleFactor :: InputPort -> NXT ScaleFactor
usGetFactoryScaleFactor input = usReadByte input 0x12
usGetFactoryScaleDivisor :: InputPort -> NXT ScaleDivisor
usGetFactoryScaleDivisor input = usReadByte input 0x13
usGetMeasurementUnits :: InputPort -> NXT String
usGetMeasurementUnits input = usReadString input 0x14 7
usGetMode :: InputPort -> NXT Mode
usGetMode input = do
mode <- usReadByte input 0x41 :: NXT Int
case mode of
0x00 -> return Off
0x01 -> return SingleShot
0x02 -> return ContinuousMeasurement
0x03 -> return EventCapture
0x04 -> return WarmReset
_ -> liftIO . throwIO $ PatternMatchFail "usGetMode"
usGetContinuousInterval :: InputPort -> NXT ContinuousInterval
usGetContinuousInterval input = usReadByte input 0x40
usGetActualZero :: InputPort -> NXT Zero
usGetActualZero input = usReadByte input 0x50
usGetActualScaleFactor :: InputPort -> NXT ScaleFactor
usGetActualScaleFactor input = usReadByte input 0x51
usGetActualScaleDivisor :: InputPort -> NXT ScaleDivisor
usGetActualScaleDivisor input = usReadByte input 0x52
usSetMode :: InputPort -> Mode -> NXT ()
usSetMode input mode = lowspeedWriteConfirm input 0 [deviceAddress, 0x41, fromIntegral . fromEnum $ mode]
usSetContinuousInterval :: InputPort -> ContinuousInterval -> NXT ()
usSetContinuousInterval input interval = lowspeedWrite input 0 $ [deviceAddress, 0x40] ++ toUByte interval
usSetActualZero :: InputPort -> Zero -> NXT ()
usSetActualZero input zero = lowspeedWrite input 0 $ [deviceAddress, 0x50] ++ toUByte zero
usSetActualScaleFactor :: InputPort -> ScaleFactor -> NXT ()
usSetActualScaleFactor input factor = lowspeedWrite input 0 $ [deviceAddress, 0x51] ++ toUByte factor
usSetActualScaleDivisor :: InputPort -> ScaleDivisor -> NXT ()
usSetActualScaleDivisor input divisor = lowspeedWrite input 0 $ [deviceAddress, 0x52] ++ toUByte divisor
usGetMeasurement :: InputPort -> MeasurementNumber -> NXT (Maybe Measurement)
usGetMeasurement input number | number >= 0 && number < 8 = do measurement <- usReadByte input $ 0x42 + fromIntegral number
if measurement == 0xFF
then return Nothing
else return $ Just measurement
| otherwise = liftIO . throwIO $ PatternMatchFail "usGetMeasurement"
usGetAllMeasurements :: InputPort -> NXT [Measurement]
usGetAllMeasurements input = mapMaybeM (usGetMeasurement input) [0..7]
where mapMaybeM f as = catMaybes <$> mapM f as