diff options
| -rw-r--r-- | hsm-drive/Hsm/Drive.hs | 198 | ||||
| -rw-r--r-- | hsm-drive/Test/Drive.hs | 39 | ||||
| -rw-r--r-- | hsm-drive/hsm-drive.cabal | 44 | ||||
| -rw-r--r-- | hsm-gpio/Hsm/GPIO.hs | 18 | ||||
| -rw-r--r-- | hsm-gpio/hsm-gpio.cabal | 1 | ||||
| -rw-r--r-- | hsm-pwm/Hsm/PWM.hs | 2 | ||||
| -rw-r--r-- | hsm-repl/Test/Repl.hs | 2 | ||||
| -rw-r--r-- | stack.yaml | 1 |
8 files changed, 299 insertions, 6 deletions
diff --git a/hsm-drive/Hsm/Drive.hs b/hsm-drive/Hsm/Drive.hs new file mode 100644 index 0000000..f9bdc43 --- /dev/null +++ b/hsm-drive/Hsm/Drive.hs @@ -0,0 +1,198 @@ +{-# LANGUAGE TypeFamilies #-} + +module Hsm.Drive + ( Drive + , Direction (..) + , Angle (..) + , Speed (..) + , Duration + , Action (..) + , drive + , runDrive + ) +where + +import Control.Concurrent (threadDelay) +import Control.Monad (forM_) +import Effectful (Dispatch (Static), DispatchOf, Eff, IOE, (:>)) +import Effectful.Dispatch.Static (SideEffects (WithSideEffects), StaticRep, evalStaticRep, unsafeEff_) +import Effectful.Exception (AsyncException, bracket_, handle) +import Hsm.GPIO (GPIO, GPIOPin (..), active, inactive, setPins) +import Hsm.Log (Log, Severity (Attention, Info, Trace), logMsg) +import Hsm.PWM (PWM, PWMChannel (..), setCycleDuration) + +data Drive (a :: * -> *) (b :: *) + +type instance DispatchOf Drive = Static WithSideEffects + +newtype instance StaticRep Drive + = Drive () + +-- Defines the complete action space for omnidirectional robots: +-- - 8-directional movement (cardinal + intercardinal) +-- - Bidirectional rotation (clockwise/counter-clockwise) +data Direction + = N + | NE + | E + | SE + | S + | SW + | W + | NW + deriving Show + +data Angle + = CCW + | CW + deriving Show + +data Speed + = Slow8 + | Slow4 + | Slow2 + | Slow + | Fast + | Top + deriving Show + +type Duration = Float + +data Action + = Move Direction Speed Duration + | Tilt Angle Speed Duration + | Stop Duration + deriving Show + +-- 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). Returned +-- values represent the duration of a whole PWM cycle in nanoseconds. +cycleDuration :: Speed -> Int +cycleDuration Slow8 = 8000000000 `div` 3200 -- 1/8 revs/s +cycleDuration Slow4 = 4000000000 `div` 3200 -- 1/4 revs/s +cycleDuration Slow2 = 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 + +ms1 :: GPIOPin +ms1 = GPIO2 + +ms2 :: GPIOPin +ms2 = GPIO3 + +ms3 :: GPIOPin +ms3 = GPIO4 + +notReset :: GPIOPin +notReset = GPIO27 + +notSleep :: GPIOPin +notSleep = GPIO22 + +dir1 :: GPIOPin +dir1 = GPIO10 + +dir2 :: GPIOPin +dir2 = GPIO9 + +dir3 :: GPIOPin +dir3 = GPIO11 + +dir4 :: GPIOPin +dir4 = GPIO5 + +diag2 :: GPIOPin +diag2 = GPIO6 + +diag1 :: GPIOPin +diag1 = GPIO13 + +step :: PWMChannel +step = PWM3 + +-- Executes a sequence of drive actions with interruption support +-- Wakes motors from SLEEP mode during execution and guarantees return to SLEEP +-- mode upon completion or interruption. +drive + :: (GPIO :> es, IOE :> es, Log "drive" :> es, Log "gpio" :> es, Log "pwm" :> es, PWM :> es) => [Action] -> Eff es () +drive actions = + bracket_ awaken sleep . handle handler . forM_ actions $ \action -> do + logMsg @"drive" Trace $ "Running action: " <> show action + runAction action + where + pinsDir = [dir1, dir2, dir3, dir4] + pinsDiag = [diag1, diag2] + pinDiagNE = [diag1] + pinDiagSE = [diag2] + pinDiagSW = [diag1] + pinDiagNW = [diag2] + pinsN = [dir2, dir4] + pinsE = [dir1, dir2] + pinsS = [dir1, dir3] + pinsW = [dir3, dir4] + pinsCCW = [dir1, dir2, dir3, dir4] + pinsCW = [] + -- 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. + awaken = do + logMsg @"drive" Trace "Enabling drivers" + setPins [notSleep] active + logMsg @"drive" Trace "Allowing drivers to come out of sleep mode" + unsafeEff_ $ threadDelay 1000 + sleep = do + logMsg @"drive" Trace "Disabling drivers" + setPins [notSleep] inactive + -- Catches any asynchronous exceptions during the execution of commands. + -- If an exception occurs, the system will log the error and ensure that + -- all pins are deactivated. + endAction = do + setCycleDuration step 0 + setPins (pinsDir <> pinsDiag) inactive + handler exception = do + logMsg @"drive" Attention $ "Async exception caught while action was running: " <> show @AsyncException exception + endAction + -- 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. + delay seconds = unsafeEff_ . threadDelay . round $ seconds * 1000000 + togglePins speed duration pins = do + setCycleDuration step $ cycleDuration speed + setPins pins active + delay duration + endAction + runAction (Move direction speed duration) = + togglePins speed duration $ + case direction of + N -> pinsN <> pinsDiag + NE -> pinsN <> pinDiagNE + E -> pinsE <> pinsDiag + SE -> pinsS <> pinDiagSE + S -> pinsS <> pinsDiag + SW -> pinsS <> pinDiagSW + W -> pinsW <> pinsDiag + NW -> pinsN <> pinDiagNW + runAction (Tilt angle speed duration) = + togglePins speed duration $ + case angle of + CCW -> pinsCCW <> pinsDiag + CW -> pinsCW <> pinsDiag + -- 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) = delay duration + +runDrive :: (GPIO :> es, IOE :> es, Log "drive" :> es, Log "gpio" :> es) => Eff (Drive : es) a -> Eff es a +runDrive = evalStaticRep (Drive ()) . bracket_ enterActive exitActive + where + alwaysActive = [ms1, ms2, ms3, notReset] + enterActive = do + logMsg @"drive" Info "Entering active drive state" + setPins alwaysActive active + exitActive = do + logMsg @"drive" Info "Exiting active drive state" + setPins alwaysActive inactive diff --git a/hsm-drive/Test/Drive.hs b/hsm-drive/Test/Drive.hs new file mode 100644 index 0000000..e6332fd --- /dev/null +++ b/hsm-drive/Test/Drive.hs @@ -0,0 +1,39 @@ +{-# LANGUAGE TemplateHaskell #-} + +import Control.Monad.Loops (whileJust_) +import Data.Function ((&)) +import Effectful (runEff) +import Effectful.Dispatch.Static (unEff, unsafeEff) +import GHC.TypeLits (AppendSymbol) +import Hsm.Core.App (bootstrapApp) +import Hsm.Drive (drive, runDrive) +import Hsm.GPIO (runGPIO) +import Hsm.Log (Severity (Info), runLogsOpt) +import Hsm.Log.Options (makeLoggerOptionParser) +import Hsm.PWM (runPWM) +import Hsm.Repl (repl, runRepl) +-- Import full module for cleaner `-ddump-splices` output +-- Avoids package/module qualifiers in generated code +import Options.Applicative +import System.IO.Echo (withoutInputEcho) + +type Name = "test-drive" + +type Prompt = AppendSymbol Name " λ " + +type Imports = '["Hsm.Drive", "Prelude"] + +type Loggers = '["drive", "gpio", "pwm", "repl"] + +$(makeLoggerOptionParser @Loggers "Options" "parser" 'Info) + +main :: IO () +main = + bootstrapApp parser "Launch Drive Service Test Application" $ \opts -> + whileJust_ repl (\actions -> unsafeEff $ withoutInputEcho . unEff (drive actions)) + & runDrive + & runGPIO @Name + & runPWM + & runRepl @Prompt @Imports + & runLogsOpt @Options @Loggers opts + & runEff diff --git a/hsm-drive/hsm-drive.cabal b/hsm-drive/hsm-drive.cabal new file mode 100644 index 0000000..0443dda --- /dev/null +++ b/hsm-drive/hsm-drive.cabal @@ -0,0 +1,44 @@ +cabal-version: 3.8 +author: Paul Oliver <contact@pauloliver.dev> +name: hsm-drive +version: 0.1.0.0 + +library + build-depends: + , base + , effectful-core + , effectful-plugin + , hsm-gpio + , hsm-log + , hsm-pwm + + default-language: GHC2024 + exposed-modules: Hsm.Drive + ghc-options: + -O2 -Wall -Werror -Wno-star-is-type -Wunused-packages + -fplugin=Effectful.Plugin + +executable test-drive + build-depends: + , base + , echo + , effectful-core + , effectful-plugin + , hsm-core + , hsm-gpio + , hsm-log + , hsm-pwm + , hsm-repl + , monad-loops + , optparse-applicative + + default-language: GHC2024 + ghc-options: + -O2 -Wall -Werror -Wno-star-is-type -Wunused-packages + -Wno-unused-imports -ddump-splices -fplugin=Effectful.Plugin + + if !arch(x86_64) + ghc-options: -optl=-mno-fix-cortex-a53-835769 + + main-is: Test/Drive.hs + other-modules: Hsm.Drive diff --git a/hsm-gpio/Hsm/GPIO.hs b/hsm-gpio/Hsm/GPIO.hs index 31b73d9..c182cf1 100644 --- a/hsm-gpio/Hsm/GPIO.hs +++ b/hsm-gpio/Hsm/GPIO.hs @@ -3,7 +3,9 @@ {-# LANGUAGE TypeFamilies #-} module Hsm.GPIO - ( GPIOPin (..) + ( active + , inactive + , GPIOPin (..) , GPIO , setPins , setAllPins @@ -13,18 +15,21 @@ where import Control.Monad (forM_, void) import Control.Monad.Trans.Cont (evalCont) +import Data.Proxy (Proxy (Proxy)) import Data.Vector.Storable (fromList, replicate, unsafeWith) import Effectful (Dispatch (Static), DispatchOf, Eff, IOE, liftIO, (:>)) import Effectful.Dispatch.Static (SideEffects (WithSideEffects), StaticRep, evalStaticRep, getStaticRep, unsafeEff_) -import Effectful.Exception (bracket) +import Effectful.Exception (bracket, finally) import Foreign.C.String (withCString) import Foreign.C.Types (CSize (CSize), CUInt) import Foreign.Ptr (Ptr) +import GHC.TypeLits (KnownSymbol, symbolVal) import Hsm.Core.Bracket (bracketCont) import Hsm.Core.Serial (makeSerial) import Hsm.GPIO.FFI ( LineRequest , LineValue + , active , chipClose , chipOpen , chipRequestLines @@ -77,9 +82,14 @@ setAllPins lineValue = do logMsg Trace $ "Setting all pins " <> show allPins <> " to " <> show lineValue unsafeEff_ . unsafeWith (replicate pinCount lineValue) $ void . lineRequestSetValues lineRequest -runGPIO :: (IOE :> es, Log "gpio" :> es) => String -> Eff (GPIO : es) a -> Eff es a -runGPIO consumer action = bracket lineRequestAlloc lineRequestDealloc $ \lineRequest -> evalStaticRep (GPIO lineRequest) action +runGPIO + :: forall c es a + . (IOE :> es, KnownSymbol c, Log "gpio" :> es) + => Eff (GPIO : es) a + -> Eff es a +runGPIO action = bracket lineRequestAlloc lineRequestDealloc $ \lineRequest -> evalStaticRep (GPIO lineRequest) . finally action $ setAllPins inactive where + consumer = symbolVal $ Proxy @c chipPath = "/dev/gpiochip0" chipAlloc = do logMsg Info $ "Opening GPIO chip " <> chipPath diff --git a/hsm-gpio/hsm-gpio.cabal b/hsm-gpio/hsm-gpio.cabal index ba538db..75e22f8 100644 --- a/hsm-gpio/hsm-gpio.cabal +++ b/hsm-gpio/hsm-gpio.cabal @@ -15,6 +15,7 @@ library default-language: GHC2024 exposed-modules: Hsm.GPIO + extra-libraries: gpiod ghc-options: -O2 -Wall -Werror -Wno-star-is-type -Wunused-packages -ddump-splices -fplugin=Effectful.Plugin diff --git a/hsm-pwm/Hsm/PWM.hs b/hsm-pwm/Hsm/PWM.hs index bc31fbc..23e9e3a 100644 --- a/hsm-pwm/Hsm/PWM.hs +++ b/hsm-pwm/Hsm/PWM.hs @@ -30,7 +30,7 @@ import Hsm.Log (Log, Severity (Info, Trace), logMsg) import System.FilePath ((</>)) import System.Posix.Files (fileAccess) -$(makeSerial "PWM" "Channel" "channelIndex" ''Int [1, 2]) +$(makeSerial "PWM" "Channel" "channelIndex" ''Int [2, 3]) data PWM (a :: * -> *) (b :: *) diff --git a/hsm-repl/Test/Repl.hs b/hsm-repl/Test/Repl.hs index 7d1431c..bd79a1e 100644 --- a/hsm-repl/Test/Repl.hs +++ b/hsm-repl/Test/Repl.hs @@ -5,4 +5,4 @@ import Hsm.Log (Severity (Trace), runLog) import Hsm.Repl (repl, runRepl) main :: IO () -main = whileJust_ repl return & runRepl @"exec-repl λ " @'["Prelude"] @[Bool] & runLog @"repl" Trace & runEff +main = whileJust_ repl return & runRepl @"test-repl λ " @'["Prelude"] @[Bool] & runLog @"repl" Trace & runEff @@ -4,6 +4,7 @@ extra-deps: packages: - hsm-cam - hsm-core + - hsm-drive - hsm-gpio - hsm-log - hsm-pwm |