From e586927bdeef9d1d59f464c0fed86977ec9fd6c4 Mon Sep 17 00:00:00 2001
From: Paul Oliver <contact@pauloliver.dev>
Date: Wed, 26 Feb 2025 18:07:58 -0800
Subject: Adds drive service and test app
---
hsm-drive/Hsm/Drive.hs | 177 +++++++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 177 insertions(+)
create mode 100644 hsm-drive/Hsm/Drive.hs
(limited to 'hsm-drive/Hsm')
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
--
cgit v1.2.1