diff options
| -rw-r--r-- | hsm-bin/Test/Drive.hs | 22 | ||||
| -rw-r--r-- | hsm-bin/hsm-bin.cabal | 8 | ||||
| -rw-r--r-- | hsm-drive/Hsm/Drive.hs | 177 | ||||
| -rw-r--r-- | hsm-drive/hsm-drive.cabal | 19 | ||||
| -rw-r--r-- | hsm-pwm/Hsm/PWM.hs | 23 | ||||
| -rw-r--r-- | stack.yaml | 1 |
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 @@ -1,5 +1,6 @@ packages: - hsm-bin + - hsm-drive - hsm-gpio - hsm-log - hsm-pwm |