From 80d694a680701db484458f7a875f9166d223cdde Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Fri, 13 Oct 2023 20:35:56 +0100 Subject: [PATCH] Added dual motor examples, and fixed some things --- examples/modules/dual_motor/all_motors.py | 88 +++++++++++++++++++ .../modules/dual_motor/multiple_motors.py | 81 +++++++++++++++++ examples/modules/dual_motor/two_motors.py | 69 +++++++++++++++ examples/modules/quad_servo/all_servos.py | 12 +-- examples/yukon_adaptive_motor.py | 56 ------------ lib/pimoroni_yukon/modules/dual_motor.py | 39 ++++---- lib/pimoroni_yukon/modules/led_strip.py | 8 +- .../modules/quad_servo_direct.py | 19 ++-- lib/pimoroni_yukon/modules/quad_servo_reg.py | 19 ++-- 9 files changed, 297 insertions(+), 94 deletions(-) create mode 100644 examples/modules/dual_motor/all_motors.py create mode 100644 examples/modules/dual_motor/multiple_motors.py create mode 100644 examples/modules/dual_motor/two_motors.py delete mode 100644 examples/yukon_adaptive_motor.py diff --git a/examples/modules/dual_motor/all_motors.py b/examples/modules/dual_motor/all_motors.py new file mode 100644 index 0000000..3d2785c --- /dev/null +++ b/examples/modules/dual_motor/all_motors.py @@ -0,0 +1,88 @@ +import math +from pimoroni_yukon import Yukon +from pimoroni_yukon.modules import DualMotorModule +from pimoroni_yukon.timing import ticks_ms, ticks_add +from pimoroni_yukon.logging import LOG_WARN +from motor import MotorCluster + +""" +How to drive up to 12 motors from a set of Dual Motor Modules connected to Slots, using a MotorCluster. +A wave pattern will be played on the attached motors. + +The MotorCluster controls the whole set of motors using PIO. +It also staggers the updates of each motor to reduce peak current draw. +""" + +# Constants +SPEED = 0.005 # How much to advance the motor phase offset by each update +UPDATES = 50 # How many times to update the motors per second +SPEED_EXTENT = 1.0 # How far from zero to drive the motors +CURRENT_LIMIT = 0.5 # The maximum current (in amps) the motors will be driven with +CLUSTER_PIO = 0 # The PIO system to use (0 or 1) to drive the motor cluster +CLUSTER_SM = 0 # The State Machines (SM) to use to drive the motor cluster + +# Variables +yukon = Yukon(logging_level=LOG_WARN) # Create a new Yukon object, with its logging level lowered +modules = [] # A list to store QuadServo module objects created later +phase_offset = 0 # The offset used to animate the motors + + +# Function to get a motor speed from its index +def speed_from_index(index, offset=0.0): + phase = ((index / DualMotorModule.NUM_MOTORS) + offset) * math.pi * 2 + speed = math.sin(phase) * SPEED_EXTENT + return speed + + +# Wrap the code in a try block, to catch any exceptions (including KeyboardInterrupt) +try: + # Find out which slots of Yukon have DualMotorModule attached + for slot in yukon.find_slots_with(DualMotorModule): + module = DualMotorModule(init_motors=False) # Create a DualMotorModule object + yukon.register_with_slot(module, slot) # Register the DualMotorModule object with the slot + modules.append(module) # Add the object to the module list + + # Record the number of motors that will be driven + NUM_MOTORS = len(modules) * DualMotorModule.NUM_MOTORS + print(f"Up to {NUM_MOTORS} motors available") + + yukon.verify_and_initialise() # Verify that DualMotorModules are attached to Yukon, and initialise them + + # Create a MotorCluster object, with a list of motor pin pairs to control. + # The pin list is created using nested list comprehension + motors = MotorCluster(CLUSTER_PIO, CLUSTER_SM, + pins=[pin for module in modules for pin in module.motor_pins]) + + yukon.enable_main_output() # Turn on power to the module slots + + for module in modules: + module.current_limit(CURRENT_LIMIT) # Change the current limit (in amps) of the motor driver + module.enable() # Enable the motor driver on the DualMotorModule + + current_time = ticks_ms() # Record the start time of the program loop + + # Loop until the BOOT/USER button is pressed + while not yukon.is_boot_pressed(): + + # Give all the motors new speeds + for current_motor in range(motors.count()): + speed = speed_from_index(current_motor, phase_offset) + motors.speed(current_motor, speed) + + # Advance the phase offset, wrapping if it exceeds 1.0 + phase_offset += SPEED + if phase_offset >= 1.0: + phase_offset -= 1.0 + + print(f"Phase = {phase_offset}") + + # Advance the current time by a number of seconds + current_time = ticks_add(current_time, int(1000 / UPDATES)) + + # Monitor sensors until the current time is reached, recording the min, max, and average for each + # This approach accounts for the updating of the rainbows taking a non-zero amount of time to complete + yukon.monitor_until_ms(current_time) + +finally: + # Put the board back into a safe state, regardless of how the program may have ended + yukon.reset() diff --git a/examples/modules/dual_motor/multiple_motors.py b/examples/modules/dual_motor/multiple_motors.py new file mode 100644 index 0000000..7b6e952 --- /dev/null +++ b/examples/modules/dual_motor/multiple_motors.py @@ -0,0 +1,81 @@ +import math +from pimoroni_yukon import Yukon +from pimoroni_yukon.modules import DualMotorModule +from pimoroni_yukon.timing import ticks_ms, ticks_add +from pimoroni_yukon.logging import LOG_WARN + +""" +How to drive up to 8 motors from a set of Dual Motor Module connected to Slots. +A wave pattern will be played on the attached motors. + +To use more motors, look at the all_motors.py example. +""" + +# Constants +SPEED = 0.005 # How much to advance the motor phase offset by each update +UPDATES = 50 # How many times to update the motors per second +SPEED_EXTENT = 1.0 # How far from zero to drive the motors +CURRENT_LIMIT = 0.5 # The maximum current (in amps) the motors will be driven with + +# Variables +yukon = Yukon(logging_level=LOG_WARN) # Create a new Yukon object, with its logging level lowered +modules = [] # A list to store DualMotorModule objects created later +phase_offset = 0 # The offset used to animate the motors + + +# Function to get a motor speed from its index +def speed_from_index(index, offset=0.0): + phase = ((index / DualMotorModule.NUM_MOTORS) + offset) * math.pi * 2 + speed = math.sin(phase) * SPEED_EXTENT + return speed + + +# Wrap the code in a try block, to catch any exceptions (including KeyboardInterrupt) +try: + # Find out which slots of Yukon have DualMotorModule attached + for slot in yukon.find_slots_with(DualMotorModule): + module = DualMotorModule() # Create a DualMotorModule object + yukon.register_with_slot(module, slot) # Register the DualMotorModule object with the slot + modules.append(module) # Add the object to the module list + + # Record the number of motors that will be driven + NUM_MOTORS = len(modules) * DualMotorModule.NUM_MOTORS + print(f"Up to {NUM_MOTORS} motors available") + + yukon.verify_and_initialise() # Verify that DualMotorModules are attached to Yukon, and initialise them + yukon.enable_main_output() # Turn on power to the module slots + + for module in modules: + module.current_limit(CURRENT_LIMIT) # Change the current limit (in amps) of the motor driver + module.enable() # Enable the motor driver on the DualMotorModule + + current_time = ticks_ms() # Record the start time of the program loop + + # Loop until the BOOT/USER button is pressed + while not yukon.is_boot_pressed(): + + # Give all the motors new speeds + current_motor = 0 + for module in modules: + for motor in module.motors: + speed = speed_from_index(current_motor, phase_offset) + motor.speed(speed) + current_motor += 1 + + # Advance the phase offset, wrapping if it exceeds 1.0 + phase_offset += SPEED + if phase_offset >= 1.0: + phase_offset -= 1.0 + + print(f"Phase = {phase_offset}") + + # Advance the current time by a number of seconds + current_time = ticks_add(current_time, int(1000 / UPDATES)) + + # Monitor sensors until the current time is reached, recording the min, max, and average for each + # This approach accounts for the updating of the rainbows taking a non-zero amount of time to complete + yukon.monitor_until_ms(current_time) + +finally: + # Put the board back into a safe state, regardless of how the program may have ended + yukon.reset() diff --git a/examples/modules/dual_motor/two_motors.py b/examples/modules/dual_motor/two_motors.py new file mode 100644 index 0000000..2e0a0de --- /dev/null +++ b/examples/modules/dual_motor/two_motors.py @@ -0,0 +1,69 @@ +import math +from pimoroni_yukon import Yukon +from pimoroni_yukon import SLOT1 as SLOT +from pimoroni_yukon.modules import DualMotorModule +from pimoroni_yukon.timing import ticks_ms, ticks_add +from pimoroni_yukon.logging import LOG_WARN + +""" +How to drive up to 2 motors from a Dual Motor Module connected to Slot1. +A wave pattern will be played on the attached motors. +""" + +# Constants +SPEED = 0.005 # How much to advance the motor phase offset by each update +UPDATES = 50 # How many times to update the motors per second +SPEED_EXTENT = 1.0 # How far from zero to drive the motors +CURRENT_LIMIT = 0.5 # The maximum current (in amps) the motors will be driven with + +# Variables +yukon = Yukon(logging_level=LOG_WARN) # Create a new Yukon object, with its logging level lowered +module = DualMotorModule() # Create a DualMotorModule object +phase_offset = 0 # The offset used to animate the motors + + +# Function to get a motor speed from its index +def speed_from_index(index, offset=0.0): + phase = ((index / DualMotorModule.NUM_MOTORS) + offset) * math.pi * 2 + speed = math.sin(phase) * SPEED_EXTENT + return speed + + +# 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 + + module.current_limit(CURRENT_LIMIT) # Change the current limit (in amps) of the motor driver + module.enable() # Enable the motor driver on the DualMotorModule + + current_time = ticks_ms() # Record the start time of the program loop + + # Loop until the BOOT/USER button is pressed + while not yukon.is_boot_pressed(): + + # Give all the motors new speeds + current_motor = 0 + for motor in module.motors: + speed = speed_from_index(current_motor, phase_offset) + motor.speed(speed) + current_motor += 1 + + # Advance the phase offset, wrapping if it exceeds 1.0 + phase_offset += SPEED + if phase_offset >= 1.0: + phase_offset -= 1.0 + + print(f"Phase = {phase_offset}") + + # Advance the current time by a number of seconds + current_time = ticks_add(current_time, int(1000 / UPDATES)) + + # Monitor sensors until the current time is reached, recording the min, max, and average for each + # This approach accounts for the updating of the rainbows taking a non-zero amount of time to complete + yukon.monitor_until_ms(current_time) + +finally: + # Put the board back into a safe state, regardless of how the program may have ended + yukon.reset() diff --git a/examples/modules/quad_servo/all_servos.py b/examples/modules/quad_servo/all_servos.py index b695bba..4f46f8a 100644 --- a/examples/modules/quad_servo/all_servos.py +++ b/examples/modules/quad_servo/all_servos.py @@ -38,15 +38,15 @@ def angle_from_index(index, offset=0.0): try: # Find out which slots of Yukon have QuadServoDirectModules attached for slot in yukon.find_slots_with(QuadServoDirectModule): - module = QuadServoDirectModule(init_servos=False) # Create a QuadServoDirectModule object, but do not have it create Servo objects - yukon.register_with_slot(module, slot) # Register the QuadServoDirectModule object with the slot - modules.append(module) # Add the object to the module list + module = QuadServoDirectModule(init_servos=False) # Create a QuadServoDirectModule object, but do not have it create Servo objects + yukon.register_with_slot(module, slot) # Register the QuadServoDirectModule object with the slot + modules.append(module) # Add the object to the module list # Find out which slots of Yukon have QuadServoRegModule attached for slot in yukon.find_slots_with(QuadServoRegModule): - module = QuadServoRegModule(init_servos=False) # Create a QuadServoRegModule object, but do not have it create Servo objects - yukon.register_with_slot(module, slot) # Register the QuadServoDirectModule object with the slot - modules.append(module) # Add the object to the module list + module = QuadServoRegModule(init_servos=False) # Create a QuadServoRegModule object, but do not have it create Servo objects + yukon.register_with_slot(module, slot) # Register the QuadServoDirectModule object with the slot + modules.append(module) # Add the object to the module list # Record the number of servos that will be driven NUM_SERVOS = len(modules) * QuadServoDirectModule.NUM_SERVOS diff --git a/examples/yukon_adaptive_motor.py b/examples/yukon_adaptive_motor.py deleted file mode 100644 index 98e841b..0000000 --- a/examples/yukon_adaptive_motor.py +++ /dev/null @@ -1,56 +0,0 @@ -import math -from pimoroni_yukon import Yukon -from pimoroni_yukon.modules import DualMotorModule - -SPEED = 5 # The speed that the motors will cycle at -UPDATES = 50 # How many times to update LEDs and Servos per second -MOTOR_EXTENT = 1.0 # How far from zero to drive the motors - -# Create a Yukon object to begin using the board -yukon = Yukon() - -# List to store the modules -motor_modules = [] - -try: - # Create a Quad Servo Direct class for each populated module slot - for slot in yukon.find_slots_with_module(DualMotorModule): - dual_motor = DualMotorModule() - yukon.register_with_slot(dual_motor, slot) - motor_modules.append(dual_motor) - - # Initialise Yukon's registered modules - yukon.initialise_modules(allow_unregistered=True) - - NUM_MOTORS = len(motor_modules) * DualMotorModule.NUM_MOTORS - print(f"Up to {NUM_MOTORS} motors available") - - # Turn on the module power - yukon.enable_main_output() - - # Enable the outputs on the regulated servo modules - for module in motor_modules: - try: - module.enable() - except AttributeError: - # No enable function - pass - - offset = 0 - while not yukon.is_boot_pressed(): - offset += SPEED / 1000.0 - - # Update all the Motors - i = 0 - for module in motor_modules: - for motor in module.motors: - angle = ((i / NUM_MOTORS) + offset) * math.pi * 2 - motor.speed(math.sin(angle) * MOTOR_EXTENT) - i += 1 - - yukon.monitored_sleep(1.0 / UPDATES) - -finally: - # Put the board back into a safe state, regardless of how the program may have ended - yukon.reset() - diff --git a/lib/pimoroni_yukon/modules/dual_motor.py b/lib/pimoroni_yukon/modules/dual_motor.py index 3f1df21..9f1aba4 100644 --- a/lib/pimoroni_yukon/modules/dual_motor.py +++ b/lib/pimoroni_yukon/modules/dual_motor.py @@ -38,7 +38,7 @@ class DualMotorModule(YukonModule): def is_module(adc_level, slow1, slow2, slow3): return adc_level == ADC_HIGH and slow1 is LOW and slow2 is LOW and slow3 is HIGH - def __init__(self, motor_type=DUAL, frequency=DEFAULT_FREQUENCY, current_limit=DEFAULT_CURRENT_LIMIT): + def __init__(self, motor_type=DUAL, frequency=DEFAULT_FREQUENCY, current_limit=DEFAULT_CURRENT_LIMIT, init_motors=True): super().__init__() self.__motor_type = motor_type if self.__motor_type == self.STEPPER: @@ -46,6 +46,7 @@ def __init__(self, motor_type=DUAL, frequency=DEFAULT_FREQUENCY, current_limit=D self.__frequency = frequency self.__current_limit = current_limit + self.__init_motors = init_motors # An ascending order list of current limits with the pin states to achieve them self.__current_limit_states = OrderedDict({ @@ -61,21 +62,19 @@ def __init__(self, motor_type=DUAL, frequency=DEFAULT_FREQUENCY, current_limit=D }) def initialise(self, slot, adc1_func, adc2_func): - try: - # Create pwm objects - self.__pwms_p = (slot.FAST2, slot.FAST4) - self.__pwms_n = (slot.FAST1, slot.FAST3) - except ValueError as e: - if slot.ID <= 2 or slot.ID >= 5: - conflicting_slot = (((slot.ID - 1) + 4) % 8) + 1 - raise type(e)(f"PWM channel(s) already in use. Check that the module in Slot{conflicting_slot} does not share the same PWM channel(s)") from None - raise type(e)("PWM channel(s) already in use. Check that a module in another slot does not share the same PWM channel(s)") from None - if self.__motor_type == self.DUAL: - from motor import Motor - # Create motor objects - self.motors = [Motor((self.__pwms_p[i], self.__pwms_n[i]), freq=self.__frequency) for i in range(len(self.__pwms_p))] + # Store the pwm pins + pins_p = (slot.FAST2, slot.FAST4) + pins_n = (slot.FAST1, slot.FAST3) + + if self.__init_motors: + from motor import Motor + + # Create motor objects + self.motors = [Motor((pins_p[i], pins_n[i]), freq=self.__frequency) for i in range(len(pins_p))] + else: + self.motor_pins = [(pins_p[i], pins_n[i]) for i in range(len(pins_p))] else: raise NotImplementedError("Stepper Motor support for the Dual Motor Module is currently not implemented") @@ -88,7 +87,7 @@ def initialise(self, slot, adc1_func, adc2_func): super().initialise(slot, adc1_func, adc2_func) def reset(self): - if self.__motor_type == self.DUAL: + if self.__motor_type == self.DUAL and self.__init_motors: for motor in self.motors: motor.disable() @@ -117,7 +116,7 @@ def current_limit(self, amps): # Find the closest current limit below the given amps value for limit, state in self.__current_limit_states.items(): - if limit < amps: + if limit > amps: break chosen_limit = limit chosen_state = state @@ -142,11 +141,15 @@ def current_limit(self, amps): @property def motor1(self): - return self.motors[0] + if self.__motor_type == self.DUAL and self.__init_motors: + return self.motors[0] + raise RuntimeError("motor1 is only accessible with the DUAL motor_type, and if init_motors was True during initialisation") @property def motor2(self): - return self.motors[1] + if self.__motor_type == self.DUAL and self.__init_motors: + return self.motors[1] + raise RuntimeError("motor2 is only accessible with the DUAL motor_type, and if init_motors was True during initialisation") def read_fault(self): return self.__read_adc1() <= self.FAULT_THRESHOLD diff --git a/lib/pimoroni_yukon/modules/led_strip.py b/lib/pimoroni_yukon/modules/led_strip.py index 1d36356..8e0ab0e 100644 --- a/lib/pimoroni_yukon/modules/led_strip.py +++ b/lib/pimoroni_yukon/modules/led_strip.py @@ -107,11 +107,15 @@ def is_enabled(self): @property def strip1(self): - return self.strips[0] + if self.__strip_type == self.DUAL_NEOPIXEL: + return self.strips[0] + raise RuntimeError("strip1 is only accessible with the DUAL_NEOPIXEL strip_type") @property def strip2(self): - return self.strips[1] + if self.__strip_type == self.DUAL_NEOPIXEL: + return self.strips[1] + raise RuntimeError("strip2 is only accessible with the DUAL_NEOPIXEL strip_type") def read_power_good(self): return self.__power_good.value() == 1 diff --git a/lib/pimoroni_yukon/modules/quad_servo_direct.py b/lib/pimoroni_yukon/modules/quad_servo_direct.py index 0de8ebb..d009f4e 100644 --- a/lib/pimoroni_yukon/modules/quad_servo_direct.py +++ b/lib/pimoroni_yukon/modules/quad_servo_direct.py @@ -32,32 +32,39 @@ def initialise(self, slot, adc1_func, adc2_func): # Create servo objects self.servos = [Servo(pins[i], freq=50) for i in range(len(pins))] else: - self.servos = None self.servo_pins = pins # Pass the slot and adc functions up to the parent now that module specific initialisation has finished super().initialise(slot, adc1_func, adc2_func) def reset(self): - if self.servos is not None: + if self.__init_servos: for servo in self.servos: servo.disable() @property def servo1(self): - return self.servos[0] + if self.__init_servos: + return self.servos[0] + raise RuntimeError("servo1 is only accessible if init_servos was True during initialisation") @property def servo2(self): - return self.servos[1] + if self.__init_servos: + return self.servos[1] + raise RuntimeError("servo2 is only accessible if init_servos was True during initialisation") @property def servo3(self): - return self.servos[2] + if self.__init_servos: + return self.servos[2] + raise RuntimeError("servo3 is only accessible if init_servos was True during initialisation") @property def servo4(self): - return self.servos[3] + if self.__init_servos: + return self.servos[3] + raise RuntimeError("servo4 is only accessible if init_servos was True during initialisation") def read_adc1(self): return self.__read_adc1() diff --git a/lib/pimoroni_yukon/modules/quad_servo_reg.py b/lib/pimoroni_yukon/modules/quad_servo_reg.py index 137913f..e390f60 100644 --- a/lib/pimoroni_yukon/modules/quad_servo_reg.py +++ b/lib/pimoroni_yukon/modules/quad_servo_reg.py @@ -37,7 +37,6 @@ def initialise(self, slot, adc1_func, adc2_func): # Create servo objects self.servos = [Servo(pins[i], freq=50) for i in range(len(pins))] else: - self.servos = None self.servo_pins = pins # Create the power control pin objects @@ -48,7 +47,7 @@ def initialise(self, slot, adc1_func, adc2_func): super().initialise(slot, adc1_func, adc2_func) def reset(self): - if self.servos is not None: + if self.__init_servos: for servo in self.servos: servo.disable() @@ -66,19 +65,27 @@ def is_enabled(self): @property def servo1(self): - return self.servos[0] + if self.__init_servos: + return self.servos[0] + raise RuntimeError("servo1 is only accessible if init_servos was True during initialisation") @property def servo2(self): - return self.servos[1] + if self.__init_servos: + return self.servos[1] + raise RuntimeError("servo2 is only accessible if init_servos was True during initialisation") @property def servo3(self): - return self.servos[2] + if self.__init_servos: + return self.servos[2] + raise RuntimeError("servo3 is only accessible if init_servos was True during initialisation") @property def servo4(self): - return self.servos[3] + if self.__init_servos: + return self.servos[3] + raise RuntimeError("servo4 is only accessible if init_servos was True during initialisation") def read_power_good(self): return self.__power_good.value() == 1