module Robotics.NXT.Sensor.Compass (
csInit,
csGetMeasurement,
csSetMode,
Mode(..),
csGetVersion,
csGetVendorID,
csGetDeviceID
) where
import Control.Monad
import Robotics.NXT
import Robotics.NXT.Data
data Mode =
AutoTrigOn
| AutoTrigOff
| ResultByte
| ResultInteger
| Frequency50
| Frequency60
| BeginCalibration
| EndCalibration
deriving (Bounded, Enum, Eq, Ord, Read, Show)
deviceAddress :: DeviceAddress
deviceAddress = 0x02
csInit :: InputPort -> NXT ()
csInit input = do
setInputModeConfirm input Lowspeed9V RawMode
ready <- lowspeedGetStatus input
when (ready > 0) $ lowspeedRead input >> return ()
csSetMode input ResultInteger
csReadString :: InputPort -> Command -> RxDataLength -> NXT String
csReadString input command size = do
lowspeedWrite input size [deviceAddress, command]
s <- lowspeedRead input
return $ dataToString0 s
csGetVersion :: InputPort -> NXT String
csGetVersion input = csReadString input 0x00 8
csGetVendorID :: InputPort -> NXT String
csGetVendorID input = csReadString input 0x08 8
csGetDeviceID :: InputPort -> NXT String
csGetDeviceID input = csReadString input 0x10 8
csSetMode :: InputPort -> Mode -> NXT ()
csSetMode input mode =
lowspeedWriteConfirm input 0 $ [deviceAddress, 0x41] ++ command
where command = case mode of
AutoTrigOn -> [0x41, 0x02]
AutoTrigOff -> [0x53, 0x01]
ResultByte -> [0x42]
ResultInteger -> [0x49]
Frequency50 -> [0x45]
Frequency60 -> [0x55]
BeginCalibration -> [0x43]
EndCalibration -> [0x44]
csGetMeasurement :: InputPort -> NXT Measurement
csGetMeasurement input = do
lowspeedWrite input 2 [deviceAddress, 0x42]
angle <- lowspeedRead input
return $ fromUWord angle