Adds drive service and test app

6 files changed, 242 insertions, 8 deletions

diff --git a/hsm-bin/Test/Drive.hs b/hsm-bin/Test/Drive.hs
new file mode 100644
index 0000000..7a58c11
--- /dev/null
+++ b/hsm-bin/Test/Drive.hs
@@ -0,0 +1,22 @@
+{-# LANGUAGE OverloadedStrings #-}
+
+import Control.IO.Region (region)
+import Control.Monad.Loops (whileJust_)
+import Hsm.Drive (DriveEnv, drive, driveEnvDefault)
+import Hsm.GPIO (allocateGPIO)
+import Hsm.PWM (allocatePWM)
+import Hsm.Readline (allocateReadline, readline)
+
+newtype Env = Env
+  { driveEnv :: DriveEnv
+  }
+
+main :: IO ()
+main =
+  region $ \ioRegion -> do
+    lineRequest <- allocateGPIO ioRegion "test-status"
+    pwmHandle <- allocatePWM ioRegion $ const 0
+    handle <- allocateReadline ioRegion
+    whileJust_ (readline handle)
+      $ drive lineRequest pwmHandle
+      $ Env driveEnvDefault
diff --git a/hsm-bin/hsm-bin.cabal b/hsm-bin/hsm-bin.cabal
index 0dc0a81..de6e1a5 100644
--- a/hsm-bin/hsm-bin.cabal
+++ b/hsm-bin/hsm-bin.cabal
@@ -24,3 +24,11 @@ executable test-status
   import:        test-executable
   build-depends: hsm-status
   main-is:       Test/Status.hs
+
+executable test-drive
+  import:        test-executable
+  build-depends:
+    , hsm-drive
+    , hsm-pwm
+
+  main-is:       Test/Drive.hs
diff --git a/hsm-drive/Hsm/Drive.hs b/hsm-drive/Hsm/Drive.hs
new file mode 100644
index 0000000..3580b5a
--- /dev/null
+++ b/hsm-drive/Hsm/Drive.hs
@@ -0,0 +1,177 @@
+{-# LANGUAGE DataKinds #-}
+{-# LANGUAGE OverloadedRecordDot #-}
+{-# LANGUAGE OverloadedStrings #-}
+
+module Hsm.Drive
+  ( Speed(..)
+  , Direction(..)
+  , Duration
+  , DriveAction(..)
+  , DriveEnv(..)
+  , driveEnvDefault
+  , drive
+  ) where
+
+import Control.Concurrent (threadDelay)
+import Control.Exception (AsyncException, handle)
+import Control.Monad (forM_)
+import Data.Text (pack)
+import Data.Typeable (Typeable)
+import Foreign.Ptr (Ptr)
+import GHC.Records (HasField)
+import Hsm.GPIO (GPIO(..), LineRequest, active, inactive, setPins)
+import Hsm.Log qualified as L
+import Hsm.PWM qualified as P
+import System.IO.Echo (withoutInputEcho)
+
+data Direction
+  = N
+  | NE
+  | E
+  | SE
+  | S
+  | SW
+  | W
+  | NW
+  | RL
+  | RR
+  deriving (Read, Show, Typeable)
+
+data Speed
+  = SlowXXX
+  | SlowXX
+  | SlowX
+  | Slow
+  | Fast
+  | Top
+  deriving (Read, Show, Typeable)
+
+type Duration = Float
+
+data DriveAction
+  = Move Direction Speed Duration
+  | Stop Duration
+  deriving (Read, Show, Typeable)
+
+-- This function maps a `Speed` value to a corresponding PWM cycle duration.
+-- It assumes a stepper motor with 200 steps per revolution, using a 1/16
+-- microstep setting (so actually, 3200 steps per revolution). The returned
+-- values represent the duration of a whole PWM cycle in nanoseconds.
+cycleDuration :: Speed -> Int
+cycleDuration SlowXXX = 8000000000 `div` 3200 -- 1/8 revs/s
+cycleDuration SlowXX = 4000000000 `div` 3200 -- 1/4 revs/s
+cycleDuration SlowX = 2000000000 `div` 3200 -- 1/2 revs/s
+cycleDuration Slow = 1000000000 `div` 3200 -- 1 revs/s
+cycleDuration Fast = 500000000 `div` 3200 -- 2 revs/s
+cycleDuration Top = 250000000 `div` 3200 -- 4 revs/s
+
+data DriveEnv = DriveEnv
+  { pinEnable :: GPIO
+  , pinDiag1 :: GPIO
+  , pinDiag2 :: GPIO
+  , pinDir1 :: GPIO
+  , pinDir2 :: GPIO
+  , pinDir3 :: GPIO
+  , pinDir4 :: GPIO
+  , pwmChannel :: P.PWMChannel
+  }
+
+driveEnvDefault :: DriveEnv
+driveEnvDefault =
+  DriveEnv
+    { pinEnable = GPIO21
+    , pinDiag1 = GPIO20
+    , pinDiag2 = GPIO16
+    , pinDir1 = GPIO12
+    , pinDir2 = GPIO7
+    , pinDir3 = GPIO8
+    , pinDir4 = GPIO25
+    , pwmChannel = P.PWM3
+    }
+
+drive ::
+     HasField "driveEnv" env DriveEnv
+  => Ptr LineRequest
+  -> P.PWMHandle
+  -> env
+  -> [DriveAction]
+  -> IO Bool
+drive lineRequest pwmHandle env actions =
+  withoutInputEcho $ handle handler runActions
+  where
+    logMsg = L.logMsg ["drive"]
+    -- Sets GPIO pins to a desired state
+    setCycleDuration = P.setCycleDuration pwmHandle env.driveEnv.pwmChannel
+    setActive pins = setPins lineRequest (pins <*> [env.driveEnv]) active
+    setInactive pins = setPins lineRequest (pins <*> [env.driveEnv]) inactive
+    -- Pin assignments for various movement directions, each direction
+    -- corresponds to a specific set of GPIO pins.
+    pinsDiag = [pinDiag1, pinDiag2]
+    pinsDir = [pinDir1, pinDir2, pinDir3, pinDir4]
+    pinDiagNE = [pinDiag1]
+    pinDiagSE = [pinDiag2]
+    pinDiagSW = [pinDiag1]
+    pinDiagNW = [pinDiag2]
+    pinsN = [pinDir2, pinDir4]
+    pinsE = [pinDir1, pinDir2]
+    pinsS = [pinDir1, pinDir3]
+    pinsW = [pinDir3, pinDir4]
+    pinsRL = [pinDir1, pinDir2, pinDir3, pinDir4]
+    pinsRR = []
+    -- Introduces a delay with the duration converted from seconds to
+    -- microseconds.
+    runDelay = threadDelay . round . (* 1000000)
+    -- Ensures that the system is reset to a safe state by setting the PWM
+    -- cycle duration to zero and deactivating all used motor control pins.
+    runRelease = do
+      setCycleDuration 0
+      setInactive $ pinsDir <> pinsDiag
+    -- Handles each movement command and activates the appropriate pins for
+    -- the requested direction. It also sets the cycle duration for PWM and
+    -- holds this state for the specified duration.
+    runAction (Move direction speed duration) = do
+      case direction of
+        N -> setActive $ pinsN <> pinsDiag
+        NE -> setActive $ pinsN <> pinDiagNE
+        E -> setActive $ pinsE <> pinsDiag
+        SE -> setActive $ pinsS <> pinDiagSE
+        S -> setActive $ pinsS <> pinsDiag
+        SW -> setActive $ pinsS <> pinDiagSW
+        W -> setActive $ pinsW <> pinsDiag
+        NW -> setActive $ pinsN <> pinDiagNW
+        RL -> setActive $ pinsRL <> pinsDiag
+        RR -> setActive $ pinsRR <> pinsDiag
+      setCycleDuration $ cycleDuration speed
+      runDelay duration
+      runRelease
+    -- A Stop command causes the system to wait for the specified duration
+    -- without performing any movement. During this period, the motor drivers
+    -- remain enabled, effectively applying a brake to the motor by holding it
+    -- in its current position.
+    runAction (Stop duration) = runDelay duration
+    -- Catches any asynchronous exceptions during the execution of commands.
+    -- If an exception occurs, the system will log the error and ensure that
+    -- the motors are safely released by calling `runRelease`.
+    handler exception = do
+      logMsg
+        $ "Async exception caught while command was running: "
+            <> pack (show @AsyncException exception)
+      runRelease
+      return False
+    -- Executes a series of drive actions
+    runActions = do
+      setCycleDuration 0
+      -- The A4988 motor driver is placed in sleep mode between commands to
+      -- save power. To wake it up, a 1-microsecond delay is required before
+      -- sending step commands. For added safety, we wait 1 millisecond to
+      -- ensure the driver is fully awake.
+      logMsg "Enabling drivers"
+      setActive [pinEnable]
+      logMsg "Allowing drivers to come out of sleep mode"
+      threadDelay 1000
+      forM_ actions $ \action -> do
+        logMsg $ "Running action " <> pack (show action)
+        runAction action
+      logMsg "Disabling drivers"
+      setInactive [pinEnable]
+      return True
diff --git a/hsm-drive/hsm-drive.cabal b/hsm-drive/hsm-drive.cabal
new file mode 100644
index 0000000..a9dbe69
--- /dev/null
+++ b/hsm-drive/hsm-drive.cabal
@@ -0,0 +1,19 @@
+cabal-version: 3.4
+author:        Paul Oliver
+build-type:    Simple
+maintainer:    contact@pauloliver.dev
+name:          hsm-drive
+version:       0.1.0.0
+
+library
+  build-depends:
+    , base
+    , echo
+    , hsm-gpio
+    , hsm-log
+    , hsm-pwm
+    , text
+
+  exposed-modules:  Hsm.Drive
+  ghc-options:      -Wall -Wunused-packages
+  default-language: GHC2021
diff --git a/hsm-pwm/Hsm/PWM.hs b/hsm-pwm/Hsm/PWM.hs
index ad4c052..6b2a882 100644
--- a/hsm-pwm/Hsm/PWM.hs
+++ b/hsm-pwm/Hsm/PWM.hs
@@ -3,7 +3,7 @@
 module Hsm.PWM
   ( PWMHandle
   , PWMChannel(PWM2, PWM3)
-  , setFrequency
+  , setCycleDuration
   , allocatePWM
   ) where
 
@@ -78,16 +78,23 @@ setDutyCycle channel dutyCycle = do
   where
     (_, _, dutyCyclePath) = channelPaths channel
 
-setFrequency :: PWMHandle -> PWMChannel -> Int -> IO ()
-setFrequency _ channel frequency = do
+setCycleDuration :: PWMHandle -> PWMChannel -> Int -> IO ()
+setCycleDuration _ channel 0 = do
+  logMsg $ "Halting PWM signals on channel " <> pack (show channel)
+  setEnable channel False
+setCycleDuration _ channel cycleDuration = do
   logMsg
-    $ "Setting frequency on channel "
+    $ "Setting cycle duration on channel "
         <> pack (show channel)
         <> " to "
-        <> pack (show frequency)
+        <> pack (show cycleDuration)
   setEnable channel False
-  setPeriod channel frequency
-  setDutyCycle channel $ frequency `div` 2
+  -- Sets the duty cycle to zero before updating the period. This prevents
+  -- `Invalid argument` errors, as the period must never be set to a value
+  -- smaller than the duty cycle.
+  setDutyCycle channel 0
+  setPeriod channel cycleDuration
+  setDutyCycle channel $ cycleDuration `div` 2
   setEnable channel True
 
 allocatePWM :: Region -> (PWMChannel -> Int) -> IO PWMHandle
@@ -118,7 +125,7 @@ allocatePWM region mapper = alloc_ region acquire $ const release
       waitWritable enablePath
       waitWritable periodPath
       waitWritable dutyCyclePath
-      setFrequency PWMHandle channel $ mapper channel
+      setCycleDuration PWMHandle channel $ mapper channel
     acquire = do
       waitWritable exportPath
       waitWritable unexportPath
diff --git a/stack.yaml b/stack.yaml
index 6011fcd..0fda386 100644
--- a/stack.yaml
+++ b/stack.yaml
@@ -1,5 +1,6 @@
 packages:
   - hsm-bin
+  - hsm-drive
   - hsm-gpio
   - hsm-log
   - hsm-pwm