diff --git a/Makefile b/Makefile index b83504e45..85cfec583 100644 --- a/Makefile +++ b/Makefile @@ -58,6 +58,7 @@ fmt_check: fmt_fix: $(MAKE) version_check || true gofumpt -l -w . + golangci-lint run -v --fix examples: $(EXAMPLES) diff --git a/drivers/gpio/easy_driver.go b/drivers/gpio/easy_driver.go index 26687d71b..c7c01a8ed 100644 --- a/drivers/gpio/easy_driver.go +++ b/drivers/gpio/easy_driver.go @@ -2,257 +2,111 @@ package gpio import ( "fmt" - "log" - "strconv" - "sync" + "strings" "time" - "github.com/hashicorp/go-multierror" "gobot.io/x/gobot/v2" ) -// EasyDriver object +const easyDriverDebug = false + +// 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 { - *Driver - - stepPin string - dirPin string - enPin string - sleepPin string - - angle float32 - rpm uint - dir int8 - stepNum int - enabled bool - sleeping bool - runStopChan chan struct{} - runStopWaitGroup *sync.WaitGroup + *StepperDriver + + stepPin string + dirPin string + enPin string + sleepPin string + anglePerStep float32 + + sleeping bool } -// NewEasyDriver returns a new driver for EasyDriver from SparkFun (https://www.sparkfun.com/products/12779) +// NewEasyDriver returns a new driver // TODO: Support selecting phase input instead of hard-wiring MS1 and MS2 to board truth table -// This should also work for the BigEasyDriver (untested) // A - DigitalWriter +// anglePerStep - Step angle of motor // stepPin - Pin corresponding to step input on EasyDriver // dirPin - Pin corresponding to dir input on EasyDriver. Optional // enPin - Pin corresponding to enabled input on EasyDriver. Optional // sleepPin - Pin corresponding to sleep input on EasyDriver. Optional -// angle - Step angle of motor func NewEasyDriver( a DigitalWriter, - angle float32, + anglePerStep float32, stepPin string, dirPin string, enPin string, sleepPin string, ) *EasyDriver { - if angle <= 0 { - panic("angle needs to be greater than zero") - } - d := &EasyDriver{ - Driver: NewDriver(a.(gobot.Connection), "EasyDriver"), - stepPin: stepPin, - dirPin: dirPin, - enPin: enPin, - sleepPin: sleepPin, - angle: angle, - rpm: 1, - dir: 1, - enabled: true, - sleeping: false, - } - d.beforeHalt = func() error { - if err := d.Stop(); err != nil { - fmt.Printf("no need to stop motion: %v\n", err) - } - - return nil + if anglePerStep <= 0 { + panic("angle per step needs to be greater than zero") } - // panic if step pin isn't set if stepPin == "" { panic("Step pin is not set") } - // 1/4 of max speed. Not too fast, not too slow - d.rpm = d.MaxSpeed() / 4 - - d.AddCommand("Move", func(params map[string]interface{}) interface{} { - degs, _ := strconv.Atoi(params["degs"].(string)) - return d.Move(degs) - }) - d.AddCommand("Step", func(params map[string]interface{}) interface{} { - return d.Step() - }) - d.AddCommand("Run", func(params map[string]interface{}) interface{} { - return d.Run() - }) - d.AddCommand("Stop", func(params map[string]interface{}) interface{} { - return d.Stop() - }) - - return d -} - -// Move the motor given number of degrees at current speed. The move can be stopped asynchronously. -func (d *EasyDriver) Move(degs int) error { - // ensure that move and run can not interfere - d.mutex.Lock() - defer d.mutex.Unlock() - - if !d.enabled { - return fmt.Errorf("motor '%s' is disabled and can not be running", d.name) - } - - if d.runStopChan != nil { - return fmt.Errorf("motor '%s' already running or moving", d.name) - } - - d.runStopChan = make(chan struct{}) - d.runStopWaitGroup = &sync.WaitGroup{} - d.runStopWaitGroup.Add(1) - - defer func() { - close(d.runStopChan) - d.runStopChan = nil - d.runStopWaitGroup.Done() - }() - - steps := int(float32(degs) / d.angle) - if steps <= 0 { - fmt.Printf("steps are smaller than zero, no move for '%s'\n", d.name) - } - - for i := 0; i < steps; i++ { - select { - case <-d.runStopChan: - // don't continue to step if driver is stopped - log.Println("stop happen") - return nil - default: - if err := d.step(); err != nil { - return err - } - } - } - - return nil -} - -// Run the stepper continuously. -func (d *EasyDriver) Run() error { - // ensure that run, can not interfere with step or move - d.mutex.Lock() - defer d.mutex.Unlock() - - if !d.enabled { - return fmt.Errorf("motor '%s' is disabled and can not be moving", d.name) - } - - if d.runStopChan != nil { - return fmt.Errorf("motor '%s' already running or moving", d.name) - } - - d.runStopChan = make(chan struct{}) - d.runStopWaitGroup = &sync.WaitGroup{} - d.runStopWaitGroup.Add(1) - - go func(name string) { - defer d.runStopWaitGroup.Done() - for { - select { - case <-d.runStopChan: - d.runStopChan = nil - return - default: - if err := d.step(); err != nil { - fmt.Printf("motor step skipped for '%s': %v\n", name, err) - } - } - } - }(d.name) - - return nil -} - -// IsMoving returns a bool stating whether motor is currently in motion -func (d *EasyDriver) IsMoving() bool { - return d.runStopChan != nil -} + stepper := NewStepperDriver(a, [4]string{}, nil, 1) + stepper.name = gobot.DefaultName("EasyDriver") + stepper.stepperDebug = easyDriverDebug + stepper.haltIfRunning = false + stepper.stepsPerRev = 360.0 / anglePerStep + d := &EasyDriver{ + StepperDriver: stepper, + stepPin: stepPin, + dirPin: dirPin, + enPin: enPin, + sleepPin: sleepPin, + anglePerStep: anglePerStep, -// Stop running the stepper -func (d *EasyDriver) Stop() error { - if !d.IsMoving() { - return fmt.Errorf("motor '%s' is not yet started", d.name) + sleeping: false, } + d.stepFunc = d.onePinStepping + d.sleepFunc = d.sleepWithSleepPin + d.beforeHalt = d.shutdown - d.runStopChan <- struct{}{} - d.runStopWaitGroup.Wait() + // 1/4 of max speed. Not too fast, not too slow + d.speedRpm = d.MaxSpeed() / 4 - return nil + return d } -// Step the stepper 1 step -func (d *EasyDriver) Step() error { - // ensure that move and step can not interfere - d.mutex.Lock() - defer d.mutex.Unlock() - - if d.IsMoving() { - return fmt.Errorf("motor '%s' already running or moving", d.name) +// SetDirection sets the direction to be moving. +func (d *EasyDriver) SetDirection(direction string) error { + direction = strings.ToLower(direction) + if direction != StepperDriverForward && direction != StepperDriverBackward { + return fmt.Errorf("Invalid direction '%s'. Value should be '%s' or '%s'", + direction, StepperDriverForward, StepperDriverBackward) } - return d.step() -} - -// SetDirection sets the direction to be moving. Valid directions are "cw" or "ccw" -func (d *EasyDriver) SetDirection(dir string) error { - // can't change direct if dirPin isn't set if d.dirPin == "" { return fmt.Errorf("dirPin is not set for '%s'", d.name) } - if dir == "ccw" { - d.dir = -1 - // high is ccw - return d.connection.(DigitalWriter).DigitalWrite(d.dirPin, 1) + writeVal := byte(0) // low is forward + if direction == StepperDriverBackward { + writeVal = 1 // high is backward } - // default to cw, even if user specified wrong value - d.dir = 1 - // low is cw - return d.connection.(DigitalWriter).DigitalWrite(d.dirPin, 0) -} - -// SetSpeed sets the speed of the motor in RPMs. 1 is the lowest and GetMaxSpeed is the highest -func (d *EasyDriver) SetSpeed(rpm uint) error { - if rpm < 1 { - d.rpm = 1 - } else if rpm > d.MaxSpeed() { - d.rpm = d.MaxSpeed() - } else { - d.rpm = rpm + if err := d.connection.(DigitalWriter).DigitalWrite(d.dirPin, writeVal); err != nil { + return err } - return nil -} - -// MaxSpeed returns the max speed of the stepper -func (d *EasyDriver) MaxSpeed() uint { - return uint(360 / d.angle) -} + // ensure that write of variable can not interfere with read in step() + d.valueMutex.Lock() + defer d.valueMutex.Unlock() + d.direction = direction -// CurrentStep returns current step number -func (d *EasyDriver) CurrentStep() int { - return d.stepNum + return nil } // Enable enables all motor output func (d *EasyDriver) Enable() error { - // can't enable if enPin isn't set. This is fine normally since it will be enabled by default if d.enPin == "" { - d.enabled = true + d.disabled = false return fmt.Errorf("enPin is not set - board '%s' is enabled by default", d.name) } @@ -261,58 +115,34 @@ func (d *EasyDriver) Enable() error { return err } - d.enabled = true + d.disabled = false return nil } // Disable disables all motor output func (d *EasyDriver) Disable() error { - // can't disable if enPin isn't set if d.enPin == "" { return fmt.Errorf("enPin is not set for '%s'", d.name) } - // stop the motor if running - err := d.tryStop() + _ = d.stopIfRunning() // drop step errors // enPin is active low - if e := d.connection.(DigitalWriter).DigitalWrite(d.enPin, 1); e != nil { - err = multierror.Append(err, e) - } else { - d.enabled = false + if err := d.connection.(DigitalWriter).DigitalWrite(d.enPin, 1); err != nil { + return err } + d.disabled = true - return err + return nil } // IsEnabled returns a bool stating whether motor is enabled func (d *EasyDriver) IsEnabled() bool { - return d.enabled -} - -// Sleep puts the driver to sleep and disables all motor output. Low power mode. -func (d *EasyDriver) Sleep() error { - // can't sleep if sleepPin isn't set - if d.sleepPin == "" { - return fmt.Errorf("sleepPin is not set for '%s'", d.name) - } - - // stop the motor if running - err := d.tryStop() - - // sleepPin is active low - if e := d.connection.(DigitalWriter).DigitalWrite(d.sleepPin, 0); e != nil { - err = multierror.Append(err, e) - } else { - d.sleeping = true - } - - return err + return !d.disabled } // Wake wakes up the driver func (d *EasyDriver) Wake() error { - // can't wake if sleepPin isn't set if d.sleepPin == "" { return fmt.Errorf("sleepPin is not set for '%s'", d.name) } @@ -335,31 +165,43 @@ func (d *EasyDriver) IsSleeping() bool { return d.sleeping } -func (d *EasyDriver) step() error { - stepsPerRev := d.MaxSpeed() +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.connection.(DigitalWriter).DigitalWrite(d.stepPin, 0); err != nil { return err } - // 1 minute / steps per revolution / revolutions per minute - // let's keep it as Microseconds so we only have to do integer math - time.Sleep(time.Duration(60*1000*1000/stepsPerRev/d.rpm) * time.Microsecond) + + time.Sleep(d.getDelayPerStep()) if err := d.connection.(DigitalWriter).DigitalWrite(d.stepPin, 1); err != nil { return err } - // increment or decrement the number of steps by 1 - d.stepNum += int(d.dir) + if d.direction == StepperDriverForward { + d.stepNum++ + } else { + d.stepNum-- + } return nil } -// tryStop stop the stepper if moving or running -func (d *EasyDriver) tryStop() error { - if !d.IsMoving() { - return nil +// sleepWithSleepPin puts the driver to sleep and disables all motor output. Low power mode. +func (d *EasyDriver) sleepWithSleepPin() error { + if d.sleepPin == "" { + return fmt.Errorf("sleepPin is not set for '%s'", d.name) } - return d.Stop() + _ = d.stopIfRunning() // drop step errors + + // sleepPin is active low + if err := d.connection.(DigitalWriter).DigitalWrite(d.sleepPin, 0); err != nil { + return err + } + d.sleeping = true + + return nil } diff --git a/drivers/gpio/easy_driver_test.go b/drivers/gpio/easy_driver_test.go index 087848d8d..5e978ed68 100644 --- a/drivers/gpio/easy_driver_test.go +++ b/drivers/gpio/easy_driver_test.go @@ -3,7 +3,6 @@ package gpio import ( "fmt" "strings" - "sync" "testing" "time" @@ -11,22 +10,21 @@ import ( "github.com/stretchr/testify/require" ) -const ( - stepAngle = 0.5 // use non int step angle to check int math - stepsPerRev = 720 -) - func initTestEasyDriverWithStubbedAdaptor() (*EasyDriver, *gpioTestAdaptor) { + const anglePerStep = 0.5 // use non int step angle to check int math + a := newGpioTestAdaptor() - d := NewEasyDriver(a, stepAngle, "1", "2", "3", "4") + d := NewEasyDriver(a, anglePerStep, "1", "2", "3", "4") return d, a } func TestNewEasyDriver(t *testing.T) { // arrange + const anglePerStep = 0.5 // use non int step angle to check int math + a := newGpioTestAdaptor() // act - d := NewEasyDriver(a, stepAngle, "1", "2", "3", "4") + d := NewEasyDriver(a, anglePerStep, "1", "2", "3", "4") // assert assert.IsType(t, &EasyDriver{}, d) assert.True(t, strings.HasPrefix(d.name, "EasyDriver")) @@ -39,30 +37,18 @@ func TestNewEasyDriver(t *testing.T) { assert.Equal(t, "2", d.dirPin) assert.Equal(t, "3", d.enPin) assert.Equal(t, "4", d.sleepPin) - assert.Equal(t, float32(stepAngle), d.angle) - assert.Equal(t, uint(180), d.rpm) - assert.Equal(t, int8(1), d.dir) + assert.Equal(t, float32(anglePerStep), d.anglePerStep) + assert.Equal(t, uint(14), d.speedRpm) + assert.Equal(t, "forward", d.direction) assert.Equal(t, 0, d.stepNum) - assert.Equal(t, true, d.enabled) + assert.Equal(t, false, d.disabled) assert.Equal(t, false, d.sleeping) - assert.Nil(t, d.runStopChan) -} - -func TestEasyDriverHalt(t *testing.T) { - // arrange - d, _ := initTestEasyDriverWithStubbedAdaptor() - require.NoError(t, d.Run()) - require.True(t, d.IsMoving()) - // act - err := d.Halt() - // assert - assert.NoError(t, err) - assert.False(t, d.IsMoving()) + assert.Nil(t, d.stopAsynchRunFunc) } -func TestEasyDriverMove(t *testing.T) { +func TestEasyDriverMoveDeg_IsMoving(t *testing.T) { tests := map[string]struct { - inputSteps int + inputDeg int simulateDisabled bool simulateAlreadyRunning bool simulateWriteErr bool @@ -72,13 +58,13 @@ func TestEasyDriverMove(t *testing.T) { wantErr string }{ "move_one": { - inputSteps: 1, + inputDeg: 1, wantWrites: 4, wantSteps: 2, wantMoving: false, }, "move_more": { - inputSteps: 20, + inputDeg: 20, wantWrites: 80, wantSteps: 40, wantMoving: false, @@ -94,9 +80,9 @@ func TestEasyDriverMove(t *testing.T) { wantErr: "already running or moving", }, "error_write": { - inputSteps: 1, + inputDeg: 1, simulateWriteErr: true, - wantWrites: 1, + wantWrites: 0, wantMoving: false, wantErr: "write error", }, @@ -105,21 +91,23 @@ func TestEasyDriverMove(t *testing.T) { t.Run(name, func(t *testing.T) { // arrange d, a := initTestEasyDriverWithStubbedAdaptor() - d.enabled = !tc.simulateDisabled - if tc.simulateAlreadyRunning { - d.runStopChan = make(chan struct{}) - defer func() { close(d.runStopChan); d.runStopChan = nil }() - } - var numCallsWrite int - a.digitalWriteFunc = func(string, byte) error { - numCallsWrite++ - if tc.simulateWriteErr { - return fmt.Errorf("write error") + defer func() { + // for cleanup dangling channels + if d.stopAsynchRunFunc != nil { + err := d.stopAsynchRunFunc(true) + assert.NoError(t, err) } - return nil + }() + // arrange: different behavior + d.disabled = tc.simulateDisabled + if tc.simulateAlreadyRunning { + d.stopAsynchRunFunc = func(bool) error { return nil } } + // arrange: writes + a.written = nil // reset writes of Start() + a.simulateWriteError = tc.simulateWriteErr // act - err := d.Move(tc.inputSteps) + err := d.MoveDeg(tc.inputDeg) // assert if tc.wantErr != "" { assert.ErrorContains(t, err, tc.wantErr) @@ -127,7 +115,7 @@ func TestEasyDriverMove(t *testing.T) { assert.NoError(t, err) } assert.Equal(t, tc.wantSteps, d.stepNum) - assert.Equal(t, tc.wantWrites, numCallsWrite) + assert.Equal(t, tc.wantWrites, len(a.written)) assert.Equal(t, tc.wantMoving, d.IsMoving()) }) } @@ -163,10 +151,10 @@ func TestEasyDriverRun_IsMoving(t *testing.T) { t.Run(name, func(t *testing.T) { // arrange d, a := initTestEasyDriverWithStubbedAdaptor() - d.enabled = !tc.simulateDisabled + d.skipStepErrors = true + d.disabled = tc.simulateDisabled if tc.simulateAlreadyRunning { - d.runStopChan = make(chan struct{}) - defer func() { close(d.runStopChan); d.runStopChan = nil }() + d.stopAsynchRunFunc = func(bool) error { return nil } } simWriteErr := tc.simulateWriteErr // to prevent data race in write function (go-called) a.digitalWriteFunc = func(string, byte) error { @@ -201,208 +189,289 @@ func TestEasyDriverStop_IsMoving(t *testing.T) { assert.False(t, d.IsMoving()) } -func TestEasyDriverStep(t *testing.T) { +func TestEasyDriverHalt_IsMoving(t *testing.T) { + // arrange + d, _ := initTestEasyDriverWithStubbedAdaptor() + require.NoError(t, d.Run()) + require.True(t, d.IsMoving()) + // act + err := d.Halt() + // assert + assert.NoError(t, err) + assert.False(t, d.IsMoving()) +} + +func TestEasyDriverSetDirection(t *testing.T) { + const anglePerStep = 0.5 // use non int step angle to check int math + tests := map[string]struct { - countCallsForth int - countCallsBack int - simulateAlreadyRunning bool - simulateWriteErr bool - wantSteps int - wantWritten []byte - wantErr string + input string + dirPin string + simulateWriteErr bool + wantVal string + wantWritten byte + wantErr string }{ - "single": { - countCallsForth: 1, - wantSteps: 1, - wantWritten: []byte{0x00, 0x01}, - }, - "many": { - countCallsForth: 4, - wantSteps: 4, - wantWritten: []byte{0x0, 0x1, 0x0, 0x1, 0x0, 0x1, 0x0, 0x1}, + "forward": { + input: "forward", + dirPin: "10", + wantWritten: 0, + wantVal: "forward", + }, + "backward": { + input: "backward", + dirPin: "11", + wantWritten: 1, + wantVal: "backward", }, - "forth_and_back": { - countCallsForth: 5, - countCallsBack: 3, - wantSteps: 2, - wantWritten: []byte{0x0, 0x1, 0x0, 0x1, 0x0, 0x1, 0x0, 0x1, 0x0, 0x1, 0x0, 0x1, 0x0, 0x1, 0x0, 0x1}, - }, - "reverse": { - countCallsBack: 3, - wantSteps: -3, - wantWritten: []byte{0x0, 0x1, 0x0, 0x1, 0x0, 0x1}, + "unknown": { + input: "unknown", + dirPin: "12", + wantWritten: 0xFF, + wantVal: "forward", + wantErr: "Invalid direction 'unknown'", }, - "error_already_running": { - countCallsForth: 1, - simulateAlreadyRunning: true, - wantErr: "already running or moving", + "error_no_pin": { + input: "forward", + dirPin: "", + wantWritten: 0xFF, + wantVal: "forward", + wantErr: "dirPin is not set", }, "error_write": { + input: "backward", + dirPin: "13", simulateWriteErr: true, - wantWritten: []byte{0x00, 0x00}, - countCallsBack: 2, + wantWritten: 0xFF, + wantVal: "forward", wantErr: "write error", }, } for name, tc := range tests { t.Run(name, func(t *testing.T) { // arrange - d, a := initTestEasyDriverWithStubbedAdaptor() - if tc.simulateAlreadyRunning { - d.runStopChan = make(chan struct{}) - defer func() { close(d.runStopChan); d.runStopChan = nil }() - } - var writtenValues []byte - a.digitalWriteFunc = func(pin string, val byte) error { - assert.Equal(t, d.stepPin, pin) - writtenValues = append(writtenValues, val) - if tc.simulateWriteErr { - return fmt.Errorf("write error") - } - return nil - } - var errs []string + a := newGpioTestAdaptor() + d := NewEasyDriver(a, anglePerStep, "1", tc.dirPin, "3", "4") + a.written = nil // reset writes of Start() + a.simulateWriteError = tc.simulateWriteErr + require.Equal(t, "forward", d.direction) // act - for i := 0; i < tc.countCallsForth; i++ { - if err := d.Step(); err != nil { - errs = append(errs, err.Error()) - } - } - d.dir = -1 - for i := 0; i < tc.countCallsBack; i++ { - if err := d.Step(); err != nil { - errs = append(errs, err.Error()) - } - } + err := d.SetDirection(tc.input) // assert if tc.wantErr != "" { - assert.Contains(t, strings.Join(errs, ","), tc.wantErr) + assert.ErrorContains(t, err, tc.wantErr) } else { - assert.Nil(t, errs) + assert.NoError(t, err) + assert.Equal(t, tc.dirPin, a.written[0].pin) + assert.Equal(t, tc.wantWritten, a.written[0].val) } - assert.Equal(t, tc.wantSteps, d.stepNum) - assert.Equal(t, tc.wantSteps, d.CurrentStep()) - assert.Equal(t, tc.wantWritten, writtenValues) + assert.Equal(t, tc.wantVal, d.direction) }) } } -func TestEasyDriverSetDirection(t *testing.T) { +func TestEasyDriverMaxSpeed(t *testing.T) { + const delayForMaxSpeed = 1428 * time.Microsecond // 1/700Hz + tests := map[string]struct { - dirPin string - input string - wantVal int8 - wantErr string + anglePerStep float32 + want uint }{ - "cw": { - input: "cw", - dirPin: "10", - wantVal: 1, + "maxspeed_for_20spr": { + anglePerStep: 360.0 / 20.0, + want: 2100, }, - "ccw": { - input: "ccw", - dirPin: "11", - wantVal: -1, + "maxspeed_for_36spr": { + anglePerStep: 360.0 / 36.0, + want: 1166, }, - "unknown": { - input: "unknown", - dirPin: "12", - wantVal: 1, + "maxspeed_for_50spr": { + anglePerStep: 360.0 / 50.0, + want: 840, }, - "error_no_pin": { - dirPin: "", - wantVal: 1, - wantErr: "dirPin is not set", + "maxspeed_for_100spr": { + anglePerStep: 360.0 / 100.0, + want: 420, + }, + "maxspeed_for_400spr": { + anglePerStep: 360.0 / 400.0, + want: 105, }, } for name, tc := range tests { t.Run(name, func(t *testing.T) { // arrange - a := newGpioTestAdaptor() - d := NewEasyDriver(a, stepAngle, "1", tc.dirPin, "3", "4") - require.Equal(t, int8(1), d.dir) + d, _ := initTestEasyDriverWithStubbedAdaptor() + d.anglePerStep = tc.anglePerStep + d.stepsPerRev = 360.0 / tc.anglePerStep // act - err := d.SetDirection(tc.input) + got := d.MaxSpeed() + d.speedRpm = got + got2 := d.getDelayPerStep() // assert - if tc.wantErr != "" { - assert.ErrorContains(t, err, tc.wantErr) - } else { - assert.NoError(t, err) - } - assert.Equal(t, tc.wantVal, d.dir) + assert.Equal(t, tc.want, got) + assert.Equal(t, delayForMaxSpeed.Microseconds()/10, got2.Microseconds()/10) }) } } func TestEasyDriverSetSpeed(t *testing.T) { const ( - angle = 10 - max = 36 // 360/angle + anglePerStep = 10 + maxRpm = 1166 ) tests := map[string]struct { - input uint - want uint + input uint + want uint + wantErr string }{ "below_minimum": { - input: 0, - want: 1, + input: 0, + want: 0, + wantErr: "RPM (0) cannot be a zero or negative value", }, "minimum": { input: 1, want: 1, }, "maximum": { - input: max, - want: max, + input: maxRpm, + want: maxRpm, }, "above_maximum": { - input: max + 1, - want: max, + input: maxRpm + 1, + want: maxRpm, + wantErr: "cannot be greater then maximal value 1166", }, } for name, tc := range tests { t.Run(name, func(t *testing.T) { // arrange - d := EasyDriver{angle: angle} + d, _ := initTestEasyDriverWithStubbedAdaptor() + d.speedRpm = 0 + d.anglePerStep = anglePerStep + d.stepsPerRev = 360.0 / anglePerStep // act err := d.SetSpeed(tc.input) // assert - assert.NoError(t, err) - assert.Equal(t, tc.want, d.rpm) + if tc.wantErr != "" { + assert.ErrorContains(t, err, tc.wantErr) + } else { + assert.NoError(t, err) + } + assert.Equal(t, tc.want, d.speedRpm) }) } } -func TestEasyDriverMaxSpeed(t *testing.T) { +func TestEasyDriver_onePinStepping(t *testing.T) { tests := map[string]struct { - angle float32 - want uint + countCallsForth int + countCallsBack int + simulateWriteErr bool + wantSteps int + wantWritten []gpioTestWritten + wantErr string }{ - "180": { - angle: 2.0, - want: 180, + "single": { + countCallsForth: 1, + wantSteps: 1, + wantWritten: []gpioTestWritten{ + {pin: "1", val: 0x00}, + {pin: "1", val: 0x01}, + }, }, - "360": { - angle: 1.0, - want: 360, + "many": { + countCallsForth: 4, + wantSteps: 4, + wantWritten: []gpioTestWritten{ + {pin: "1", val: 0x0}, + {pin: "1", val: 0x1}, + {pin: "1", val: 0x0}, + {pin: "1", val: 0x1}, + {pin: "1", val: 0x0}, + {pin: "1", val: 0x1}, + {pin: "1", val: 0x0}, + {pin: "1", val: 0x1}, + }, }, - "720": { - angle: 0.5, - want: 720, + "forth_and_back": { + countCallsForth: 5, + countCallsBack: 3, + wantSteps: 2, + wantWritten: []gpioTestWritten{ + {pin: "1", val: 0x0}, + {pin: "1", val: 0x1}, + {pin: "1", val: 0x0}, + {pin: "1", val: 0x1}, + {pin: "1", val: 0x0}, + {pin: "1", val: 0x1}, + {pin: "1", val: 0x0}, + {pin: "1", val: 0x1}, + {pin: "1", val: 0x0}, + {pin: "1", val: 0x1}, + {pin: "1", val: 0x0}, + {pin: "1", val: 0x1}, + {pin: "1", val: 0x0}, + {pin: "1", val: 0x1}, + {pin: "1", val: 0x0}, + {pin: "1", val: 0x1}, + }, + }, + "reverse": { + countCallsBack: 3, + wantSteps: -3, + wantWritten: []gpioTestWritten{ + {pin: "1", val: 0x0}, + {pin: "1", val: 0x1}, + {pin: "1", val: 0x0}, + {pin: "1", val: 0x1}, + {pin: "1", val: 0x0}, + {pin: "1", val: 0x1}, + }, + }, + "error_write": { + simulateWriteErr: true, + countCallsBack: 2, + wantErr: "write error", }, } for name, tc := range tests { t.Run(name, func(t *testing.T) { // arrange - d := EasyDriver{angle: tc.angle} - // act & assert - assert.Equal(t, tc.want, d.MaxSpeed()) + d, a := initTestEasyDriverWithStubbedAdaptor() + a.written = nil // reset writes of Start() + a.simulateWriteError = tc.simulateWriteErr + var errs []string + // act + for i := 0; i < tc.countCallsForth; i++ { + if err := d.onePinStepping(); err != nil { + errs = append(errs, err.Error()) + } + } + d.direction = "backward" + for i := 0; i < tc.countCallsBack; i++ { + if err := d.onePinStepping(); err != nil { + errs = append(errs, err.Error()) + } + } + // assert + if tc.wantErr != "" { + assert.Contains(t, strings.Join(errs, ","), tc.wantErr) + } else { + assert.Nil(t, errs) + } + assert.Equal(t, tc.wantSteps, d.stepNum) + assert.Equal(t, tc.wantSteps, d.CurrentStep()) + assert.Equal(t, tc.wantWritten, a.written) }) } } func TestEasyDriverEnable_IsEnabled(t *testing.T) { + const anglePerStep = 0.5 // use non int step angle to check int math + tests := map[string]struct { enPin string simulateWriteErr bool @@ -429,7 +498,7 @@ func TestEasyDriverEnable_IsEnabled(t *testing.T) { "error_write": { enPin: "12", simulateWriteErr: true, - wantWrites: 1, + wantWrites: 0, wantEnabled: false, wantErr: "write error", }, @@ -438,42 +507,34 @@ func TestEasyDriverEnable_IsEnabled(t *testing.T) { t.Run(name, func(t *testing.T) { // arrange a := newGpioTestAdaptor() - d := NewEasyDriver(a, stepAngle, "1", "2", tc.enPin, "4") - var numCallsWrite int - var writtenPin string - writtenValue := byte(0xFF) - a.digitalWriteFunc = func(pin string, val byte) error { - numCallsWrite++ - writtenPin = pin - writtenValue = val - if tc.simulateWriteErr { - return fmt.Errorf("write error") - } - return nil - } - d.enabled = false + d := NewEasyDriver(a, anglePerStep, "1", "2", tc.enPin, "4") + a.written = nil // reset writes of Start() + a.simulateWriteError = tc.simulateWriteErr + d.disabled = true require.False(t, d.IsEnabled()) // act err := d.Enable() // assert + assert.Equal(t, tc.wantWrites, len(a.written)) if tc.wantErr != "" { assert.ErrorContains(t, err, tc.wantErr) } else { assert.NoError(t, err) - assert.Equal(t, byte(0), writtenValue) // enable pin is active low + assert.Equal(t, tc.enPin, a.written[0].pin) + assert.Equal(t, byte(0), a.written[0].val) // enable pin is active low } assert.Equal(t, tc.wantEnabled, d.IsEnabled()) - assert.Equal(t, tc.wantWrites, numCallsWrite) - assert.Equal(t, tc.enPin, writtenPin) }) } } func TestEasyDriverDisable_IsEnabled(t *testing.T) { + const anglePerStep = 0.5 // use non int step angle to check int math + tests := map[string]struct { enPin string runBefore bool - simulateWriteErr string + simulateWriteErr bool wantWrites int wantEnabled bool wantErr string @@ -497,7 +558,7 @@ func TestEasyDriverDisable_IsEnabled(t *testing.T) { }, "error_write": { enPin: "12", - simulateWriteErr: "write error", + simulateWriteErr: true, wantWrites: 1, wantEnabled: true, wantErr: "write error", @@ -507,14 +568,11 @@ func TestEasyDriverDisable_IsEnabled(t *testing.T) { t.Run(name, func(t *testing.T) { // arrange a := newGpioTestAdaptor() - d := NewEasyDriver(a, stepAngle, "1", "2", tc.enPin, "4") - writeMutex := sync.Mutex{} + d := NewEasyDriver(a, anglePerStep, "1", "2", tc.enPin, "4") var numCallsWrite int var writtenPin string writtenValue := byte(0xFF) a.digitalWriteFunc = func(pin string, val byte) error { - writeMutex.Lock() - defer writeMutex.Unlock() if pin == d.stepPin { // we do not consider call of step() return nil @@ -522,8 +580,8 @@ func TestEasyDriverDisable_IsEnabled(t *testing.T) { numCallsWrite++ writtenPin = pin writtenValue = val - if tc.simulateWriteErr != "" { - return fmt.Errorf(tc.simulateWriteErr) + if tc.simulateWriteErr { + return fmt.Errorf("write error") } return nil } @@ -532,7 +590,7 @@ func TestEasyDriverDisable_IsEnabled(t *testing.T) { require.True(t, d.IsMoving()) time.Sleep(time.Millisecond) } - d.enabled = true + d.disabled = false require.True(t, d.IsEnabled()) // act err := d.Disable() @@ -552,37 +610,68 @@ func TestEasyDriverDisable_IsEnabled(t *testing.T) { } func TestEasyDriverSleep_IsSleeping(t *testing.T) { + const anglePerStep = 0.5 // use non int step angle to check int math + tests := map[string]struct { - sleepPin string - runBefore bool - wantSleep bool - wantErr string + sleepPin string + runBefore bool + simulateWriteErr bool + wantWrites int + wantSleep bool + wantErr string }{ "basic": { - sleepPin: "10", - wantSleep: true, + sleepPin: "10", + wantWrites: 1, + wantSleep: true, }, "with_run": { - sleepPin: "11", - runBefore: true, - wantSleep: true, + sleepPin: "11", + runBefore: true, + wantWrites: 1, + wantSleep: true, }, "error_no_pin": { - sleepPin: "", - wantSleep: false, - wantErr: "sleepPin is not set", + sleepPin: "", + wantSleep: false, + wantWrites: 0, + wantErr: "sleepPin is not set", + }, + "error_write": { + sleepPin: "12", + simulateWriteErr: true, + wantWrites: 1, + wantSleep: false, + wantErr: "write error", }, } for name, tc := range tests { t.Run(name, func(t *testing.T) { // arrange a := newGpioTestAdaptor() - d := NewEasyDriver(a, stepAngle, "1", "2", "3", tc.sleepPin) + d := NewEasyDriver(a, anglePerStep, "1", "2", "3", tc.sleepPin) + d.sleeping = false + require.False(t, d.IsSleeping()) + // arrange: writes + var numCallsWrite int + var writtenPin string + writtenValue := byte(0xFF) + a.digitalWriteFunc = func(pin string, val byte) error { + if pin == d.stepPin { + // we do not consider call of step() + return nil + } + numCallsWrite++ + writtenPin = pin + writtenValue = val + if tc.simulateWriteErr { + return fmt.Errorf("write error") + } + return nil + } if tc.runBefore { require.NoError(t, d.Run()) } - d.sleeping = false - require.False(t, d.IsSleeping()) // act err := d.Sleep() // assert @@ -590,35 +679,68 @@ func TestEasyDriverSleep_IsSleeping(t *testing.T) { assert.ErrorContains(t, err, tc.wantErr) } else { assert.NoError(t, err) + assert.Equal(t, byte(0), writtenValue) // sleep pin is active low } assert.Equal(t, tc.wantSleep, d.IsSleeping()) + assert.Equal(t, tc.wantWrites, numCallsWrite) + assert.Equal(t, tc.sleepPin, writtenPin) }) } } func TestEasyDriverWake_IsSleeping(t *testing.T) { + const anglePerStep = 0.5 // use non int step angle to check int math + tests := map[string]struct { - sleepPin string - wantSleep bool - wantErr string + sleepPin string + simulateWriteErr bool + wantWrites int + wantSleep bool + wantErr string }{ "basic": { - sleepPin: "10", - wantSleep: false, + sleepPin: "10", + wantWrites: 1, + wantSleep: false, }, "error_no_pin": { - sleepPin: "", - wantSleep: true, - wantErr: "sleepPin is not set", + sleepPin: "", + wantWrites: 0, + wantSleep: true, + wantErr: "sleepPin is not set", + }, + "error_write": { + sleepPin: "12", + simulateWriteErr: true, + wantWrites: 1, + wantSleep: true, + wantErr: "write error", }, } for name, tc := range tests { t.Run(name, func(t *testing.T) { // arrange a := newGpioTestAdaptor() - d := NewEasyDriver(a, stepAngle, "1", "2", "3", tc.sleepPin) + d := NewEasyDriver(a, anglePerStep, "1", "2", "3", tc.sleepPin) d.sleeping = true require.True(t, d.IsSleeping()) + // arrange: writes + var numCallsWrite int + var writtenPin string + writtenValue := byte(0xFF) + a.digitalWriteFunc = func(pin string, val byte) error { + if pin == d.stepPin { + // we do not consider call of step() + return nil + } + numCallsWrite++ + writtenPin = pin + writtenValue = val + if tc.simulateWriteErr { + return fmt.Errorf("write error") + } + return nil + } // act err := d.Wake() // assert @@ -626,8 +748,11 @@ func TestEasyDriverWake_IsSleeping(t *testing.T) { assert.ErrorContains(t, err, tc.wantErr) } else { assert.NoError(t, err) + assert.Equal(t, byte(1), writtenValue) // sleep pin is active low } assert.Equal(t, tc.wantSleep, d.IsSleeping()) + assert.Equal(t, tc.wantWrites, numCallsWrite) + assert.Equal(t, tc.sleepPin, writtenPin) }) } } diff --git a/drivers/gpio/helpers_test.go b/drivers/gpio/helpers_test.go index 41c051159..a12fa3983 100644 --- a/drivers/gpio/helpers_test.go +++ b/drivers/gpio/helpers_test.go @@ -18,15 +18,22 @@ type digitalPinMock struct { writeFunc func(val int) (err error) } +type gpioTestWritten struct { + pin string + val byte +} + type gpioTestAdaptor struct { - name string - pinMap map[string]gobot.DigitalPinner - port string - mtx sync.Mutex - digitalReadFunc func(ping string) (val int, err error) - digitalWriteFunc func(pin string, val byte) (err error) - pwmWriteFunc func(pin string, val byte) (err error) - servoWriteFunc func(pin string, val byte) (err error) + name string + pinMap map[string]gobot.DigitalPinner + port string + written []gpioTestWritten + simulateWriteError bool + mtx sync.Mutex + digitalReadFunc func(ping string) (val int, err error) + digitalWriteFunc func(pin string, val byte) (err error) + pwmWriteFunc func(pin string, val byte) (err error) + servoWriteFunc func(pin string, val byte) (err error) } func newGpioTestAdaptor() *gpioTestAdaptor { @@ -62,6 +69,11 @@ func (t *gpioTestAdaptor) DigitalRead(pin string) (val int, err error) { func (t *gpioTestAdaptor) DigitalWrite(pin string, val byte) (err error) { t.mtx.Lock() defer t.mtx.Unlock() + if t.simulateWriteError { + return fmt.Errorf("write error") + } + w := gpioTestWritten{pin: pin, val: val} + t.written = append(t.written, w) return t.digitalWriteFunc(pin, val) } diff --git a/drivers/gpio/stepper_driver.go b/drivers/gpio/stepper_driver.go index 1a9469a33..626302f8e 100644 --- a/drivers/gpio/stepper_driver.go +++ b/drivers/gpio/stepper_driver.go @@ -1,8 +1,11 @@ package gpio import ( - "errors" + "fmt" + "log" "math" + "os" + "os/signal" "strconv" "strings" "sync" @@ -11,30 +14,39 @@ import ( "gobot.io/x/gobot/v2" ) +const ( + stepperDriverDebug = false + + // StepperDriverForward is to set the stepper to run in forward direction (e.g. turn clock wise) + StepperDriverForward = "forward" + // StepperDriverBackward is to set the stepper to run in backward direction (e.g. turn counter clock wise) + StepperDriverBackward = "backward" +) + type phase [][4]byte // StepperModes to decide on Phase and Stepping var StepperModes = struct { - SinglePhaseStepping [][4]byte - DualPhaseStepping [][4]byte - HalfStepping [][4]byte + SinglePhaseStepping phase + DualPhaseStepping phase + HalfStepping phase }{ // 1 cycle = 4 steps with lesser torque - SinglePhaseStepping: [][4]byte{ + SinglePhaseStepping: phase{ {1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}, }, // 1 cycle = 4 steps with higher torque and current - DualPhaseStepping: [][4]byte{ + DualPhaseStepping: phase{ {1, 0, 0, 1}, {1, 1, 0, 0}, {0, 1, 1, 0}, {0, 0, 1, 1}, }, // 1 cycle = 8 steps with lesser torque than full stepping - HalfStepping: [][4]byte{ + HalfStepping: phase{ {1, 0, 0, 1}, {1, 0, 0, 0}, {1, 1, 0, 0}, @@ -46,19 +58,26 @@ var StepperModes = struct { }, } -// StepperDriver object +// StepperDriver is a common driver for stepper motors. It supports 3 different stepping modes. type StepperDriver struct { - name string + *Driver + pins [4]string - connection DigitalWriter phase phase - stepsPerRev uint - moving bool - direction string - stepNum int - speed uint - mutex *sync.Mutex - gobot.Commander + stepsPerRev float32 + + stepperDebug bool + speedRpm uint + direction string + skipStepErrors bool + haltIfRunning bool // stop automatically if run is called + disabled bool + valueMutex *sync.Mutex // to ensure that read and write of values do not interfere + + stepFunc func() error + sleepFunc func() error + stepNum int + stopAsynchRunFunc func(bool) error } // NewStepperDriver returns a new StepperDriver given a @@ -67,193 +86,401 @@ type StepperDriver struct { // Phase - Defined by StepperModes {SinglePhaseStepping, DualPhaseStepping, HalfStepping} // Steps - No of steps per revolution of Stepper motor func NewStepperDriver(a DigitalWriter, pins [4]string, phase phase, stepsPerRev uint) *StepperDriver { - s := &StepperDriver{ - name: gobot.DefaultName("Stepper"), - connection: a, - pins: pins, - phase: phase, - stepsPerRev: stepsPerRev, - moving: false, - direction: "forward", - stepNum: 0, - speed: 1, - mutex: &sync.Mutex{}, - Commander: gobot.NewCommander(), + if stepsPerRev <= 0 { + panic("steps per revolution needs to be greater than zero") } - s.speed = s.GetMaxSpeed() - - s.AddCommand("Move", func(params map[string]interface{}) interface{} { + d := &StepperDriver{ + Driver: NewDriver(a.(gobot.Connection), "Stepper"), + pins: pins, + phase: phase, + stepsPerRev: float32(stepsPerRev), + stepperDebug: stepperDriverDebug, + skipStepErrors: false, + haltIfRunning: true, + direction: StepperDriverForward, + stepNum: 0, + speedRpm: 1, + valueMutex: &sync.Mutex{}, + } + d.speedRpm = d.MaxSpeed() + d.stepFunc = d.phasedStepping + d.sleepFunc = d.sleepOuputs + d.beforeHalt = d.shutdown + + d.AddCommand("MoveDeg", func(params map[string]interface{}) interface{} { + degs, _ := strconv.Atoi(params["degs"].(string)) + return d.MoveDeg(degs) + }) + d.AddCommand("Move", func(params map[string]interface{}) interface{} { steps, _ := strconv.Atoi(params["steps"].(string)) - return s.Move(steps) + return d.Move(steps) + }) + d.AddCommand("Step", func(params map[string]interface{}) interface{} { + return d.Move(1) + }) + d.AddCommand("Run", func(params map[string]interface{}) interface{} { + return d.Run() }) - s.AddCommand("Run", func(params map[string]interface{}) interface{} { - return s.Run() + d.AddCommand("Sleep", func(params map[string]interface{}) interface{} { + return d.Sleep() }) - s.AddCommand("Halt", func(params map[string]interface{}) interface{} { - return s.Halt() + d.AddCommand("Stop", func(params map[string]interface{}) interface{} { + return d.Stop() + }) + d.AddCommand("Halt", func(params map[string]interface{}) interface{} { + return d.Halt() }) - return s + return d } -// Name of StepperDriver -func (s *StepperDriver) Name() string { return s.name } +// Move moves the motor for given number of steps. +func (d *StepperDriver) Move(stepsToMove int) error { + d.mutex.Lock() + defer d.mutex.Unlock() + + if err := d.stepAsynch(float64(stepsToMove)); err != nil { + // something went wrong with preparation + return err + } -// SetName sets name for StepperDriver -func (s *StepperDriver) SetName(n string) { s.name = n } + err := d.stopAsynchRunFunc(false) // wait to finish with err or nil + d.stopAsynchRunFunc = nil -// Connection returns StepperDriver's connection -func (s *StepperDriver) Connection() gobot.Connection { return s.connection.(gobot.Connection) } + return err +} -// Start implements the Driver interface and keeps running the stepper till halt is called -func (s *StepperDriver) Start() error { return nil } +// MoveDeg moves the motor given number of degrees at current speed. Negative values cause to move in backward direction. +func (d *StepperDriver) MoveDeg(degs int) error { + d.mutex.Lock() + defer d.mutex.Unlock() -// Run continuously runs the stepper -func (s *StepperDriver) Run() error { - // halt if already moving - if s.moving { - if err := s.Halt(); err != nil { - return err - } + stepsToMove := float64(degs) * float64(d.stepsPerRev) / 360 + + if err := d.stepAsynch(stepsToMove); err != nil { + // something went wrong with preparation + return err } - s.mutex.Lock() - s.moving = true - s.mutex.Unlock() + err := d.stopAsynchRunFunc(false) // wait to finish with err or nil + d.stopAsynchRunFunc = nil - delay := s.getDelayPerStep() + return err +} - go func() { - for { - if !s.moving { - break - } - if err := s.step(); err != nil { - panic(err) - } - time.Sleep(delay) - } - }() +// Run runs the stepper continuously. Stop needs to be done with call Stop(). +func (d *StepperDriver) Run() error { + d.mutex.Lock() + defer d.mutex.Unlock() - return nil + return d.stepAsynch(float64(math.MaxInt) + 1) } -// Halt implements the Driver interface and halts the motion of the Stepper -func (s *StepperDriver) Halt() error { - s.mutex.Lock() - s.moving = false - s.mutex.Unlock() - return nil +// IsMoving returns a bool stating whether motor is currently in motion +func (d *StepperDriver) IsMoving() bool { + return d.stopAsynchRunFunc != nil +} + +// Stop running the stepper +func (d *StepperDriver) Stop() error { + if d.stopAsynchRunFunc == nil { + return fmt.Errorf("'%s' is not yet started", d.name) + } + + err := d.stopAsynchRunFunc(true) + d.stopAsynchRunFunc = nil + + return err } -// SetDirection sets the direction in which motor should be moving, Default is forward -func (s *StepperDriver) SetDirection(direction string) error { +// Sleep release all pins to the same output level, so no current is consumed anymore. +func (d *StepperDriver) Sleep() error { + return d.sleepFunc() +} + +// SetDirection sets the direction in which motor should be moving, default is forward. +// Changing the direction affects the next step, also for asynchronous running. +func (d *StepperDriver) SetDirection(direction string) error { direction = strings.ToLower(direction) - if direction != "forward" && direction != "backward" { - return errors.New("Invalid direction. Value should be forward or backward") + if direction != StepperDriverForward && direction != StepperDriverBackward { + return fmt.Errorf("Invalid direction '%s'. Value should be '%s' or '%s'", + direction, StepperDriverForward, StepperDriverBackward) } - s.mutex.Lock() - s.direction = direction - s.mutex.Unlock() + // ensure that write of variable can not interfere with read in step() + d.valueMutex.Lock() + defer d.valueMutex.Unlock() + d.direction = direction + return nil } -// IsMoving returns a bool stating whether motor is currently in motion -func (s *StepperDriver) IsMoving() bool { - return s.moving +// MaxSpeed gives the max RPM of motor +// max. speed is limited by: +// * motor friction, inertia and inductance, load inertia +// * full step rate is normally below 1000 per second (1kHz), typically not more than ~400 per second +// * mostly not more than 1000-2000rpm (20-40 revolutions per second) are possible +// * higher values can be achieved only by ramp-up the velocity +// * duration of GPIO write (PI1 can reach up to 70kHz, typically 20kHz, so this is most likely not the limiting factor) +// * the hardware driver, to force the high current transitions for the max. speed +// * there are CNC steppers with 1000..20.000 steps per revolution, which works with faster step rates (e.g. 200kHz) +func (d *StepperDriver) MaxSpeed() uint { + const maxStepsPerSecond = 700 // a typical value for a normal, lightly loaded motor + return uint(float32(60*maxStepsPerSecond) / d.stepsPerRev) } -// Step moves motor one step in giving direction -func (s *StepperDriver) step() error { - if s.direction == "forward" { - s.stepNum++ - } else { - s.stepNum-- +// SetSpeed sets the rpm for the next move or run. A valid value is between 1 and MaxSpeed(). +// The run needs to be stopped and called again after set this value. +func (d *StepperDriver) SetSpeed(rpm uint) error { + var err error + if rpm <= 0 { + rpm = 0 + err = fmt.Errorf("RPM (%d) cannot be a zero or negative value", rpm) } - if s.stepNum >= int(s.stepsPerRev) { - s.stepNum = 0 - } else if s.stepNum < 0 { - s.stepNum = int(s.stepsPerRev) - 1 + maxRpm := d.MaxSpeed() + if rpm > maxRpm { + rpm = maxRpm + err = fmt.Errorf("RPM (%d) cannot be greater then maximal value %d", rpm, maxRpm) } - r := int(math.Abs(float64(s.stepNum))) % len(s.phase) + d.valueMutex.Lock() + defer d.valueMutex.Unlock() + d.speedRpm = rpm - for i, v := range s.phase[r] { - if err := s.connection.DigitalWrite(s.pins[i], v); err != nil { - return err - } - } + return err +} - return nil +// CurrentStep gives the current step of motor +func (d *StepperDriver) CurrentStep() int { + // ensure that read can not interfere with write in step() + d.valueMutex.Lock() + defer d.valueMutex.Unlock() + + return d.stepNum } -// Move moves the motor for given number of steps -func (s *StepperDriver) Move(stepsToMove int) error { - if stepsToMove == 0 { - return s.Halt() +// SetHaltIfRunning with the given value. Normally a call of Run() returns an error if already running. If set this +// to true, the next call of Run() cause a automatic stop before. +func (d *StepperDriver) SetHaltIfRunning(val bool) { + d.haltIfRunning = val +} + +// shutdown the driver +func (d *StepperDriver) shutdown() error { + // stops the continuous motion of the stepper, if running + return d.stopIfRunning() +} + +func (d *StepperDriver) stepAsynch(stepsToMove float64) error { + if d.disabled { + return fmt.Errorf("'%s' is disabled and can not be running or moving", d.name) } - if s.moving { - // stop previous motion - if err := s.Halt(); err != nil { + // if running, return error or stop automatically + if d.stopAsynchRunFunc != nil { + if !d.haltIfRunning { + return fmt.Errorf("'%s' already running or moving", d.name) + } + d.debug("stop former run forcefully") + if err := d.stopAsynchRunFunc(true); err != nil { + d.stopAsynchRunFunc = nil return err } } - s.mutex.Lock() - s.moving = true - s.direction = "forward" + // prepare stepping behavior + stepsLeft := uint64(math.Abs(stepsToMove)) + if stepsLeft == 0 { + return fmt.Errorf("no steps to do for '%s'", d.name) + } + + // t [min] = steps [st] / (steps_per_revolution [st/u] * speed [u/min]) or + // t [min] = steps [st] * delay_per_step [min/st], use safety factor 2 and a small offset of 100 ms + // prepare this timeout outside of stop function to prevent data race with stepsLeft + stopTimeout := time.Duration(2*stepsLeft)*d.getDelayPerStep() + 100*time.Millisecond + endlessMovement := false - if stepsToMove < 0 { - s.direction = "backward" + if stepsLeft > math.MaxInt { + stopTimeout = 100 * time.Millisecond + endlessMovement = true + } else { + d.direction = "forward" + if stepsToMove < 0 { + d.direction = "backward" + } } - s.mutex.Unlock() - stepsLeft := int64(math.Abs(float64(stepsToMove))) - delay := s.getDelayPerStep() + // prepare new asynchronous stepping + onceDoneChan := make(chan struct{}) + runStopChan := make(chan struct{}) + runErrChan := make(chan error) + + sigChan := make(chan os.Signal, 1) + signal.Notify(sigChan, os.Interrupt) + + d.stopAsynchRunFunc = func(forceStop bool) error { + defer func() { + d.debug("RUN: cleanup stop channel") + if runStopChan != nil { + close(runStopChan) + } + runStopChan = nil + d.debug("STOP: cleanup err channel") + if runErrChan != nil { + close(runErrChan) + } + runErrChan = nil + d.debug("STOP: cleanup done") + }() + + d.debug("STOP: wait for once done") + <-onceDoneChan // wait for the first step was called - for stepsLeft > 0 { - if err := s.step(); err != nil { + // send stop for endless movement or a forceful stop happen + if endlessMovement || forceStop { + d.debug("STOP: send stop channel") + runStopChan <- struct{}{} + } + + if !endlessMovement && forceStop { + // do not wait if an normal movement was stopped forcefully + log.Printf("'%s' was forcefully stopped\n", d.name) + return nil + } + + // wait for go routine is finished and cleanup + d.debug(fmt.Sprintf("STOP: wait %s for err channel", stopTimeout)) + select { + case err := <-runErrChan: return err + case <-time.After(stopTimeout): + return fmt.Errorf("'%s' was not finished in %s", d.name, stopTimeout) } - stepsLeft-- - time.Sleep(delay) } - s.moving = false + d.debug(fmt.Sprintf("going to start go routine - endless=%t, steps=%d", endlessMovement, stepsLeft)) + go func(name string) { + var err error + var onceDone bool + defer func() { + // some cases here: + // * stop by stop channel: error should be send as nil + // * count of steps reached: error should be send as nil + // * write error occurred + // * for Run(): caller needs to send stop channel and read the error + // * for Move(): caller waits for the error, but don't send stop channel + // + d.debug(fmt.Sprintf("RUN: write '%v' to err channel", err)) + runErrChan <- err + }() + for stepsLeft > 0 { + select { + case <-sigChan: + d.debug("RUN: OS signal received") + err = fmt.Errorf("OS signal received") + return + case <-runStopChan: + d.debug("RUN: stop channel received") + return + default: + if err == nil { + err = d.stepFunc() + if err != nil { + if d.skipStepErrors { + fmt.Printf("step skipped for '%s': %v\n", name, err) + err = nil + } else { + d.debug("RUN: write error occurred") + } + } + if !onceDone { + close(onceDoneChan) // to inform that we are ready for stop now + onceDone = true + d.debug("RUN: once done") + } + if !endlessMovement { + if err != nil { + return + } + stepsLeft-- + } + } + } + } + }(d.name) + return nil } // getDelayPerStep gives the delay per step -func (s *StepperDriver) getDelayPerStep() time.Duration { - // Do not remove *1000 and change duration to time.Millisecond. It has been done for a reason - return time.Duration(60000*1000/(s.stepsPerRev*s.speed)) * time.Microsecond +// formula: delay_per_step [min] = 1/(steps_per_revolution * speed [rpm]) +func (d *StepperDriver) getDelayPerStep() time.Duration { + // considering a max. speed of 1000 rpm and max. 1000 steps per revolution, a microsecond resolution is needed + // if the motor or application needs bigger values, switch to nanosecond is needed + return time.Duration(60*1000*1000/(d.stepsPerRev*float32(d.speedRpm))) * time.Microsecond } -// GetCurrentStep gives the current step of motor -func (s *StepperDriver) GetCurrentStep() int { - return s.stepNum -} +// phasedStepping moves the motor one step with the configured speed and direction. The speed can be adjusted by SetSpeed() +// and the direction can be changed by SetDirection() asynchronously. +func (d *StepperDriver) phasedStepping() error { + // ensure that read and write of variables (direction, stepNum) can not interfere + d.valueMutex.Lock() + defer d.valueMutex.Unlock() -// GetMaxSpeed gives the max RPM of motor -func (s *StepperDriver) GetMaxSpeed() uint { - // considering time for 1 rev as no of steps per rev * 1.5 (min time req between each step) - return uint(60000 / (float64(s.stepsPerRev) * 1.5)) -} + oldStepNum := d.stepNum -// SetSpeed sets the rpm -func (s *StepperDriver) SetSpeed(rpm uint) error { - if rpm <= 0 { - return errors.New("RPM cannot be a zero or negative value") + if d.direction == StepperDriverForward { + d.stepNum++ + } else { + d.stepNum-- + } + + if d.stepNum >= int(d.stepsPerRev) { + d.stepNum = 0 + } else if d.stepNum < 0 { + d.stepNum = int(d.stepsPerRev) - 1 } - m := s.GetMaxSpeed() - if rpm > m { - rpm = m + r := int(math.Abs(float64(d.stepNum))) % len(d.phase) + + for i, v := range d.phase[r] { + if err := d.connection.(DigitalWriter).DigitalWrite(d.pins[i], v); err != nil { + d.stepNum = oldStepNum + return err + } } - s.speed = rpm + delay := d.getDelayPerStep() + time.Sleep(delay) + return nil } + +func (d *StepperDriver) sleepOuputs() error { + for _, pin := range d.pins { + if err := d.connection.(DigitalWriter).DigitalWrite(pin, 0); err != nil { + return err + } + } + return nil +} + +// stopIfRunning stop the stepper if moving or running +func (d *StepperDriver) stopIfRunning() error { + // stops the continuous motion of the stepper, if running + if d.stopAsynchRunFunc == nil { + return nil + } + + err := d.stopAsynchRunFunc(true) + d.stopAsynchRunFunc = nil + + return err +} + +func (d *StepperDriver) debug(text string) { + if d.stepperDebug { + fmt.Println(text) + } +} diff --git a/drivers/gpio/stepper_driver_test.go b/drivers/gpio/stepper_driver_test.go index 4c3271b6b..237b05901 100644 --- a/drivers/gpio/stepper_driver_test.go +++ b/drivers/gpio/stepper_driver_test.go @@ -1,104 +1,429 @@ package gpio import ( + "fmt" + "log" "strings" "testing" "time" "github.com/stretchr/testify/assert" + "github.com/stretchr/testify/require" ) -const ( - stepsInRev = 32 -) - -func initStepperMotorDriver() *StepperDriver { - return NewStepperDriver(newGpioTestAdaptor(), [4]string{"7", "11", "13", "15"}, StepperModes.DualPhaseStepping, stepsInRev) -} +func initTestStepperDriverWithStubbedAdaptor() (*StepperDriver, *gpioTestAdaptor) { + const stepsPerRev = 32 -func TestStepperDriverRun(t *testing.T) { - d := initStepperMotorDriver() - _ = d.Run() - assert.True(t, d.IsMoving()) + a := newGpioTestAdaptor() + d := NewStepperDriver(a, [4]string{"7", "11", "13", "15"}, StepperModes.DualPhaseStepping, stepsPerRev) + return d, a } -func TestStepperDriverHalt(t *testing.T) { - d := initStepperMotorDriver() - _ = d.Run() - time.Sleep(200 * time.Millisecond) - _ = d.Halt() - assert.False(t, d.IsMoving()) -} - -func TestStepperDriverDefaultName(t *testing.T) { - d := initStepperMotorDriver() - assert.True(t, strings.HasPrefix(d.Name(), "Stepper")) -} +func TestNewStepperDriver(t *testing.T) { + // arrange + const stepsPerRev = 32 -func TestStepperDriverSetName(t *testing.T) { - name := "SomeStepperSriver" - d := initStepperMotorDriver() - d.SetName(name) - assert.Equal(t, name, d.Name()) + a := newGpioTestAdaptor() + // act + d := NewStepperDriver(a, [4]string{"7", "11", "13", "15"}, StepperModes.DualPhaseStepping, stepsPerRev) + // assert + assert.IsType(t, &StepperDriver{}, d) + assert.True(t, strings.HasPrefix(d.name, "Stepper")) + assert.Equal(t, a, d.connection) + assert.NoError(t, d.afterStart()) + assert.NoError(t, d.beforeHalt()) + assert.NotNil(t, d.Commander) + assert.NotNil(t, d.mutex) + assert.Equal(t, "forward", d.direction) + assert.Equal(t, StepperModes.DualPhaseStepping, d.phase) + assert.Equal(t, float32(stepsPerRev), d.stepsPerRev) + assert.Equal(t, 0, d.stepNum) + assert.Nil(t, d.stopAsynchRunFunc) } -func TestStepperDriverSetDirection(t *testing.T) { - dir := "backward" - d := initStepperMotorDriver() - _ = d.SetDirection(dir) - assert.Equal(t, dir, d.direction) -} +func TestStepperMove_IsMoving(t *testing.T) { + const stepsPerRev = 32 -func TestStepperDriverDefaultDirection(t *testing.T) { - d := initStepperMotorDriver() - assert.Equal(t, "forward", d.direction) + tests := map[string]struct { + inputSteps int + noAutoStopIfRunning bool + simulateAlreadyRunning bool + simulateWriteErr bool + wantWrites int + wantSteps int + wantMoving bool + wantErr string + }{ + "move_forward": { + inputSteps: 2, + wantWrites: 8, + wantSteps: 2, + wantMoving: false, + }, + "move_more_forward": { + inputSteps: 10, + wantWrites: 40, + wantSteps: 10, + wantMoving: false, + }, + "move_forward_full_revolution": { + inputSteps: stepsPerRev, + wantWrites: 128, + wantSteps: 0, // will be reset after each revision + wantMoving: false, + }, + "move_backward": { + inputSteps: -2, + wantWrites: 8, + wantSteps: stepsPerRev - 2, + wantMoving: false, + }, + "move_more_backward": { + inputSteps: -10, + wantWrites: 40, + wantSteps: stepsPerRev - 10, + wantMoving: false, + }, + "move_backward_full_revolution": { + inputSteps: -stepsPerRev, + wantWrites: 128, + wantSteps: 0, // will be reset after each revision + wantMoving: false, + }, + "already_running_autostop": { + inputSteps: 3, + simulateAlreadyRunning: true, + wantWrites: 12, + wantSteps: 3, + wantMoving: false, + }, + "error_already_running": { + noAutoStopIfRunning: true, + simulateAlreadyRunning: true, + wantMoving: true, + wantErr: "already running or moving", + }, + "error_no_steps": { + inputSteps: 0, + wantWrites: 0, + wantSteps: 0, + wantMoving: false, + wantErr: "no steps to do", + }, + "error_write": { + inputSteps: 1, + simulateWriteErr: true, + wantWrites: 0, + wantMoving: false, + wantErr: "write error", + }, + } + for name, tc := range tests { + t.Run(name, func(t *testing.T) { + // arrange + d, a := initTestStepperDriverWithStubbedAdaptor() + defer func() { + // for cleanup dangling channels + if d.stopAsynchRunFunc != nil { + err := d.stopAsynchRunFunc(true) + assert.NoError(t, err) + } + }() + // arrange: different behavior + d.haltIfRunning = !tc.noAutoStopIfRunning + if tc.simulateAlreadyRunning { + d.stopAsynchRunFunc = func(bool) error { log.Println("former run stopped"); return nil } + } + // arrange: writes + a.written = nil // reset writes of Start() + a.simulateWriteError = tc.simulateWriteErr + // act + err := d.Move(tc.inputSteps) + // assert + if tc.wantErr != "" { + assert.ErrorContains(t, err, tc.wantErr) + } else { + assert.NoError(t, err) + } + assert.Equal(t, tc.wantSteps, d.stepNum) + assert.Equal(t, tc.wantWrites, len(a.written)) + assert.Equal(t, tc.wantMoving, d.IsMoving()) + }) + } } -func TestStepperDriverInvalidDirection(t *testing.T) { - d := initStepperMotorDriver() - err := d.SetDirection("reverse") - assert.ErrorContains(t, err, "Invalid direction. Value should be forward or backward") +func TestStepperRun_IsMoving(t *testing.T) { + tests := map[string]struct { + noAutoStopIfRunning bool + simulateAlreadyRunning bool + simulateWriteErr bool + wantMoving bool + wantErr string + }{ + "run": { + wantMoving: true, + }, + "error_write": { + simulateWriteErr: true, + wantMoving: true, + }, + "error_already_running": { + noAutoStopIfRunning: true, + simulateAlreadyRunning: true, + wantMoving: true, + wantErr: "already running or moving", + }, + } + for name, tc := range tests { + t.Run(name, func(t *testing.T) { + // arrange + d, a := initTestStepperDriverWithStubbedAdaptor() + defer func() { + // for cleanup dangling channels + if d.stopAsynchRunFunc != nil { + err := d.stopAsynchRunFunc(true) + assert.NoError(t, err) + } + }() + // arrange: different behavior + writeChan := make(chan struct{}) + if tc.noAutoStopIfRunning { + // in this case no write should be called + close(writeChan) + writeChan = nil + d.haltIfRunning = false + } else { + d.haltIfRunning = true + } + if tc.simulateAlreadyRunning { + d.stopAsynchRunFunc = func(bool) error { return nil } + } + // arrange: writes + simWriteErr := tc.simulateWriteErr // to prevent data race in write function (go-called) + var firstWriteDone bool + a.digitalWriteFunc = func(string, byte) error { + if firstWriteDone { + return nil // to prevent to much output and write to channel + } + writeChan <- struct{}{} + firstWriteDone = true + if simWriteErr { + return fmt.Errorf("write error") + } + return nil + } + // act + err := d.Run() + // assert + if tc.wantErr != "" { + assert.ErrorContains(t, err, tc.wantErr) + } else { + assert.NoError(t, err) + } + assert.Equal(t, tc.wantMoving, d.IsMoving()) + if writeChan != nil { + // wait until the first write was called and a little bit longer + <-writeChan + time.Sleep(10 * time.Millisecond) + var asynchErr error + if d.stopAsynchRunFunc != nil { + asynchErr = d.stopAsynchRunFunc(false) + d.stopAsynchRunFunc = nil + } + if tc.simulateWriteErr { + assert.Error(t, asynchErr) + } else { + assert.NoError(t, asynchErr) + } + } + }) + } } -func TestStepperDriverMoveForward(t *testing.T) { - d := initStepperMotorDriver() - _ = d.Move(1) - assert.Equal(t, 1, d.GetCurrentStep()) - - _ = d.Move(10) - assert.Equal(t, 11, d.GetCurrentStep()) +func TestStepperStop_IsMoving(t *testing.T) { + tests := map[string]struct { + running bool + wantErr string + }{ + "stop_running": { + running: true, + }, + "errro_not_started": { + running: false, + wantErr: "is not yet started", + }, + } + for name, tc := range tests { + t.Run(name, func(t *testing.T) { + // arrange + d, _ := initTestStepperDriverWithStubbedAdaptor() + if tc.running { + require.NoError(t, d.Run()) + require.True(t, d.IsMoving()) + } + // act + err := d.Stop() + // assert + if tc.wantErr != "" { + assert.ErrorContains(t, err, tc.wantErr) + } else { + assert.NoError(t, err) + } + assert.False(t, d.IsMoving()) + }) + } } -func TestStepperDriverMoveBackward(t *testing.T) { - d := initStepperMotorDriver() - _ = d.Move(-1) - assert.Equal(t, stepsInRev-1, d.GetCurrentStep()) - - _ = d.Move(-10) - assert.Equal(t, stepsInRev-11, d.GetCurrentStep()) +func TestStepperHalt_IsMoving(t *testing.T) { + tests := map[string]struct { + running bool + }{ + "halt_running": { + running: true, + }, + "halt_not_started": { + running: false, + }, + } + for name, tc := range tests { + t.Run(name, func(t *testing.T) { + // arrange + d, _ := initTestStepperDriverWithStubbedAdaptor() + if tc.running { + require.NoError(t, d.Run()) + require.True(t, d.IsMoving()) + } + // act + err := d.Halt() + // assert + assert.NoError(t, err) + assert.False(t, d.IsMoving()) + }) + } } -func TestStepperDriverMoveFullRotation(t *testing.T) { - d := initStepperMotorDriver() - _ = d.Move(stepsInRev) - assert.Equal(t, 0, d.GetCurrentStep()) +func TestStepperSetDirection(t *testing.T) { + tests := map[string]struct { + input string + wantVal string + wantErr string + }{ + "direction_forward": { + input: "forward", + wantVal: "forward", + }, + "direction_backward": { + input: "backward", + wantVal: "backward", + }, + "error_invalid_direction": { + input: "reverse", + wantVal: "forward", + wantErr: "Invalid direction 'reverse'", + }, + } + for name, tc := range tests { + t.Run(name, func(t *testing.T) { + // arrange + d, _ := initTestStepperDriverWithStubbedAdaptor() + require.Equal(t, "forward", d.direction) + // act + err := d.SetDirection(tc.input) + // assert + if tc.wantErr != "" { + assert.ErrorContains(t, err, tc.wantErr) + } else { + assert.NoError(t, err) + } + assert.Equal(t, tc.wantVal, d.direction) + }) + } } -func TestStepperDriverMotorSetSpeedMoreThanMax(t *testing.T) { - d := initStepperMotorDriver() - m := d.GetMaxSpeed() +func TestStepperMaxSpeed(t *testing.T) { + const delayForMaxSpeed = 1428 * time.Microsecond // 1/700Hz - _ = d.SetSpeed(m + 1) - assert.Equal(t, d.speed, m) + tests := map[string]struct { + stepsPerRev float32 + want uint + }{ + "maxspeed_for_20spr": { + stepsPerRev: 20, + want: 2100, + }, + "maxspeed_for_50spr": { + stepsPerRev: 50, + want: 840, + }, + "maxspeed_for_100spr": { + stepsPerRev: 100, + want: 420, + }, + "maxspeed_for_400spr": { + stepsPerRev: 400, + want: 105, + }, + "maxspeed_for_1000spr": { + stepsPerRev: 1000, + want: 42, + }, + } + for name, tc := range tests { + t.Run(name, func(t *testing.T) { + // arrange + d := StepperDriver{stepsPerRev: tc.stepsPerRev} + // act + got := d.MaxSpeed() + d.speedRpm = got + got2 := d.getDelayPerStep() + // assert + assert.Equal(t, tc.want, got) + assert.Equal(t, delayForMaxSpeed, got2) + }) + } } -func TestStepperDriverMotorSetSpeedLessOrEqualMax(t *testing.T) { - d := initStepperMotorDriver() - m := d.GetMaxSpeed() - - _ = d.SetSpeed(m - 1) - assert.Equal(t, d.speed, m-1) +func TestStepperSetSpeed(t *testing.T) { + const maxRpm = 1166 - _ = d.SetSpeed(m) - assert.Equal(t, d.speed, m) + tests := map[string]struct { + input uint + want uint + wantErr string + }{ + "below_minimum": { + input: 0, + want: 0, + wantErr: "RPM (0) cannot be a zero or negative value", + }, + "minimum": { + input: 1, + want: 1, + }, + "maximum": { + input: maxRpm, + want: maxRpm, + }, + "above_maximum": { + input: maxRpm + 1, + want: maxRpm, + wantErr: "cannot be greater then maximal value 1166", + }, + } + for name, tc := range tests { + t.Run(name, func(t *testing.T) { + // arrange + d, _ := initTestStepperDriverWithStubbedAdaptor() + d.stepsPerRev = 36 + // act + err := d.SetSpeed(tc.input) + // assert + if tc.wantErr != "" { + assert.ErrorContains(t, err, tc.wantErr) + } else { + assert.NoError(t, err) + } + assert.Equal(t, tc.want, d.speedRpm) + }) + } } diff --git a/examples/raspi_stepper_move.go b/examples/raspi_stepper_move.go index 61784039c..2dd62cd60 100644 --- a/examples/raspi_stepper_move.go +++ b/examples/raspi_stepper_move.go @@ -7,7 +7,9 @@ package main import ( - "fmt" + "log" + "os" + "time" "gobot.io/x/gobot/v2" "gobot.io/x/gobot/v2/drivers/gpio" @@ -15,22 +17,59 @@ import ( ) func main() { + const ( + coilA1 = "7" + coilA2 = "13" + coilB1 = "11" + coilB2 = "15" + + degPerStep = 1.875 + countRot = 10 + ) + stepPerRevision := int(360.0 / degPerStep) + r := raspi.NewAdaptor() - stepper := gpio.NewStepperDriver(r, [4]string{"7", "11", "13", "15"}, gpio.StepperModes.DualPhaseStepping, 2048) + stepper := gpio.NewStepperDriver(r, [4]string{coilA1, coilB1, coilA2, coilB2}, gpio.StepperModes.DualPhaseStepping, + uint(stepPerRevision)) work := func() { - // set spped - stepper.SetSpeed(15) + defer func() { + ec := 0 + // set current to zero to prevent overheating + if err := stepper.Sleep(); err != nil { + ec = 1 + log.Println("work done", err) + } else { + log.Println("work done") + } + + os.Exit(ec) + }() + + gobot.After(5*time.Second, func() { + // this stops only the current movement and the next will start immediately (if any) + // this means for the example, that the first rotation stops after ~5 rotations + log.Println("asynchron stop after 5 sec.") + if err := stepper.Stop(); err != nil { + log.Println(err) + } + }) + + // one rotation per second + if err := stepper.SetSpeed(60); err != nil { + log.Println("set speed", err) + } - // Move forward one revolution - if err := stepper.Move(2048); err != nil { - fmt.Println(err) + // Move forward N revolution + if err := stepper.Move(stepPerRevision * countRot); err != nil { + log.Println("move forward", err) } - // Move backward one revolution - if err := stepper.Move(-2048); err != nil { - fmt.Println(err) + // Move backward N revolution + if err := stepper.MoveDeg(-360 * countRot); err != nil { + log.Println("move backward", err) } + return } robot := gobot.NewRobot("stepperBot",