module System.Hardware.Arduino.Parts.Servo(
Servo, attach
, setAngle
) where
import Control.Monad (when)
import Data.Bits (shiftR, (.&.))
import Data.Maybe (fromMaybe)
import System.Hardware.Arduino
import System.Hardware.Arduino.Comm
import System.Hardware.Arduino.Data
data Servo = Servo { servoPin :: IPin
, minPulse :: Int
, maxPulse :: Int
}
attach :: Pin
-> Maybe Int
-> Maybe Int
-> Arduino Servo
attach p mbMin mbMax
| Just m <- mbMin, m < 0
= die "Servo.attach: minimum pulse width must be positive" ["Received: " ++ show m]
| Just m <- mbMax, m < 0
= die "Servo.attach: maximum pulse width must be positive" ["Received: " ++ show m]
| True
= do let minPulse = fromMaybe 544 mbMin
maxPulse = fromMaybe 2400 mbMax
debug $ "Attaching servo on pin: " ++ show p ++ " with parameters: " ++ show (minPulse, maxPulse)
when (minPulse >= maxPulse) $ die "Servo.attach: min pulse duration must be less than max pulse duration"
[ "Received min-pulse: " ++ show minPulse
, "Received max-pulse: " ++ show maxPulse
]
setPinMode p SERVO
(ip, _) <- convertAndCheckPin "Servo.attach" p SERVO
return Servo { servoPin = ip
, minPulse = fromMaybe 544 mbMin
, maxPulse = fromMaybe 2400 mbMax
}
setAngle :: Servo -> Int -> Arduino ()
setAngle Servo{servoPin, minPulse, maxPulse} angle
| angle < 0 || angle > 180
= die "Servo.setAngle: angle must be between 0 and 180." ["Received: " ++ show angle]
| True
= do let duration = minPulse + ((maxPulse minPulse) * angle) `div` 180
debug $ "Setting servo on pin: " ++ show servoPin ++ " " ++ show angle ++ " degrees, via a pulse of " ++ show duration ++ " microseconds."
when (duration >= 16383) $ die "Servo.setAngle angle setting: out-of-range."
[ "Servo pin : " ++ show servoPin
, "Angle required : " ++ show angle
, "Min pulse duration: " ++ show minPulse
, "Max pulse duration: " ++ show maxPulse
, "Duration needed : " ++ show duration
, "Exceeds max value : 16383"
]
let msb = fromIntegral $ (duration `shiftR` 7) .&. 0x7f
lsb = fromIntegral $ duration .&. 0x7f
send $ AnalogPinWrite servoPin lsb msb