From 81723cf969a0a5d3ade57f9122adf61e9279eed6 Mon Sep 17 00:00:00 2001 From: zombieleet Date: Sat, 12 Sep 2026 05:28:16 +0200 Subject: [PATCH] easystepper: add non blocking movement and dynamic speed Move does not return until the motor stops. A program cannot read a sensor or a remote control while a motor turns. The speed is also fixed at New. This adds an API that leaves the loop with the caller. SetRPM, SetRPMs change speed, also while a motor turns MoveAsync start a movement of n steps and return at once Start turn until Stop Update do one step if a step is due Stop, IsMoving control and read the movement Move and Off keep their behaviour, so existing programs do not change. Move had an error of one step. The first pass of the loop applied the step the motor was already in, so Move(n) moved n-1 steps and Move(1) did not move at all. Move and DualDevice.Move now do all n steps and share their step logic with MoveAsync, so both give the same coil sequence. Update must not do a burst of steps when the caller is late, because a motor loses its position if it must catch up. It does one step and starts the schedule again. The dual proportional calculation needs 64 bits. Two step counts near 65535 overflow a uint32 and the slower motor then gets too few steps. Tested on an ESP32 with a 28BYJ-48 motor and a ULN2003 board. --- easystepper/easystepper.go | 325 +++++++++++++++++++++++++---- examples/easystepper/async/main.go | 64 ++++++ smoketest.sh | 1 + 3 files changed, 347 insertions(+), 43 deletions(-) create mode 100644 examples/easystepper/async/main.go diff --git a/easystepper/easystepper.go b/easystepper/easystepper.go index b6b93f830..56711dda9 100644 --- a/easystepper/easystepper.go +++ b/easystepper/easystepper.go @@ -7,6 +7,10 @@ import ( "time" ) +var ( + ErrRPM = errors.New("rpm must be greater than zero") +) + // StepMode determines the coil sequence used to perform a single step type StepMode uint8 @@ -55,11 +59,58 @@ type Device struct { stepDelay time.Duration stepNumber uint8 stepMode StepMode + + // stepCount is the number of steps for one full revolution. + // SetRPM needs it to calculate a new stepDelay. + stepCount uint + + // remainingSteps is how many steps MoveAsync has left to do. + remainingSteps uint32 + + // direction is true to move forward and false to move backward. + direction bool + + // moving is true when the motor has movement scheduled. + moving bool + + // continuous is true to move until Stop. If it is false, movement + // stops when remainingSteps is zero. + continuous bool + + // nextStep is the time of the next step. Update uses it to return + // immediately when no step is necessary. + nextStep time.Time } // DualDevice holds information for controlling 2 motors type DualDevice struct { devices [2]*Device + + // moving is true when a non blocking dual movement is active. + moving bool + + // continuous is true to move both motors until Stop. + continuous bool + + // directions is the direction of each motor. + directions [2]bool + + // totalSteps is the number of steps requested for each motor. + // Update needs it to divide the steps of the slower motor. + totalSteps [2]uint32 + + // completedSteps is the number of steps each motor has done. + completedSteps [2]uint32 + + // primary is the motor with the most steps. It controls the + // timing of a coordinated MoveAsync. + primary uint8 + + // secondary is the other motor. + secondary uint8 + + // nextStep is the time of the next coordinated step. + nextStep time.Time } // New returns a new single easystepper driver given a DeviceConfig @@ -71,6 +122,7 @@ func New(config DeviceConfig) (*Device, error) { pins: [4]machine.Pin{config.Pin1, config.Pin2, config.Pin3, config.Pin4}, stepDelay: time.Second * 60 / time.Duration((config.StepCount * config.RPM)), stepMode: config.Mode, + stepCount: config.StepCount, }, nil } @@ -107,62 +159,248 @@ func (d *DualDevice) Configure() { d.devices[1].Configure() } -// Move rotates the motor the number of given steps -// (negative steps will rotate it the opposite direction) +// Move rotates the motor the number of given steps and waits until the +// movement is complete. Negative steps rotate it the opposite direction. func (d *Device) Move(steps int32) { + if steps == 0 { + return + } direction := steps > 0 if steps < 0 { steps = -steps } - steps += int32(d.stepNumber) - var s int32 - d.stepMotor(d.stepNumber) - for s = int32(d.stepNumber); s < steps; s++ { + for i := int32(0); i < steps; i++ { + d.step(direction) time.Sleep(d.stepDelay) - d.moveDirectionSteps(direction, s) } } -// Off turns off all motor pins +// MoveAsync schedules a number of steps and returns immediately. Negative +// steps move backward. You must call Update to make the motor move. +func (d *Device) MoveAsync(steps int32) { + if steps == 0 { + return + } + d.continuous = false + d.direction = steps > 0 + if steps < 0 { + steps = -steps + } + d.remainingSteps = uint32(steps) + d.moving = true + d.nextStep = time.Now() +} + +// Start moves the motor until Stop and returns immediately. You must call +// Update to make the motor move. +func (d *Device) Start(direction bool) { + d.direction = direction + d.continuous = true + d.moving = true + d.nextStep = time.Now() +} + +// Update does one step if a step is due. It does not block. Call it +// frequently from the main loop. +func (d *Device) Update() { + if !d.moving { + return + } + now := time.Now() + if now.Before(d.nextStep) { + return + } + d.step(d.direction) + if !d.continuous { + d.remainingSteps-- + if d.remainingSteps == 0 { + d.moving = false + return + } + } + d.nextStep = schedule(d.nextStep, now, d.stepDelay) +} + +// schedule gives the time of the next step. It adds the delay to the last +// time to prevent drift, but starts from now if the caller is very late. +func schedule(last, now time.Time, delay time.Duration) time.Time { + next := last.Add(delay) + if next.Before(now) { + return now.Add(delay) + } + return next +} + +// Stop ends the movement from MoveAsync or Start. The coils stay on, so the +// motor holds its position. Use Off to also remove power from the coils. +func (d *Device) Stop() { + d.moving = false + d.continuous = false + d.remainingSteps = 0 +} + +// IsMoving tells you if the motor has an active movement. +func (d *Device) IsMoving() bool { + return d.moving +} + +// Off turns off all motor pins. This removes power from the coils, so the +// motor does not hold its position. func (d *Device) Off() { for _, pin := range d.pins { pin.Low() } } +// SetRPM changes the speed of the motor. You can call it while the motor +// moves. The new speed applies to the steps that follow. +func (d *Device) SetRPM(rpm uint) error { + if rpm == 0 { + return ErrRPM + } + d.stepDelay = time.Second * 60 / time.Duration(d.stepCount*rpm) + return nil +} + // Move rotates the motors the number of given steps // (negative steps will rotate it the opposite direction) func (d *DualDevice) Move(stepsA, stepsB int32) { - min := uint8(1) - max := uint8(0) - var directions [2]bool - var minStep int32 + if stepsA == 0 && stepsB == 0 { + return + } + primary, secondary, directions, totals := d.plan(stepsA, stepsB) + var completed [2]uint32 + + for completed[primary] < totals[primary] { + d.devices[primary].step(directions[primary]) + completed[primary]++ + + if completed[secondary] < share(completed[primary], totals, primary, secondary) { + d.devices[secondary].step(directions[secondary]) + completed[secondary]++ + } + time.Sleep(d.devices[primary].stepDelay) + } +} - directions[0] = stepsA > 0 - directions[1] = stepsB > 0 +// plan gives the motor with the most steps, the other motor, the direction +// of each motor, and the number of steps each motor must do. +func (d *DualDevice) plan(stepsA, stepsB int32) (uint8, uint8, [2]bool, [2]uint32) { + directions := [2]bool{stepsA > 0, stepsB > 0} if stepsA < 0 { stepsA = -stepsA } if stepsB < 0 { stepsB = -stepsB } + primary, secondary := uint8(0), uint8(1) if stepsB > stepsA { - stepsA, stepsB = stepsB, stepsA - max, min = min, max - } - d.devices[0].stepMotor(d.devices[0].stepNumber) - d.devices[1].stepMotor(d.devices[1].stepNumber) - stepsA += int32(d.devices[max].stepNumber) - minStep = int32(d.devices[min].stepNumber) - for s := int32(d.devices[max].stepNumber); s < stepsA; s++ { - time.Sleep(d.devices[0].stepDelay) - d.devices[max].moveDirectionSteps(directions[max], s) - - if ((s * stepsB) / stepsA) > minStep { - minStep++ - d.devices[min].moveDirectionSteps(directions[min], minStep) - } + primary, secondary = 1, 0 + } + return primary, secondary, directions, [2]uint32{uint32(stepsA), uint32(stepsB)} +} + +// share gives the steps the slower motor must have done. It needs 64 bits +// because two step counts near 65535 overflow a uint32. +func share(done uint32, totals [2]uint32, primary, secondary uint8) uint32 { + return uint32(uint64(done) * uint64(totals[secondary]) / uint64(totals[primary])) +} + +// MoveAsync starts a movement of both motors and returns immediately. Both +// motors stop together, as with Move. Call Update to make them move. +func (d *DualDevice) MoveAsync(stepsA, stepsB int32) { + if stepsA == 0 && stepsB == 0 { + return + } + // Update drives each motor directly in this mode, so cancel any + // movement that Start gave to the two motors. + d.devices[0].Stop() + d.devices[1].Stop() + + d.primary, d.secondary, d.directions, d.totalSteps = d.plan(stepsA, stepsB) + d.completedSteps[0] = 0 + d.completedSteps[1] = 0 + d.continuous = false + d.moving = true + d.nextStep = time.Now() +} + +// SetRPM changes the speed of both motors. +func (d *DualDevice) SetRPM(rpm uint) error { + return d.SetRPMs(rpm, rpm) +} + +// SetRPMs changes the speed of each motor. Different speeds turn a robot +// that has one motor on each wheel. +func (d *DualDevice) SetRPMs(rpmA, rpmB uint) error { + if err := d.devices[0].SetRPM(rpmA); err != nil { + return err } + return d.devices[1].SetRPM(rpmB) +} + +// Update does the next step of a DualDevice movement if a step is due. It +// does not block. Call it frequently from the main loop. +func (d *DualDevice) Update() { + if !d.moving { + return + } + + // After Start each motor keeps its own speed and its own timing. + if d.continuous { + d.devices[0].Update() + d.devices[1].Update() + return + } + + now := time.Now() + if now.Before(d.nextStep) { + return + } + + primary := d.primary + secondary := d.secondary + + d.devices[primary].step(d.directions[primary]) + d.completedSteps[primary]++ + + if d.completedSteps[secondary] < share(d.completedSteps[primary], d.totalSteps, primary, secondary) { + d.devices[secondary].step(d.directions[secondary]) + d.completedSteps[secondary]++ + } + + if d.completedSteps[primary] >= d.totalSteps[primary] { + d.moving = false + return + } + + d.nextStep = schedule(d.nextStep, now, d.devices[primary].stepDelay) +} + +// Start moves both motors until Stop and returns immediately. Each motor +// keeps its own speed from SetRPMs. Call Update to make them move. +func (d *DualDevice) Start(directionA, directionB bool) { + d.continuous = true + d.moving = true + d.devices[0].Start(directionA) + d.devices[1].Start(directionB) +} + +// Stop ends all movement. The coils stay on, so the motors hold their +// position. Use Off to also remove power from the coils. +func (d *DualDevice) Stop() { + d.moving = false + d.continuous = false + d.devices[0].Stop() + d.devices[1].Stop() +} + +// IsMoving tells you if one of the motors has an active movement. +func (d *DualDevice) IsMoving() bool { + if d.continuous { + return d.devices[0].IsMoving() || d.devices[1].IsMoving() + } + return d.moving } // Off turns off all motor pins @@ -171,6 +409,21 @@ func (d *DualDevice) Off() { d.devices[1].Off() } +// step moves the motor one step. It does not wait, so Move and Update can +// both use it. Forward in 4 step mode gives 0, 1, 2, 3, 0, 1, and backward +// gives 0, 3, 2, 1, 0, 3. +func (d *Device) step(direction bool) { + // Length of the coil sequence, which is 4 or 8. This is not the + // stepCount field, which is the steps for one revolution. + seq := uint8(d.stepMode.stepCount()) + if direction { + d.stepNumber = (d.stepNumber + 1) % seq + } else { + d.stepNumber = (d.stepNumber + seq - 1) % seq + } + d.stepMotor(d.stepNumber) +} + // stepMotor changes the pins' state to the correct step func (d *Device) stepMotor(step uint8) { switch d.stepMode { @@ -260,17 +513,3 @@ func (d *Device) stepMotor8(step uint8) { } d.stepNumber = step } - -// moveDirectionSteps uses the direction to calculate the correct step and change the motor to it. -// Direction true: (4-step mode) 0, 1, 2, 3, 0, 1, 2, ... -// Direction false: (4-step mode) 0, 3, 2, 1, 0, 3, 2, ... -// Direction true: (8-step mode) 0, 1, 2, 3, 4, 5, 6, 7, 0, 1, 2, ... -// Direction false: (8-step mode) 0, 7, 6, 5, 4, 3, 2, 1, 0, 7, 6, ... -func (d *Device) moveDirectionSteps(direction bool, step int32) { - modulus := int32(d.stepMode.stepCount()) - if direction { - d.stepMotor(uint8(step % modulus)) - } else { - d.stepMotor(uint8(((-step % modulus) + modulus) % modulus)) - } -} diff --git a/examples/easystepper/async/main.go b/examples/easystepper/async/main.go new file mode 100644 index 000000000..3a8945ba1 --- /dev/null +++ b/examples/easystepper/async/main.go @@ -0,0 +1,64 @@ +package main + +import ( + "machine" + "time" + + "tinygo.org/x/drivers/easystepper" +) + +const stepsPerTurn = 2048 + +func main() { + motor, err := easystepper.New(easystepper.DeviceConfig{ + Pin1: machine.P13, Pin2: machine.P15, Pin3: machine.P14, Pin4: machine.P16, + StepCount: stepsPerTurn, RPM: 4, Mode: easystepper.ModeFour, + }) + if err != nil { + println("motor init failed:", err.Error()) + return + } + motor.Configure() + + for { + // MoveAsync returns at once. Update does the steps, so the loop is + // free for other work. + println("one turn, faster every 700ms") + motor.MoveAsync(stepsPerTurn) + + rpm := uint(4) + loops := 0 + change := time.Now().Add(700 * time.Millisecond) + + for motor.IsMoving() { + motor.Update() + + // Your own work goes here. The motor does not stop it. + loops++ + + // SetRPM works while the motor turns. Move cannot do this. + if rpm < 16 && time.Now().After(change) { + rpm += 2 + motor.SetRPM(rpm) + println("rpm:", rpm) + change = time.Now().Add(700 * time.Millisecond) + } + } + println("loop ran", loops, "times while the motor turned") + motor.Off() + time.Sleep(time.Second) + + // Start turns until Stop. Update still does the steps. + println("continuous for 3s, then stop") + motor.SetRPM(10) + motor.Start(false) + + end := time.Now().Add(3 * time.Second) + for time.Now().Before(end) { + motor.Update() + } + motor.Stop() + motor.Off() + time.Sleep(time.Second) + } +} diff --git a/smoketest.sh b/smoketest.sh index 3d20b67d8..1959e484b 100755 --- a/smoketest.sh +++ b/smoketest.sh @@ -26,6 +26,7 @@ tinygo build -size short -o ./build/test.hex -target=bluepill ./examples/ds1307/ tinygo build -size short -o ./build/test.hex -target=itsybitsy-m0 ./examples/ds3231/alarms/main.go tinygo build -size short -o ./build/test.hex -target=itsybitsy-m0 ./examples/ds3231/basic/main.go tinygo build -size short -o ./build/test.hex -target=microbit ./examples/easystepper/main.go +tinygo build -size short -o ./build/test.hex -target=microbit ./examples/easystepper/async/main.go tinygo build -size short -o ./build/test.hex -target=itsybitsy-m0 ./examples/flash/console/spi tinygo build -size short -o ./build/test.hex -target=pyportal ./examples/flash/console/qspi tinygo build -size short -o ./build/test.hex -target=microbit ./examples/gc9a01/main.go