1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
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
|
