-------------------------------------------------------------------------------------------------
-- |
-- Module      :  System.Hardware.Arduino.Parts.Servo
-- Copyright   :  (c) Levent Erkok
-- License     :  BSD3
-- Maintainer  :  erkokl@gmail.com
-- Stability   :  experimental
--
-- Abstractions for servo motors. See "System.Hardware.Arduino.SamplePrograms.Servo" for
-- example uses.
-------------------------------------------------------------------------------------------------

{-# LANGUAGE NamedFieldPuns #-}
module System.Hardware.Arduino.Parts.Servo(
   -- * Attaching a servo motor on a pin
     Servo, attach
   -- * Setting servo position
   , 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

-- | A servo motor. Note that this type is abstract, use 'attach' to
-- create an instance.
data Servo = Servo { Servo -> IPin
servoPin :: IPin  -- ^ The internal-pin that controls the servo
                   , Servo -> Int
minPulse :: Int   -- ^ Pulse-width (microseconds) for the minumum (0-degree) angle.
                   , Servo -> Int
maxPulse :: Int   -- ^ Pulse-width (microseconds) for the maximum (typically 180-degree) angle.
                   }

-- | Create a servo motor instance. The default values for the min/max angle pulse-widths, while typical,
-- may need to be adjusted based on the specs of the actual servo motor. Check the data-sheet for your
-- servo to find the proper values. The default values of @544@ and @2400@ microseconds are typical, so you might
-- want to start by passing 'Nothing' for both parameters and adjusting as necessary.
attach :: Pin            -- ^ Pin controlling the servo. Should be a pin that supports SERVO mode.
       -> Maybe Int      -- ^ Pulse-width (in microseconds) for the minumum 0-degree angle. Default: @544@.
       -> Maybe Int      -- ^ Pulse-width (in microseconds) for the maximum, typically 180-degree, angle. Default: @2400@.
       -> 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 Int -> Int -> Bool
forall a. Ord a => a -> a -> Bool
< Int
0
  = String -> [String] -> Arduino Servo
forall a. String -> [String] -> Arduino a
die String
"Servo.attach: minimum pulse width must be positive" [String
"Received: " String -> String -> String
forall a. [a] -> [a] -> [a]
++ Int -> String
forall a. Show a => a -> String
show Int
m]
  | Just Int
m <- Maybe Int
mbMax, Int
m Int -> Int -> Bool
forall a. Ord a => a -> a -> Bool
< Int
0
  = String -> [String] -> Arduino Servo
forall a. String -> [String] -> Arduino a
die String
"Servo.attach: maximum pulse width must be positive" [String
"Received: " String -> String -> String
forall a. [a] -> [a] -> [a]
++ Int -> String
forall a. Show a => a -> String
show Int
m]
  | Bool
True
  = do let minPulse :: Int
minPulse = Int -> Maybe Int -> Int
forall a. a -> Maybe a -> a
fromMaybe Int
544  Maybe Int
mbMin
           maxPulse :: Int
maxPulse = Int -> Maybe Int -> Int
forall a. a -> Maybe a -> a
fromMaybe Int
2400 Maybe Int
mbMax
       String -> Arduino ()
debug (String -> Arduino ()) -> String -> Arduino ()
forall a b. (a -> b) -> a -> b
$ String
"Attaching servo on pin: " String -> String -> String
forall a. [a] -> [a] -> [a]
++ Pin -> String
forall a. Show a => a -> String
show Pin
p String -> String -> String
forall a. [a] -> [a] -> [a]
++ String
" with parameters: " String -> String -> String
forall a. [a] -> [a] -> [a]
++ (Int, Int) -> String
forall a. Show a => a -> String
show (Int
minPulse, Int
maxPulse)
       Bool -> Arduino () -> Arduino ()
forall (f :: * -> *). Applicative f => Bool -> f () -> f ()
when (Int
minPulse Int -> Int -> Bool
forall a. Ord a => a -> a -> Bool
>= Int
maxPulse) (Arduino () -> Arduino ()) -> Arduino () -> Arduino ()
forall a b. (a -> b) -> a -> b
$ String -> [String] -> Arduino ()
forall a. String -> [String] -> Arduino a
die String
"Servo.attach: min pulse duration must be less than max pulse duration"
                                         [ String
"Received min-pulse: " String -> String -> String
forall a. [a] -> [a] -> [a]
++ Int -> String
forall a. Show a => a -> String
show Int
minPulse
                                         , String
"Received max-pulse: " String -> String -> String
forall a. [a] -> [a] -> [a]
++ Int -> String
forall a. Show a => a -> String
show Int
maxPulse
                                         ]
       Pin -> PinMode -> Arduino ()
setPinMode Pin
p PinMode
SERVO
       (ip, _) <- String -> Pin -> PinMode -> Arduino (IPin, PinData)
convertAndCheckPin String
"Servo.attach" Pin
p PinMode
SERVO
       return Servo { servoPin = ip
                    , minPulse = fromMaybe 544  mbMin
                    , maxPulse = fromMaybe 2400 mbMax
                    }

-- | Set the angle of the servo. The argument should be a number between 0 and 180,
-- indicating the desired angle setting in degrees.
setAngle :: Servo -> Int -> Arduino ()
setAngle :: Servo -> Int -> Arduino ()
setAngle Servo{IPin
servoPin :: Servo -> IPin
servoPin :: IPin
servoPin, Int
minPulse :: Servo -> Int
minPulse :: Int
minPulse, Int
maxPulse :: Servo -> Int
maxPulse :: Int
maxPulse} Int
angle
  | Int
angle Int -> Int -> Bool
forall a. Ord a => a -> a -> Bool
< Int
0 Bool -> Bool -> Bool
|| Int
angle Int -> Int -> Bool
forall a. Ord a => a -> a -> Bool
> Int
180
  = String -> [String] -> Arduino ()
forall a. String -> [String] -> Arduino a
die String
"Servo.setAngle: angle must be between 0 and 180." [String
"Received: " String -> String -> String
forall a. [a] -> [a] -> [a]
++ Int -> String
forall a. Show a => a -> String
show Int
angle]
  | Bool
True
  = do let duration :: Int
duration = Int
minPulse Int -> Int -> Int
forall a. Num a => a -> a -> a
+ ((Int
maxPulse Int -> Int -> Int
forall a. Num a => a -> a -> a
- Int
minPulse) Int -> Int -> Int
forall a. Num a => a -> a -> a
* Int
angle) Int -> Int -> Int
forall a. Integral a => a -> a -> a
`div` Int
180
       String -> Arduino ()
debug (String -> Arduino ()) -> String -> Arduino ()
forall a b. (a -> b) -> a -> b
$ String
"Setting servo on pin: " String -> String -> String
forall a. [a] -> [a] -> [a]
++ IPin -> String
forall a. Show a => a -> String
show IPin
servoPin String -> String -> String
forall a. [a] -> [a] -> [a]
++ String
" " String -> String -> String
forall a. [a] -> [a] -> [a]
++ Int -> String
forall a. Show a => a -> String
show Int
angle String -> String -> String
forall a. [a] -> [a] -> [a]
++ String
" degrees, via a pulse of " String -> String -> String
forall a. [a] -> [a] -> [a]
++ Int -> String
forall a. Show a => a -> String
show Int
duration String -> String -> String
forall a. [a] -> [a] -> [a]
++ String
" microseconds."
       -- In arduino, the most we can send is 16383; not that a servo should need such a large value, but
       -- just in case
       Bool -> Arduino () -> Arduino ()
forall (f :: * -> *). Applicative f => Bool -> f () -> f ()
when (Int
duration Int -> Int -> Bool
forall a. Ord a => a -> a -> Bool
>= Int
16383) (Arduino () -> Arduino ()) -> Arduino () -> Arduino ()
forall a b. (a -> b) -> a -> b
$ String -> [String] -> Arduino ()
forall a. String -> [String] -> Arduino a
die String
"Servo.setAngle angle setting: out-of-range."
                                      [ String
"Servo pin         : " String -> String -> String
forall a. [a] -> [a] -> [a]
++ IPin -> String
forall a. Show a => a -> String
show IPin
servoPin
                                      , String
"Angle required    : " String -> String -> String
forall a. [a] -> [a] -> [a]
++ Int -> String
forall a. Show a => a -> String
show Int
angle
                                      , String
"Min pulse duration: " String -> String -> String
forall a. [a] -> [a] -> [a]
++ Int -> String
forall a. Show a => a -> String
show Int
minPulse
                                      , String
"Max pulse duration: " String -> String -> String
forall a. [a] -> [a] -> [a]
++ Int -> String
forall a. Show a => a -> String
show Int
maxPulse
                                      , String
"Duration needed   : " String -> String -> String
forall a. [a] -> [a] -> [a]
++ Int -> String
forall a. Show a => a -> String
show Int
duration
                                      , String
"Exceeds max value : 16383"
                                      ]
       let msb :: Word8
msb = Int -> Word8
forall a b. (Integral a, Num b) => a -> b
fromIntegral (Int -> Word8) -> Int -> Word8
forall a b. (a -> b) -> a -> b
$ (Int
duration Int -> Int -> Int
forall a. Bits a => a -> Int -> a
`shiftR` Int
7) Int -> Int -> Int
forall a. Bits a => a -> a -> a
.&. Int
0x7f
           lsb :: Word8
lsb = Int -> Word8
forall a b. (Integral a, Num b) => a -> b
fromIntegral (Int -> Word8) -> Int -> Word8
forall a b. (a -> b) -> a -> b
$ Int
duration Int -> Int -> Int
forall a. Bits a => a -> a -> a
.&. Int
0x7f
       Request -> Arduino ()
send (Request -> Arduino ()) -> Request -> Arduino ()
forall a b. (a -> b) -> a -> b
$ IPin -> Word8 -> Word8 -> Request
AnalogPinWrite IPin
servoPin Word8
lsb Word8
msb