InitialHEADmaster

1 files changed, 105 insertions, 0 deletions

diff --git a/hsm-drive/Hsm/Drive.hs b/hsm-drive/Hsm/Drive.hs
new file mode 100644
index 0000000..dffb8dc
--- /dev/null
+++ b/hsm-drive/Hsm/Drive.hs
@@ -0,0 +1,105 @@
+{-# LANGUAGE DataKinds #-}
+{-# LANGUAGE OverloadedRecordDot #-}
+{-# LANGUAGE OverloadedStrings #-}
+
+module Hsm.Drive
+  ( Speed(Slow, Medium, Fast)
+  , Direction(Forward, Backward)
+  , Duration
+  , DriveEnv(..)
+  , Command(Move)
+  , pwmMapperDefault
+  , driveEnvDefault
+  , drive
+  ) where
+
+import Control.Concurrent (threadDelay)
+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 (PWMChannel(PWM3), PWMHandle, setDutyCycle)
+
+data Speed
+  = Slow
+  | Medium
+  | Fast
+  deriving (Read, Show, Typeable)
+
+data Direction
+  = Forward
+  | Backward
+  deriving (Read, Show, Typeable)
+
+type Duration = Float
+
+data Command
+  = Move Speed Direction Duration
+  | Stop Duration
+  deriving (Read, Show, Typeable)
+
+data DriveEnv = DriveEnv
+  { gpioM1F :: GPIO
+  , gpioM1B :: GPIO
+  , gpioM2F :: GPIO
+  , gpioM2B :: GPIO
+  , gpioM3F :: GPIO
+  , gpioM3B :: GPIO
+  , gpioM4F :: GPIO
+  , gpioM4B :: GPIO
+  , pwmChannel :: PWMChannel
+  , pwmPeriod :: Int
+  , pwmMapper :: Speed -> Int
+  }
+
+pwmMapperDefault :: Speed -> Int
+pwmMapperDefault Slow = 500000
+pwmMapperDefault Medium = 750000
+pwmMapperDefault Fast = 1000000
+
+driveEnvDefault :: DriveEnv
+driveEnvDefault =
+  DriveEnv
+    { gpioM1F = GPIO24
+    , gpioM1B = GPIO25
+    , gpioM2F = GPIO8
+    , gpioM2B = GPIO7
+    , gpioM3F = GPIO12
+    , gpioM3B = GPIO16
+    , gpioM4F = GPIO20
+    , gpioM4B = GPIO21
+    , pwmChannel = PWM3
+    , pwmPeriod = 1000000 -- 1ms
+    , pwmMapper = pwmMapperDefault
+    }
+
+drive ::
+     HasField "driveEnv" env DriveEnv
+  => PWMHandle
+  -> Ptr LineRequest
+  -> env
+  -> [Command]
+  -> IO Bool
+drive pwmHandle lineRequest env commands = do
+  mapM_ runCommand commands
+  return True
+  where
+    logMsg = L.logMsg ["drive"]
+    pinsForward = [gpioM1F, gpioM2F, gpioM3F, gpioM4F] <*> [env.driveEnv]
+    pinsBackward = [gpioM1B, gpioM2B, gpioM3B, gpioM4B] <*> [env.driveEnv]
+    toMicroSeconds = round . (* 1000000)
+    runCommand command = do
+      logMsg $ "Running command: " <> pack (show command)
+      case command of
+        (Move speed direction duration) -> do
+          case direction of
+            Forward -> setPins lineRequest pinsForward active
+            Backward -> setPins lineRequest pinsBackward active
+          setDutyCycle pwmHandle env.driveEnv.pwmChannel
+            $ env.driveEnv.pwmMapper speed
+          threadDelay $ toMicroSeconds duration
+          setDutyCycle pwmHandle env.driveEnv.pwmChannel 0
+          setPins lineRequest (pinsForward <> pinsBackward) inactive
+        (Stop duration) -> threadDelay $ toMicroSeconds duration