diff --git a/docs/modules/dual_motor.md b/docs/modules/dual_motor.md index 0690f49..76d759c 100644 --- a/docs/modules/dual_motor.md +++ b/docs/modules/dual_motor.md @@ -10,7 +10,7 @@ This is the library reference for the [Dual Motor / Bipolar Stepper Module for Y - [Current Limiting](#current-limiting) - [Accessing the DC Motors](#accessing-the-dc-motors) - [More than 8 Motors](#more-than-8-motors) - - [Accessing the Stepper Motor](#accessing-the-stepper-motor) + - [Driving a Stepper Motor](#driving-a-stepper-motor) - [Onboard Sensors](#onboard-sensors) - [References](#references) - [Constants](#constants) @@ -138,9 +138,29 @@ pins = [pin for module in modules for pin in module.motor_pins] ``` -### Accessing the Stepper Motor +### Driving a Stepper Motor -Currently the Yukon library does not support stepper motors. For now it is recommended to make use of a 3rd-party Micropython library, and follow the instructions from the [More than 8 Motors](#more-than-8-motors) section above to get direct access to the GPIO pins that control the motor driver. +For driving a stepper motor, the Yukon library includes a class called `OkayStepper`. It is likely this will be replaced with a better solution in the future, hence the name. + +To use `OkayStepper`, set up the `DualMotorModule` as you would for driving two DC motors. Then, once verified and initialised, pass its two `Motor` objects to the stepper class. + +```python +stepper = OkayStepper(module.motor1, module.motor2) +``` + +From there, the stepper motor can be controlled with the following functions. + +```python +OkayStepper(motor_a: Motor, motor_b: Motor, current_scale: float=DEFAULT_CURRENT_SCALE, microsteps: int=DEFAULT_MICROSTEPS, debug_pin: Pin=None) +hold() -> None +release() -> None +move_to(step: float, travel_time: float, debug: int=True) -> None +move_by(steps: float, travel_time: float, debug: int=True) -> None +is_moving() -> bool +wait_for_move() -> None +``` + +To see how these functions may be used, look at the [single_stepper.py](../../examples/modules/dual_motor/single_stepper.py) example. ### Onboard Sensors @@ -162,8 +182,20 @@ MOTOR_2 = 1 # Only for DUAL motor_type NUM_STEPPERS = 1 FAULT_THRESHOLD = 0.1 DEFAULT_FREQUENCY = 25000 -DEFAULT_CURRENT_LIMIT = CURRENT_LIMIT_3 TEMPERATURE_THRESHOLD = 50.0 + +CURRENT_LIMIT_1 = 0.161 +CURRENT_LIMIT_2 = 0.251 +CURRENT_LIMIT_3 = 0.444 +CURRENT_LIMIT_4 = 0.786 +CURRENT_LIMIT_5 = 1.143 +CURRENT_LIMIT_6 = 1.611 +CURRENT_LIMIT_7 = 1.890 +CURRENT_LIMIT_8 = 2.153 +CURRENT_LIMIT_9 = 2.236 +MIN_CURRENT_LIMIT = CURRENT_LIMIT_1 +DEFAULT_CURRENT_LIMIT = CURRENT_LIMIT_3 +MAX_CURRENT_LIMIT = CURRENT_LIMIT_9 ``` ### Variables diff --git a/examples/modules/dual_motor/single_stepper.py b/examples/modules/dual_motor/single_stepper.py new file mode 100644 index 0000000..6d25051 --- /dev/null +++ b/examples/modules/dual_motor/single_stepper.py @@ -0,0 +1,68 @@ +from pimoroni_yukon import Yukon +from pimoroni_yukon import SLOT1 as SLOT +from pimoroni_yukon.modules import DualMotorModule +from pimoroni_yukon.extras.stepper import OkayStepper + +""" +How to drive a stepper motor from a Dual Motor Module connected to Slot1. +A sequence of movements will be played. +""" + +# Put your movements here! +# First value is the position (in steps) to go to, second is the duration (in seconds) to get there +# 200 steps will produce a complete rotation for a typical stepper with 1.8 degrees per step +MOVEMENTS = [(200, 2), + (400, 1), + (0, 3), + (0, 2), + (-100, 2), + (0, 5)] + +# Constants +CURRENT_SCALE = 0.5 # How much to scale the output current to the stepper motor by, between 0.0 and 1.0 + +# Variables +yukon = Yukon() # Create a new Yukon object +module = DualMotorModule() # Create a DualMotorModule object +stepper = None # A variable to store an OkayStepper object created later +next_movement = 0 # The index of the next movement to perform + +# Wrap the code in a try block, to catch any exceptions (including KeyboardInterrupt) +try: + yukon.register_with_slot(module, SLOT) # Register the DualMotorModule object with the slot + yukon.verify_and_initialise() # Verify that a DualMotorModule is attached to Yukon, and initialise it + yukon.enable_main_output() # Turn on power to the module slots + + # Create a class for controlling the stepper motor, in this case OkayStepper, and provide it with the DualMotorModule's two outputs + stepper = OkayStepper(module.motor1, module.motor2, current_scale=CURRENT_SCALE) + + # Set the hardware current limit to its maximum (OkayStepper controls current with PWM instead) + module.set_current_limit(DualMotorModule.MAX_CURRENT_LIMIT) + module.enable() # Enable the motor driver on the DualMotorModule + + # Loop until the BOOT/USER button is pressed + while not yukon.is_boot_pressed(): + + if not stepper.is_moving(): + # Have we reached the end of the movements? + if next_movement >= len(MOVEMENTS): + stepper.release() # Release the stepper motor + module.disable() # Disable the motor driver + yukon.set_led('A', False) # Show that the stepper motor is no longer moving + break # Exit the main while loop + + step, travel_time = MOVEMENTS[next_movement] # Extract the current movement from the list + stepper.move_to(step, travel_time) # Initiate the movement + yukon.set_led('A', True) # Show that the stepper motor is moving + next_movement += 1 # Advance the next movement index + + # Perform a single check of Yukon's internal voltage, current, and temperature sensors + # NOTE. This is currently commented out as it interfers with the precise timing needed by stepper motors + # yukon.monitor_once() + +finally: + if stepper is not None: + stepper.release() # Release the stepper motor (if not already done so) + + # Put the board back into a safe state, regardless of how the program may have ended + yukon.reset() diff --git a/lib/pimoroni_yukon/extras/stepper.py b/lib/pimoroni_yukon/extras/stepper.py new file mode 100644 index 0000000..2db8ab2 --- /dev/null +++ b/lib/pimoroni_yukon/extras/stepper.py @@ -0,0 +1,148 @@ +# SPDX-FileCopyrightText: 2023 Christopher Parrott for Pimoroni Ltd +# +# SPDX-License-Identifier: MIT + +import math +from machine import Timer, Pin + +""" +A timer-based class for driving a stepper motor. +There are likely to be many quirks and missing features of this class, that make it just "okay", hence the name. +The hope is to improve on this based on user feedback, and port it to a C++ module to improve performance. +""" + + +class OkayStepper: + DEFAULT_CURRENT_SCALE = 0.5 + HOLD_CURRENT_PERCENT = 0.2 + + STEP_PHASES = 4 + DEFAULT_MICROSTEPS = 8 + + def __init__(self, motor_a, motor_b, current_scale=DEFAULT_CURRENT_SCALE, microsteps=DEFAULT_MICROSTEPS, debug_pin=None): + self.motor_a = motor_a + self.motor_b = motor_b + self.__debug_pin = debug_pin + if self.__debug_pin is not None: + self.__debug_pin.init(Pin.OUT) + + self.__moving = False + self.current_microstep = 0 + self.end_microstep = 0 + + self.step_timer = Timer() + + current_scale = max(min(current_scale, 1.0), 0.0) + + self.__microsteps = microsteps + self.__total_microsteps = self.STEP_PHASES * microsteps + self.__step_table = self.__create_table(current_scale) + self.__hold_table = self.__create_table(current_scale * self.HOLD_CURRENT_PERCENT) + + def __create_table(self, current_scale): + table = [0] * self.__total_microsteps + for i in range(self.__total_microsteps): + angle = (i / self.__total_microsteps) * math.pi * 2.0 + table[i] = (math.cos(angle) * current_scale, + 0 - math.sin(angle) * current_scale) + return table + + def __set_duties(self, table): + stepper_entry = table[self.current_microstep % self.__total_microsteps] + self.motor_a.duty(stepper_entry[0]) + self.motor_b.duty(stepper_entry[1]) + + def hold(self): + self.__set_duties(self.__hold_table) + + def release(self): + self.step_timer.deinit() + self.motor_a.disable() + self.motor_b.disable() + + def __increase_microstep(self, timer): + if self.__debug_pin is not None: + self.__debug_pin.on() + + self.current_microstep += 1 + if self.current_microstep < self.end_microstep: + self.__set_duties(self.__step_table) + else: + timer.deinit() + self.hold() + self.__moving = False + + if self.__debug_pin is not None: + self.__debug_pin.off() + + def __decrease_microstep(self, timer): + if self.__debug_pin is not None: + self.__debug_pin.on() + + self.current_microstep -= 1 + if self.current_microstep > self.end_microstep: + self.__set_duties(self.__step_table) + else: + timer.deinit() + self.hold() + self.__moving = False + + if self.__debug_pin is not None: + self.__debug_pin.off() + + def __hold_microstep(self, timer): + if self.__debug_pin is not None: + self.__debug_pin.on() + + timer.deinit() + self.hold() + self.__moving = False + + if self.__debug_pin is not None: + self.__debug_pin.off() + + def __move_by(self, microstep_diff, travel_time, debug=True): + if microstep_diff != 0: + self.end_microstep = self.current_microstep + microstep_diff + + tick_hz = 1000000 + period_per_step = int((travel_time * tick_hz) / abs(microstep_diff)) + while (period_per_step // 10) * 10 == period_per_step and tick_hz > 1000: + period_per_step //= 10 + tick_hz //= 10 + + if debug: + print(f"> Moving from {self.current_microstep / self.__microsteps} to {self.end_microstep / self.__microsteps}, in {travel_time}s") + + self.__moving = True + self.step_timer.init(mode=Timer.PERIODIC, period=period_per_step, tick_hz=tick_hz, + callback=self.__increase_microstep if microstep_diff > 0 else self.__decrease_microstep) + else: + if debug: + print(f"> Idling at {self.current_microstep / self.__microsteps} for {travel_time}s") + + self.hold() + self.__moving = True + self.step_timer.init(mode=Timer.ONE_SHOT, + period=int(travel_time * 1000), + tick_hz=1000, + callback=self.__hold_microstep) + + def move_to(self, step, travel_time, debug=True): + self.step_timer.deinit() + + microstep_diff = int(step * self.__microsteps) - self.current_microstep + self.__move_by(microstep_diff, travel_time, debug) + + def move_by(self, steps, travel_time, debug=True): + self.step_timer.deinit() + + microstep_diff = int(steps * self.__microsteps) + self.__move_by(microstep_diff, travel_time, debug) + + def is_moving(self): + return self.__moving + + def wait_for_move(self): + while self.__moving: + pass diff --git a/lib/pimoroni_yukon/modules/dual_motor.py b/lib/pimoroni_yukon/modules/dual_motor.py index 9cdd57f..d0183be 100644 --- a/lib/pimoroni_yukon/modules/dual_motor.py +++ b/lib/pimoroni_yukon/modules/dual_motor.py @@ -8,17 +8,6 @@ from pimoroni_yukon.errors import FaultError, OverTemperatureError import pimoroni_yukon.logging as logging -# The current (in amps) associated with each limit (Do Not Modify!) -CURRENT_LIMIT_1 = 0.161 -CURRENT_LIMIT_2 = 0.251 -CURRENT_LIMIT_3 = 0.444 -CURRENT_LIMIT_4 = 0.786 -CURRENT_LIMIT_5 = 1.143 -CURRENT_LIMIT_6 = 1.611 -CURRENT_LIMIT_7 = 1.890 -CURRENT_LIMIT_8 = 2.153 -CURRENT_LIMIT_9 = 2.236 - class DualMotorModule(YukonModule): NAME = "Dual Motor" @@ -30,9 +19,22 @@ class DualMotorModule(YukonModule): NUM_STEPPERS = 1 FAULT_THRESHOLD = 0.1 DEFAULT_FREQUENCY = 25000 - DEFAULT_CURRENT_LIMIT = CURRENT_LIMIT_3 TEMPERATURE_THRESHOLD = 50.0 + # The current (in amps) associated with each limit (Do Not Modify!) + CURRENT_LIMIT_1 = 0.161 + CURRENT_LIMIT_2 = 0.251 + CURRENT_LIMIT_3 = 0.444 + CURRENT_LIMIT_4 = 0.786 + CURRENT_LIMIT_5 = 1.143 + CURRENT_LIMIT_6 = 1.611 + CURRENT_LIMIT_7 = 1.890 + CURRENT_LIMIT_8 = 2.153 + CURRENT_LIMIT_9 = 2.236 + MIN_CURRENT_LIMIT = CURRENT_LIMIT_1 + DEFAULT_CURRENT_LIMIT = CURRENT_LIMIT_3 + MAX_CURRENT_LIMIT = CURRENT_LIMIT_9 + # | ADC1 | ADC2 | SLOW1 | SLOW2 | SLOW3 | Module | Condition (if any) | # |-------|-------|-------|-------|-------|----------------------|-----------------------------| # | HIGH | ALL | 0 | 0 | 1 | Dual Motor | | @@ -52,15 +54,15 @@ def __init__(self, motor_type=DUAL, frequency=DEFAULT_FREQUENCY, current_limit=D # An ascending order list of current limits with the pin states to achieve them self.__current_limit_states = OrderedDict({ - CURRENT_LIMIT_1: (0, 0), - CURRENT_LIMIT_2: (-1, 0), - CURRENT_LIMIT_3: (0, -1), - CURRENT_LIMIT_4: (1, 0), - CURRENT_LIMIT_5: (-1, -1), - CURRENT_LIMIT_6: (0, 1), - CURRENT_LIMIT_7: (1, -1), - CURRENT_LIMIT_8: (-1, 1), - CURRENT_LIMIT_9: (1, 1), + self.CURRENT_LIMIT_1: (0, 0), + self.CURRENT_LIMIT_2: (-1, 0), + self.CURRENT_LIMIT_3: (0, -1), + self.CURRENT_LIMIT_4: (1, 0), + self.CURRENT_LIMIT_5: (-1, -1), + self.CURRENT_LIMIT_6: (0, 1), + self.CURRENT_LIMIT_7: (1, -1), + self.CURRENT_LIMIT_8: (-1, 1), + self.CURRENT_LIMIT_9: (1, 1), }) def initialise(self, slot, adc1_func, adc2_func): @@ -113,8 +115,8 @@ def set_current_limit(self, amps): raise RuntimeError("Cannot change current limit whilst motor driver is active") # Start with the lowest limit - chosen_limit = CURRENT_LIMIT_1 - chosen_state = self.__current_limit_states[CURRENT_LIMIT_1] + chosen_limit = self.MIN_CURRENT_LIMIT + chosen_state = self.__current_limit_states[chosen_limit] # Find the closest current limit below the given amps value for limit, state in self.__current_limit_states.items():