mirror of
https://github.com/hybridgroup/gobot.git
synced 2025-04-27 13:48:56 +08:00
gpio: fix data race in StepperDriver (#1029)
This commit is contained in:
Thomas Kohler
GitHub
parent
9e311b28e4
commit
a04ce8a7f2
1
Makefile
1
Makefile
@ -58,6 +58,7 @@ fmt_check:
|
||||
fmt_fix:
|
||||
$(MAKE) version_check || true
|
||||
gofumpt -l -w .
|
||||
golangci-lint run -v --fix
|
||||
|
||||
examples: $(EXAMPLES)
|
||||
|
||||
|
||||
@ -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
|
||||
*StepperDriver
|
||||
|
||||
stepPin string
|
||||
dirPin string
|
||||
enPin string
|
||||
sleepPin string
|
||||
anglePerStep float32
|
||||
|
||||
angle float32
|
||||
rpm uint
|
||||
dir int8
|
||||
stepNum int
|
||||
enabled bool
|
||||
sleeping bool
|
||||
runStopChan chan struct{}
|
||||
runStopWaitGroup *sync.WaitGroup
|
||||
}
|
||||
|
||||
// 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")
|
||||
if anglePerStep <= 0 {
|
||||
panic("angle per step 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
|
||||
}
|
||||
|
||||
// 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
|
||||
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,
|
||||
|
||||
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()
|
||||
})
|
||||
sleeping: false,
|
||||
}
|
||||
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
|
||||
|
||||
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)
|
||||
// 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)
|
||||
}
|
||||
|
||||
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
|
||||
}
|
||||
|
||||
// Stop running the stepper
|
||||
func (d *EasyDriver) Stop() error {
|
||||
if !d.IsMoving() {
|
||||
return fmt.Errorf("motor '%s' is not yet started", d.name)
|
||||
}
|
||||
|
||||
d.runStopChan <- struct{}{}
|
||||
d.runStopWaitGroup.Wait()
|
||||
|
||||
return nil
|
||||
}
|
||||
|
||||
// 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)
|
||||
}
|
||||
|
||||
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
|
||||
}
|
||||
|
||||
// ensure that write of variable can not interfere with read in step()
|
||||
d.valueMutex.Lock()
|
||||
defer d.valueMutex.Unlock()
|
||||
d.direction = direction
|
||||
|
||||
return nil
|
||||
}
|
||||
|
||||
// MaxSpeed returns the max speed of the stepper
|
||||
func (d *EasyDriver) MaxSpeed() uint {
|
||||
return uint(360 / d.angle)
|
||||
}
|
||||
|
||||
// CurrentStep returns current step number
|
||||
func (d *EasyDriver) CurrentStep() int {
|
||||
return d.stepNum
|
||||
}
|
||||
|
||||
// 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 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)
|
||||
|
||||
return nil
|
||||
}
|
||||
|
||||
// tryStop stop the stepper if moving or running
|
||||
func (d *EasyDriver) tryStop() error {
|
||||
if !d.IsMoving() {
|
||||
return nil
|
||||
if d.direction == StepperDriverForward {
|
||||
d.stepNum++
|
||||
} else {
|
||||
d.stepNum--
|
||||
}
|
||||
|
||||
return d.Stop()
|
||||
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)
|
||||
}
|
||||
|
||||
_ = 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
|
||||
}
|
||||
|
||||
@ -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)
|
||||
assert.Nil(t, d.stopAsynchRunFunc)
|
||||
}
|
||||
|
||||
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())
|
||||
}
|
||||
|
||||
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
|
||||
defer func() {
|
||||
// for cleanup dangling channels
|
||||
if d.stopAsynchRunFunc != nil {
|
||||
err := d.stopAsynchRunFunc(true)
|
||||
assert.NoError(t, err)
|
||||
}
|
||||
}()
|
||||
// arrange: different behavior
|
||||
d.disabled = 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")
|
||||
}
|
||||
return nil
|
||||
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,45 +189,250 @@ 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 {
|
||||
input string
|
||||
dirPin string
|
||||
simulateWriteErr bool
|
||||
wantVal string
|
||||
wantWritten byte
|
||||
wantErr string
|
||||
}{
|
||||
"forward": {
|
||||
input: "forward",
|
||||
dirPin: "10",
|
||||
wantWritten: 0,
|
||||
wantVal: "forward",
|
||||
},
|
||||
"backward": {
|
||||
input: "backward",
|
||||
dirPin: "11",
|
||||
wantWritten: 1,
|
||||
wantVal: "backward",
|
||||
},
|
||||
"unknown": {
|
||||
input: "unknown",
|
||||
dirPin: "12",
|
||||
wantWritten: 0xFF,
|
||||
wantVal: "forward",
|
||||
wantErr: "Invalid direction 'unknown'",
|
||||
},
|
||||
"error_no_pin": {
|
||||
input: "forward",
|
||||
dirPin: "",
|
||||
wantWritten: 0xFF,
|
||||
wantVal: "forward",
|
||||
wantErr: "dirPin is not set",
|
||||
},
|
||||
"error_write": {
|
||||
input: "backward",
|
||||
dirPin: "13",
|
||||
simulateWriteErr: true,
|
||||
wantWritten: 0xFF,
|
||||
wantVal: "forward",
|
||||
wantErr: "write error",
|
||||
},
|
||||
}
|
||||
for name, tc := range tests {
|
||||
t.Run(name, func(t *testing.T) {
|
||||
// arrange
|
||||
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
|
||||
err := d.SetDirection(tc.input)
|
||||
// assert
|
||||
if tc.wantErr != "" {
|
||||
assert.ErrorContains(t, err, tc.wantErr)
|
||||
} else {
|
||||
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.wantVal, d.direction)
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
func TestEasyDriverMaxSpeed(t *testing.T) {
|
||||
const delayForMaxSpeed = 1428 * time.Microsecond // 1/700Hz
|
||||
|
||||
tests := map[string]struct {
|
||||
anglePerStep float32
|
||||
want uint
|
||||
}{
|
||||
"maxspeed_for_20spr": {
|
||||
anglePerStep: 360.0 / 20.0,
|
||||
want: 2100,
|
||||
},
|
||||
"maxspeed_for_36spr": {
|
||||
anglePerStep: 360.0 / 36.0,
|
||||
want: 1166,
|
||||
},
|
||||
"maxspeed_for_50spr": {
|
||||
anglePerStep: 360.0 / 50.0,
|
||||
want: 840,
|
||||
},
|
||||
"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
|
||||
d, _ := initTestEasyDriverWithStubbedAdaptor()
|
||||
d.anglePerStep = tc.anglePerStep
|
||||
d.stepsPerRev = 360.0 / tc.anglePerStep
|
||||
// act
|
||||
got := d.MaxSpeed()
|
||||
d.speedRpm = got
|
||||
got2 := d.getDelayPerStep()
|
||||
// assert
|
||||
assert.Equal(t, tc.want, got)
|
||||
assert.Equal(t, delayForMaxSpeed.Microseconds()/10, got2.Microseconds()/10)
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
func TestEasyDriverSetSpeed(t *testing.T) {
|
||||
const (
|
||||
anglePerStep = 10
|
||||
maxRpm = 1166
|
||||
)
|
||||
|
||||
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, _ := initTestEasyDriverWithStubbedAdaptor()
|
||||
d.speedRpm = 0
|
||||
d.anglePerStep = anglePerStep
|
||||
d.stepsPerRev = 360.0 / anglePerStep
|
||||
// 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)
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
func TestEasyDriver_onePinStepping(t *testing.T) {
|
||||
tests := map[string]struct {
|
||||
countCallsForth int
|
||||
countCallsBack int
|
||||
simulateAlreadyRunning bool
|
||||
simulateWriteErr bool
|
||||
wantSteps int
|
||||
wantWritten []byte
|
||||
wantWritten []gpioTestWritten
|
||||
wantErr string
|
||||
}{
|
||||
"single": {
|
||||
countCallsForth: 1,
|
||||
wantSteps: 1,
|
||||
wantWritten: []byte{0x00, 0x01},
|
||||
wantWritten: []gpioTestWritten{
|
||||
{pin: "1", val: 0x00},
|
||||
{pin: "1", val: 0x01},
|
||||
},
|
||||
},
|
||||
"many": {
|
||||
countCallsForth: 4,
|
||||
wantSteps: 4,
|
||||
wantWritten: []byte{0x0, 0x1, 0x0, 0x1, 0x0, 0x1, 0x0, 0x1},
|
||||
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},
|
||||
},
|
||||
},
|
||||
"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},
|
||||
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: []byte{0x0, 0x1, 0x0, 0x1, 0x0, 0x1},
|
||||
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_already_running": {
|
||||
countCallsForth: 1,
|
||||
simulateAlreadyRunning: true,
|
||||
wantErr: "already running or moving",
|
||||
},
|
||||
"error_write": {
|
||||
simulateWriteErr: true,
|
||||
wantWritten: []byte{0x00, 0x00},
|
||||
countCallsBack: 2,
|
||||
wantErr: "write error",
|
||||
},
|
||||
@ -248,29 +441,18 @@ func TestEasyDriverStep(t *testing.T) {
|
||||
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
|
||||
}
|
||||
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.Step(); err != nil {
|
||||
if err := d.onePinStepping(); err != nil {
|
||||
errs = append(errs, err.Error())
|
||||
}
|
||||
}
|
||||
d.dir = -1
|
||||
d.direction = "backward"
|
||||
for i := 0; i < tc.countCallsBack; i++ {
|
||||
if err := d.Step(); err != nil {
|
||||
if err := d.onePinStepping(); err != nil {
|
||||
errs = append(errs, err.Error())
|
||||
}
|
||||
}
|
||||
@ -282,127 +464,14 @@ func TestEasyDriverStep(t *testing.T) {
|
||||
}
|
||||
assert.Equal(t, tc.wantSteps, d.stepNum)
|
||||
assert.Equal(t, tc.wantSteps, d.CurrentStep())
|
||||
assert.Equal(t, tc.wantWritten, writtenValues)
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
func TestEasyDriverSetDirection(t *testing.T) {
|
||||
tests := map[string]struct {
|
||||
dirPin string
|
||||
input string
|
||||
wantVal int8
|
||||
wantErr string
|
||||
}{
|
||||
"cw": {
|
||||
input: "cw",
|
||||
dirPin: "10",
|
||||
wantVal: 1,
|
||||
},
|
||||
"ccw": {
|
||||
input: "ccw",
|
||||
dirPin: "11",
|
||||
wantVal: -1,
|
||||
},
|
||||
"unknown": {
|
||||
input: "unknown",
|
||||
dirPin: "12",
|
||||
wantVal: 1,
|
||||
},
|
||||
"error_no_pin": {
|
||||
dirPin: "",
|
||||
wantVal: 1,
|
||||
wantErr: "dirPin is not set",
|
||||
},
|
||||
}
|
||||
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)
|
||||
// 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.dir)
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
func TestEasyDriverSetSpeed(t *testing.T) {
|
||||
const (
|
||||
angle = 10
|
||||
max = 36 // 360/angle
|
||||
)
|
||||
|
||||
tests := map[string]struct {
|
||||
input uint
|
||||
want uint
|
||||
}{
|
||||
"below_minimum": {
|
||||
input: 0,
|
||||
want: 1,
|
||||
},
|
||||
"minimum": {
|
||||
input: 1,
|
||||
want: 1,
|
||||
},
|
||||
"maximum": {
|
||||
input: max,
|
||||
want: max,
|
||||
},
|
||||
"above_maximum": {
|
||||
input: max + 1,
|
||||
want: max,
|
||||
},
|
||||
}
|
||||
for name, tc := range tests {
|
||||
t.Run(name, func(t *testing.T) {
|
||||
// arrange
|
||||
d := EasyDriver{angle: angle}
|
||||
// act
|
||||
err := d.SetSpeed(tc.input)
|
||||
// assert
|
||||
assert.NoError(t, err)
|
||||
assert.Equal(t, tc.want, d.rpm)
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
func TestEasyDriverMaxSpeed(t *testing.T) {
|
||||
tests := map[string]struct {
|
||||
angle float32
|
||||
want uint
|
||||
}{
|
||||
"180": {
|
||||
angle: 2.0,
|
||||
want: 180,
|
||||
},
|
||||
"360": {
|
||||
angle: 1.0,
|
||||
want: 360,
|
||||
},
|
||||
"720": {
|
||||
angle: 0.5,
|
||||
want: 720,
|
||||
},
|
||||
}
|
||||
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())
|
||||
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
|
||||
simulateWriteErr bool
|
||||
wantWrites int
|
||||
wantSleep bool
|
||||
wantErr string
|
||||
}{
|
||||
"basic": {
|
||||
sleepPin: "10",
|
||||
wantWrites: 1,
|
||||
wantSleep: true,
|
||||
},
|
||||
"with_run": {
|
||||
sleepPin: "11",
|
||||
runBefore: true,
|
||||
wantWrites: 1,
|
||||
wantSleep: true,
|
||||
},
|
||||
"error_no_pin": {
|
||||
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
|
||||
simulateWriteErr bool
|
||||
wantWrites int
|
||||
wantSleep bool
|
||||
wantErr string
|
||||
}{
|
||||
"basic": {
|
||||
sleepPin: "10",
|
||||
wantWrites: 1,
|
||||
wantSleep: false,
|
||||
},
|
||||
"error_no_pin": {
|
||||
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)
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
@ -18,10 +18,17 @@ 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
|
||||
written []gpioTestWritten
|
||||
simulateWriteError bool
|
||||
mtx sync.Mutex
|
||||
digitalReadFunc func(ping string) (val int, err error)
|
||||
digitalWriteFunc func(pin string, val byte) (err error)
|
||||
@ -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)
|
||||
}
|
||||
|
||||
|
||||
@ -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
|
||||
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
|
||||
speed uint
|
||||
mutex *sync.Mutex
|
||||
gobot.Commander
|
||||
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,
|
||||
if stepsPerRev <= 0 {
|
||||
panic("steps per revolution needs to be greater than zero")
|
||||
}
|
||||
d := &StepperDriver{
|
||||
Driver: NewDriver(a.(gobot.Connection), "Stepper"),
|
||||
pins: pins,
|
||||
phase: phase,
|
||||
stepsPerRev: stepsPerRev,
|
||||
moving: false,
|
||||
direction: "forward",
|
||||
stepsPerRev: float32(stepsPerRev),
|
||||
stepperDebug: stepperDriverDebug,
|
||||
skipStepErrors: false,
|
||||
haltIfRunning: true,
|
||||
direction: StepperDriverForward,
|
||||
stepNum: 0,
|
||||
speed: 1,
|
||||
mutex: &sync.Mutex{},
|
||||
Commander: gobot.NewCommander(),
|
||||
speedRpm: 1,
|
||||
valueMutex: &sync.Mutex{},
|
||||
}
|
||||
s.speed = s.GetMaxSpeed()
|
||||
d.speedRpm = d.MaxSpeed()
|
||||
d.stepFunc = d.phasedStepping
|
||||
d.sleepFunc = d.sleepOuputs
|
||||
d.beforeHalt = d.shutdown
|
||||
|
||||
s.AddCommand("Move", func(params map[string]interface{}) interface{} {
|
||||
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)
|
||||
})
|
||||
s.AddCommand("Run", func(params map[string]interface{}) interface{} {
|
||||
return s.Run()
|
||||
d.AddCommand("Step", func(params map[string]interface{}) interface{} {
|
||||
return d.Move(1)
|
||||
})
|
||||
s.AddCommand("Halt", func(params map[string]interface{}) interface{} {
|
||||
return s.Halt()
|
||||
d.AddCommand("Run", func(params map[string]interface{}) interface{} {
|
||||
return d.Run()
|
||||
})
|
||||
d.AddCommand("Sleep", func(params map[string]interface{}) interface{} {
|
||||
return d.Sleep()
|
||||
})
|
||||
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()
|
||||
|
||||
// SetName sets name for StepperDriver
|
||||
func (s *StepperDriver) SetName(n string) { s.name = n }
|
||||
|
||||
// Connection returns StepperDriver's connection
|
||||
func (s *StepperDriver) Connection() gobot.Connection { return s.connection.(gobot.Connection) }
|
||||
|
||||
// Start implements the Driver interface and keeps running the stepper till halt is called
|
||||
func (s *StepperDriver) Start() error { return nil }
|
||||
|
||||
// Run continuously runs the stepper
|
||||
func (s *StepperDriver) Run() error {
|
||||
// halt if already moving
|
||||
if s.moving {
|
||||
if err := s.Halt(); err != nil {
|
||||
if err := d.stepAsynch(float64(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()
|
||||
|
||||
go func() {
|
||||
for {
|
||||
if !s.moving {
|
||||
break
|
||||
}
|
||||
if err := s.step(); err != nil {
|
||||
panic(err)
|
||||
}
|
||||
time.Sleep(delay)
|
||||
}
|
||||
}()
|
||||
|
||||
return nil
|
||||
return err
|
||||
}
|
||||
|
||||
// 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
|
||||
}
|
||||
// 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()
|
||||
|
||||
// SetDirection sets the direction in which motor should be moving, Default is forward
|
||||
func (s *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")
|
||||
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.direction = direction
|
||||
s.mutex.Unlock()
|
||||
return nil
|
||||
err := d.stopAsynchRunFunc(false) // wait to finish with err or nil
|
||||
d.stopAsynchRunFunc = nil
|
||||
|
||||
return err
|
||||
}
|
||||
|
||||
// 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 d.stepAsynch(float64(math.MaxInt) + 1)
|
||||
}
|
||||
|
||||
// IsMoving returns a bool stating whether motor is currently in motion
|
||||
func (s *StepperDriver) IsMoving() bool {
|
||||
return s.moving
|
||||
func (d *StepperDriver) IsMoving() bool {
|
||||
return d.stopAsynchRunFunc != nil
|
||||
}
|
||||
|
||||
// Step moves motor one step in giving direction
|
||||
func (s *StepperDriver) step() error {
|
||||
if s.direction == "forward" {
|
||||
s.stepNum++
|
||||
} else {
|
||||
s.stepNum--
|
||||
// Stop running the stepper
|
||||
func (d *StepperDriver) Stop() error {
|
||||
if d.stopAsynchRunFunc == nil {
|
||||
return fmt.Errorf("'%s' is not yet started", d.name)
|
||||
}
|
||||
|
||||
if s.stepNum >= int(s.stepsPerRev) {
|
||||
s.stepNum = 0
|
||||
} else if s.stepNum < 0 {
|
||||
s.stepNum = int(s.stepsPerRev) - 1
|
||||
}
|
||||
err := d.stopAsynchRunFunc(true)
|
||||
d.stopAsynchRunFunc = nil
|
||||
|
||||
r := int(math.Abs(float64(s.stepNum))) % len(s.phase)
|
||||
|
||||
for i, v := range s.phase[r] {
|
||||
if err := s.connection.DigitalWrite(s.pins[i], v); err != nil {
|
||||
return err
|
||||
}
|
||||
|
||||
// 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 != StepperDriverForward && direction != StepperDriverBackward {
|
||||
return fmt.Errorf("Invalid direction '%s'. Value should be '%s' or '%s'",
|
||||
direction, StepperDriverForward, StepperDriverBackward)
|
||||
}
|
||||
}
|
||||
|
||||
// ensure that write of variable can not interfere with read in step()
|
||||
d.valueMutex.Lock()
|
||||
defer d.valueMutex.Unlock()
|
||||
d.direction = direction
|
||||
|
||||
return nil
|
||||
}
|
||||
|
||||
// Move moves the motor for given number of steps
|
||||
func (s *StepperDriver) Move(stepsToMove int) error {
|
||||
if stepsToMove == 0 {
|
||||
return s.Halt()
|
||||
// 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)
|
||||
}
|
||||
|
||||
// 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.moving {
|
||||
// stop previous motion
|
||||
if err := s.Halt(); err != nil {
|
||||
maxRpm := d.MaxSpeed()
|
||||
if rpm > maxRpm {
|
||||
rpm = maxRpm
|
||||
err = fmt.Errorf("RPM (%d) cannot be greater then maximal value %d", rpm, maxRpm)
|
||||
}
|
||||
|
||||
d.valueMutex.Lock()
|
||||
defer d.valueMutex.Unlock()
|
||||
d.speedRpm = rpm
|
||||
|
||||
return err
|
||||
}
|
||||
|
||||
// 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
|
||||
}
|
||||
|
||||
// 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 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 stepsLeft > math.MaxInt {
|
||||
stopTimeout = 100 * time.Millisecond
|
||||
endlessMovement = true
|
||||
} else {
|
||||
d.direction = "forward"
|
||||
if stepsToMove < 0 {
|
||||
s.direction = "backward"
|
||||
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)
|
||||
|
||||
for stepsLeft > 0 {
|
||||
if err := s.step(); err != nil {
|
||||
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
|
||||
|
||||
// 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)
|
||||
}
|
||||
}
|
||||
|
||||
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--
|
||||
time.Sleep(delay)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}(d.name)
|
||||
|
||||
s.moving = false
|
||||
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--
|
||||
}
|
||||
|
||||
m := s.GetMaxSpeed()
|
||||
if rpm > m {
|
||||
rpm = m
|
||||
if d.stepNum >= int(d.stepsPerRev) {
|
||||
d.stepNum = 0
|
||||
} else if d.stepNum < 0 {
|
||||
d.stepNum = int(d.stepsPerRev) - 1
|
||||
}
|
||||
|
||||
s.speed = rpm
|
||||
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
|
||||
}
|
||||
}
|
||||
|
||||
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)
|
||||
}
|
||||
}
|
||||
|
||||
@ -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 initTestStepperDriverWithStubbedAdaptor() (*StepperDriver, *gpioTestAdaptor) {
|
||||
const stepsPerRev = 32
|
||||
|
||||
func initStepperMotorDriver() *StepperDriver {
|
||||
return NewStepperDriver(newGpioTestAdaptor(), [4]string{"7", "11", "13", "15"}, StepperModes.DualPhaseStepping, stepsInRev)
|
||||
a := newGpioTestAdaptor()
|
||||
d := NewStepperDriver(a, [4]string{"7", "11", "13", "15"}, StepperModes.DualPhaseStepping, stepsPerRev)
|
||||
return d, a
|
||||
}
|
||||
|
||||
func TestStepperDriverRun(t *testing.T) {
|
||||
d := initStepperMotorDriver()
|
||||
_ = d.Run()
|
||||
assert.True(t, d.IsMoving())
|
||||
}
|
||||
func TestNewStepperDriver(t *testing.T) {
|
||||
// arrange
|
||||
const stepsPerRev = 32
|
||||
|
||||
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 TestStepperDriverSetName(t *testing.T) {
|
||||
name := "SomeStepperSriver"
|
||||
d := initStepperMotorDriver()
|
||||
d.SetName(name)
|
||||
assert.Equal(t, name, d.Name())
|
||||
}
|
||||
|
||||
func TestStepperDriverSetDirection(t *testing.T) {
|
||||
dir := "backward"
|
||||
d := initStepperMotorDriver()
|
||||
_ = d.SetDirection(dir)
|
||||
assert.Equal(t, dir, d.direction)
|
||||
}
|
||||
|
||||
func TestStepperDriverDefaultDirection(t *testing.T) {
|
||||
d := initStepperMotorDriver()
|
||||
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 TestStepperDriverInvalidDirection(t *testing.T) {
|
||||
d := initStepperMotorDriver()
|
||||
err := d.SetDirection("reverse")
|
||||
assert.ErrorContains(t, err, "Invalid direction. Value should be forward or backward")
|
||||
func TestStepperMove_IsMoving(t *testing.T) {
|
||||
const stepsPerRev = 32
|
||||
|
||||
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 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 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 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 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 TestStepperDriverMoveFullRotation(t *testing.T) {
|
||||
d := initStepperMotorDriver()
|
||||
_ = d.Move(stepsInRev)
|
||||
assert.Equal(t, 0, 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 TestStepperDriverMotorSetSpeedMoreThanMax(t *testing.T) {
|
||||
d := initStepperMotorDriver()
|
||||
m := d.GetMaxSpeed()
|
||||
|
||||
_ = d.SetSpeed(m + 1)
|
||||
assert.Equal(t, d.speed, m)
|
||||
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 TestStepperDriverMotorSetSpeedLessOrEqualMax(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-1)
|
||||
|
||||
_ = d.SetSpeed(m)
|
||||
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 TestStepperSetSpeed(t *testing.T) {
|
||||
const maxRpm = 1166
|
||||
|
||||
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)
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
@ -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)
|
||||
|
||||
// Move forward one revolution
|
||||
if err := stepper.Move(2048); err != nil {
|
||||
fmt.Println(err)
|
||||
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")
|
||||
}
|
||||
|
||||
// Move backward one revolution
|
||||
if err := stepper.Move(-2048); err != nil {
|
||||
fmt.Println(err)
|
||||
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 N revolution
|
||||
if err := stepper.Move(stepPerRevision * countRot); err != nil {
|
||||
log.Println("move forward", err)
|
||||
}
|
||||
|
||||
// Move backward N revolution
|
||||
if err := stepper.MoveDeg(-360 * countRot); err != nil {
|
||||
log.Println("move backward", err)
|
||||
}
|
||||
return
|
||||
}
|
||||
|
||||
robot := gobot.NewRobot("stepperBot",
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user