{-# LANGUAGE NamedFieldPuns #-}
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 { Servo -> IPin
servoPin :: IPin
, Servo -> Int
minPulse :: Int
, Servo -> Int
maxPulse :: Int
}
attach :: Pin
-> Maybe Int
-> Maybe Int
-> Arduino Servo
attach :: Pin -> Maybe Int -> Maybe Int -> Arduino Servo
attach Pin
p Maybe Int
mbMin Maybe Int
mbMax
| Just Int
m <- Maybe Int
mbMin, Int
m forall a. Ord a => a -> a -> Bool
< Int
0
= forall a. String -> [String] -> Arduino a
die String
"Servo.attach: minimum pulse width must be positive" [String
"Received: " forall a. [a] -> [a] -> [a]
++ forall a. Show a => a -> String
show Int
m]
| Just Int
m <- Maybe Int
mbMax, Int
m forall a. Ord a => a -> a -> Bool
< Int
0
= forall a. String -> [String] -> Arduino a
die String
"Servo.attach: maximum pulse width must be positive" [String
"Received: " forall a. [a] -> [a] -> [a]
++ forall a. Show a => a -> String
show Int
m]
| Bool
True
= do let minPulse :: Int
minPulse = forall a. a -> Maybe a -> a
fromMaybe Int
544 Maybe Int
mbMin
maxPulse :: Int
maxPulse = forall a. a -> Maybe a -> a
fromMaybe Int
2400 Maybe Int
mbMax
String -> Arduino ()
debug forall a b. (a -> b) -> a -> b
$ String
"Attaching servo on pin: " forall a. [a] -> [a] -> [a]
++ forall a. Show a => a -> String
show Pin
p forall a. [a] -> [a] -> [a]
++ String
" with parameters: " forall a. [a] -> [a] -> [a]
++ forall a. Show a => a -> String
show (Int
minPulse, Int
maxPulse)
forall (f :: * -> *). Applicative f => Bool -> f () -> f ()
when (Int
minPulse forall a. Ord a => a -> a -> Bool
>= Int
maxPulse) forall a b. (a -> b) -> a -> b
$ forall a. String -> [String] -> Arduino a
die String
"Servo.attach: min pulse duration must be less than max pulse duration"
[ String
"Received min-pulse: " forall a. [a] -> [a] -> [a]
++ forall a. Show a => a -> String
show Int
minPulse
, String
"Received max-pulse: " forall a. [a] -> [a] -> [a]
++ forall a. Show a => a -> String
show Int
maxPulse
]
Pin -> PinMode -> Arduino ()
setPinMode Pin
p PinMode
SERVO
(IPin
ip, PinData
_) <- String -> Pin -> PinMode -> Arduino (IPin, PinData)
convertAndCheckPin String
"Servo.attach" Pin
p PinMode
SERVO
forall (m :: * -> *) a. Monad m => a -> m a
return Servo { servoPin :: IPin
servoPin = IPin
ip
, minPulse :: Int
minPulse = forall a. a -> Maybe a -> a
fromMaybe Int
544 Maybe Int
mbMin
, maxPulse :: Int
maxPulse = forall a. a -> Maybe a -> a
fromMaybe Int
2400 Maybe Int
mbMax
}
setAngle :: Servo -> Int -> Arduino ()
setAngle :: Servo -> Int -> Arduino ()
setAngle Servo{IPin
servoPin :: IPin
servoPin :: Servo -> IPin
servoPin, Int
minPulse :: Int
minPulse :: Servo -> Int
minPulse, Int
maxPulse :: Int
maxPulse :: Servo -> Int
maxPulse} Int
angle
| Int
angle forall a. Ord a => a -> a -> Bool
< Int
0 Bool -> Bool -> Bool
|| Int
angle forall a. Ord a => a -> a -> Bool
> Int
180
= forall a. String -> [String] -> Arduino a
die String
"Servo.setAngle: angle must be between 0 and 180." [String
"Received: " forall a. [a] -> [a] -> [a]
++ forall a. Show a => a -> String
show Int
angle]
| Bool
True
= do let duration :: Int
duration = Int
minPulse forall a. Num a => a -> a -> a
+ ((Int
maxPulse forall a. Num a => a -> a -> a
- Int
minPulse) forall a. Num a => a -> a -> a
* Int
angle) forall a. Integral a => a -> a -> a
`div` Int
180
String -> Arduino ()
debug forall a b. (a -> b) -> a -> b
$ String
"Setting servo on pin: " forall a. [a] -> [a] -> [a]
++ forall a. Show a => a -> String
show IPin
servoPin forall a. [a] -> [a] -> [a]
++ String
" " forall a. [a] -> [a] -> [a]
++ forall a. Show a => a -> String
show Int
angle forall a. [a] -> [a] -> [a]
++ String
" degrees, via a pulse of " forall a. [a] -> [a] -> [a]
++ forall a. Show a => a -> String
show Int
duration forall a. [a] -> [a] -> [a]
++ String
" microseconds."
forall (f :: * -> *). Applicative f => Bool -> f () -> f ()
when (Int
duration forall a. Ord a => a -> a -> Bool
>= Int
16383) forall a b. (a -> b) -> a -> b
$ forall a. String -> [String] -> Arduino a
die String
"Servo.setAngle angle setting: out-of-range."
[ String
"Servo pin : " forall a. [a] -> [a] -> [a]
++ forall a. Show a => a -> String
show IPin
servoPin
, String
"Angle required : " forall a. [a] -> [a] -> [a]
++ forall a. Show a => a -> String
show Int
angle
, String
"Min pulse duration: " forall a. [a] -> [a] -> [a]
++ forall a. Show a => a -> String
show Int
minPulse
, String
"Max pulse duration: " forall a. [a] -> [a] -> [a]
++ forall a. Show a => a -> String
show Int
maxPulse
, String
"Duration needed : " forall a. [a] -> [a] -> [a]
++ forall a. Show a => a -> String
show Int
duration
, String
"Exceeds max value : 16383"
]
let msb :: Word8
msb = forall a b. (Integral a, Num b) => a -> b
fromIntegral forall a b. (a -> b) -> a -> b
$ (Int
duration forall a. Bits a => a -> Int -> a
`shiftR` Int
7) forall a. Bits a => a -> a -> a
.&. Int
0x7f
lsb :: Word8
lsb = forall a b. (Integral a, Num b) => a -> b
fromIntegral forall a b. (a -> b) -> a -> b
$ Int
duration forall a. Bits a => a -> a -> a
.&. Int
0x7f
Request -> Arduino ()
send forall a b. (a -> b) -> a -> b
$ IPin -> Word8 -> Word8 -> Request
AnalogPinWrite IPin
servoPin Word8
lsb Word8
msb