-
-
Notifications
You must be signed in to change notification settings - Fork 1k
/
easy_driver.go
270 lines (217 loc) · 6.83 KB
/
easy_driver.go
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
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
package gpio
import (
"fmt"
"strings"
"time"
"gobot.io/x/gobot/v2"
)
const easyDriverDebug = false
// easyOptionApplier needs to be implemented by each configurable option type
type easyOptionApplier interface {
apply(cfg *easyConfiguration)
}
// easyConfiguration contains all changeable attributes of the driver.
type easyConfiguration struct {
dirPin string
enPin string
sleepPin string
}
// easyDirPinOption is the type for applying a pin for change direction
type easyDirPinOption string
// easyEnPinOption is the type for applying a pin for device disabling/enabling
type easyEnPinOption string
// easySleepPinOption is the type for applying a pin for setting device to sleep/wake
type easySleepPinOption string
// EasyDriver is an driver for stepper hardware board from SparkFun (https://www.sparkfun.com/products/12779)
// This should also work for the BigEasyDriver (untested). It is basically a wrapper for the common StepperDriver{}
// with the specific additions for the board, e.g. direction, enable and sleep outputs.
type EasyDriver struct {
*StepperDriver
easyCfg *easyConfiguration
stepPin string
anglePerStep float32
sleeping bool
}
// NewEasyDriver returns a new driver
// TODO: Support selecting phase input instead of hard-wiring MS1 and MS2 to board truth table
// A - DigitalWriter
// anglePerStep - Step angle of motor
// stepPin - Pin corresponding to step input on EasyDriver
//
// Supported options:
//
// "WithName"
// "WithEasyDirectionPin"
// "WithEasyEnablePin"
// "WithEasySleepPin"
func NewEasyDriver(a DigitalWriter, anglePerStep float32, stepPin string, opts ...interface{}) *EasyDriver {
if anglePerStep <= 0 {
panic("angle per step needs to be greater than zero")
}
if stepPin == "" {
panic("step pin is mandatory for easy driver")
}
stepper := NewStepperDriver(a, [4]string{}, nil, 1)
stepper.driverCfg.name = gobot.DefaultName("EasyDriver")
stepper.stepperDebug = easyDriverDebug
stepper.haltIfRunning = false
stepper.stepsPerRev = 360.0 / anglePerStep
d := &EasyDriver{
StepperDriver: stepper,
easyCfg: &easyConfiguration{},
stepPin: stepPin,
anglePerStep: anglePerStep,
}
d.stepFunc = d.onePinStepping
d.sleepFunc = d.sleepWithSleepPin
d.beforeHalt = d.shutdown
// 1/4 of max speed. Not too fast, not too slow
d.speedRpm = d.MaxSpeed() / 4
for _, opt := range opts {
switch o := opt.(type) {
case optionApplier:
o.apply(d.driverCfg)
case easyOptionApplier:
o.apply(d.easyCfg)
default:
oNames := []string{"WithEasyDirectionPin", "WithEasyEnablePin", "WithEasySleepPin"}
msg := fmt.Sprintf("'%s' can not be applied on '%s', consider to use one of the options instead: %s",
opt, d.driverCfg.name, strings.Join(oNames, ", "))
panic(msg)
}
}
return d
}
// WithEasyDirectionPin configure a pin for change the moving direction.
func WithEasyDirectionPin(pin string) easyOptionApplier {
return easyDirPinOption(pin)
}
// WithEasyEnablePin configure a pin for disabling/enabling the driver.
func WithEasyEnablePin(pin string) easyOptionApplier {
return easyEnPinOption(pin)
}
// WithEasySleepPin configure a pin for sleep/wake the driver.
func WithEasySleepPin(pin string) easyOptionApplier {
return easySleepPinOption(pin)
}
// SetDirection sets the direction to be moving.
func (d *EasyDriver) SetDirection(direction string) error {
if d.easyCfg.dirPin == "" {
return fmt.Errorf("dirPin is not set for '%s'", d.driverCfg.name)
}
direction = strings.ToLower(direction)
if direction != StepperDriverForward && direction != StepperDriverBackward {
return fmt.Errorf("Invalid direction '%s'. Value should be '%s' or '%s'",
direction, StepperDriverForward, StepperDriverBackward)
}
writeVal := byte(0) // low is forward
if direction == StepperDriverBackward {
writeVal = 1 // high is backward
}
if err := d.digitalWrite(d.easyCfg.dirPin, writeVal); err != nil {
return err
}
// ensure that write of variable can not interfere with read in step()
d.valueMutex.Lock()
defer d.valueMutex.Unlock()
d.direction = direction
return nil
}
// Enable enables all motor output
func (d *EasyDriver) Enable() error {
if d.easyCfg.enPin == "" {
d.disabled = false
return fmt.Errorf("enPin is not set - board '%s' is enabled by default", d.driverCfg.name)
}
// enPin is active low
if err := d.digitalWrite(d.easyCfg.enPin, 0); err != nil {
return err
}
d.disabled = false
return nil
}
// Disable disables all motor output
func (d *EasyDriver) Disable() error {
if d.easyCfg.enPin == "" {
return fmt.Errorf("enPin is not set for '%s'", d.driverCfg.name)
}
_ = d.stopIfRunning() // drop step errors
// enPin is active low
if err := d.digitalWrite(d.easyCfg.enPin, 1); err != nil {
return err
}
d.disabled = true
return nil
}
// IsEnabled returns a bool stating whether motor is enabled
func (d *EasyDriver) IsEnabled() bool {
return !d.disabled
}
// Wake wakes up the driver
func (d *EasyDriver) Wake() error {
if d.easyCfg.sleepPin == "" {
return fmt.Errorf("sleepPin is not set for '%s'", d.driverCfg.name)
}
// sleepPin is active low
if err := d.digitalWrite(d.easyCfg.sleepPin, 1); err != nil {
return err
}
d.sleeping = false
// we need to wait 1ms after sleeping before doing a step to charge the step pump (according to data sheet)
time.Sleep(1 * time.Millisecond)
return nil
}
// IsSleeping returns a bool stating whether motor is sleeping
func (d *EasyDriver) IsSleeping() bool {
return d.sleeping
}
func (d *EasyDriver) onePinStepping() error {
// ensure that read and write of variables (direction, stepNum) can not interfere
d.valueMutex.Lock()
defer d.valueMutex.Unlock()
// a valid steps occurs for a low to high transition
if err := d.digitalWrite(d.stepPin, 0); err != nil {
return err
}
time.Sleep(d.getDelayPerStep())
if err := d.digitalWrite(d.stepPin, 1); err != nil {
return err
}
if d.direction == StepperDriverForward {
d.stepNum++
} else {
d.stepNum--
}
return nil
}
// sleepWithSleepPin puts the driver to sleep and disables all motor output. Low power mode.
func (d *EasyDriver) sleepWithSleepPin() error {
if d.easyCfg.sleepPin == "" {
return fmt.Errorf("sleepPin is not set for '%s'", d.driverCfg.name)
}
_ = d.stopIfRunning() // drop step errors
// sleepPin is active low
if err := d.digitalWrite(d.easyCfg.sleepPin, 0); err != nil {
return err
}
d.sleeping = true
return nil
}
func (o easyDirPinOption) String() string {
return "direction pin option easy driver"
}
func (o easyEnPinOption) String() string {
return "enable pin option easy driver"
}
func (o easySleepPinOption) String() string {
return "sleep pin option easy driver"
}
func (o easyDirPinOption) apply(cfg *easyConfiguration) {
cfg.dirPin = string(o)
}
func (o easyEnPinOption) apply(cfg *easyConfiguration) {
cfg.enPin = string(o)
}
func (o easySleepPinOption) apply(cfg *easyConfiguration) {
cfg.sleepPin = string(o)
}