From 7d33120a5d87d2a4052bcb5cee60bbb6fd139342 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Fri, 3 Nov 2023 14:37:02 +0000 Subject: [PATCH] Finished off big motor examples --- docs/audio_amp.md | 57 ------- .../big_motor/all_motors_no_encoders.py | 89 +++++++++++ examples/modules/big_motor/multiple_motors.py | 99 ++++++++++++ .../modules/big_motor/position_control.py | 124 +++++++++++++++ .../big_motor/position_on_velocity_control.py | 141 ++++++++++++++++++ examples/modules/big_motor/single_motor.py | 28 ++-- .../big_motor/tuning/motor_profiler.py | 1 + .../tuning/position_on_velocity_tuning.py | 125 ++++++++++++++++ .../big_motor/tuning/position_tuning.py | 25 ++-- .../big_motor/tuning/velocity_tuning.py | 110 ++++++++++++++ .../modules/big_motor/velocity_control.py | 126 ++++++++++++++++ examples/modules/dual_motor/all_motors.py | 1 - 12 files changed, 845 insertions(+), 81 deletions(-) delete mode 100644 docs/audio_amp.md create mode 100644 examples/modules/big_motor/all_motors_no_encoders.py create mode 100644 examples/modules/big_motor/multiple_motors.py create mode 100644 examples/modules/big_motor/position_control.py create mode 100644 examples/modules/big_motor/position_on_velocity_control.py create mode 100644 examples/modules/big_motor/tuning/position_on_velocity_tuning.py create mode 100644 examples/modules/big_motor/tuning/velocity_tuning.py create mode 100644 examples/modules/big_motor/velocity_control.py diff --git a/docs/audio_amp.md b/docs/audio_amp.md deleted file mode 100644 index 1c25ead..0000000 --- a/docs/audio_amp.md +++ /dev/null @@ -1,57 +0,0 @@ -# Audio Amp Module - Library Reference - -This is the library reference for the [Audio Amp Module for Yukon](https://pimoroni.com/yukon). - -- [Constants](#constants) -- [Variables \& Properties](#variables--properties) -- [Methods](#methods) - - -## Constants -```python -NAME = "Audio Amp" -AMP_I2C_ADDRESS = 0x38 -TEMPERATURE_THRESHOLD = 50.0 -``` - -## Variables & Properties -```python -I2S_DATA : SLOT -I2S_CLK : SLOT -I2S_FS : SLOT -``` - -## Methods -```python -## Address Checking ## -@staticmethod -is_module(adc1_level : int, adc2_level : int, slow1 : bool, slow2 : bool, slow3 :bool) -> bool - -## Initialisation ## -AudioAmpModule() -initialise(slot : SLOT, adc1_func : Any, adc2_func : Any) -> None -reset() -> None - -## Power Control ## -enable() -> None -disable() -> None -is_enabled() -> bool - -## Output Control ## -exit_soft_shutdown() -> None -set_volume(volume : float) -> None - -## Sensing ## -read_temperature() -> float - -## Monitoring ## -monitor() -> None -get_readings() -> OrderedDict -process_readings() -> None -clear_readings() -> None - -## Soft I2C ## -write_i2c_reg(register : int, data : int) -> None -read_i2c_reg(register : int) -> int -detect_i2c() -> int -``` diff --git a/examples/modules/big_motor/all_motors_no_encoders.py b/examples/modules/big_motor/all_motors_no_encoders.py new file mode 100644 index 0000000..92041ac --- /dev/null +++ b/examples/modules/big_motor/all_motors_no_encoders.py @@ -0,0 +1,89 @@ +import math +from pimoroni_yukon import Yukon +from pimoroni_yukon.modules import BigMotorModule +from pimoroni_yukon.timing import ticks_ms, ticks_add +from motor import MotorCluster + +""" +How to drive up to 6 motors from a set of Big Motor + Encoder 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. + +Note: Is not possible to use more than 4 encoders whilst using a motor cluster, hence encoders being omitted from this example. +Attempting to do so causes a hard lock. This is likely a PIO program space limitation, though this needs to be investigated +""" + +# 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 +WAVE_SCALE = 1.0 # A scale to apply to the phase calculation to expand or contract the wave +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() # Create a new Yukon object +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 * WAVE_SCALE) / BigMotorModule.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 BigMotorModule attached + for slot in yukon.find_slots_with(BigMotorModule): + module = BigMotorModule(init_motor=False, # Create a BigMotorModule object + init_encoder=False) + yukon.register_with_slot(module, slot) # Register the BigMotorModule 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) * BigMotorModule.NUM_MOTORS + print(f"Up to {NUM_MOTORS} motors available") + + yukon.verify_and_initialise() # Verify that BigMotorModules 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 list comprehension + motors = MotorCluster(CLUSTER_PIO, CLUSTER_SM, + pins=[module.motor_pins for module in modules]) + + yukon.enable_main_output() # Turn on power to the module slots + + for module in modules: + module.enable() # Enable the motor driver on the BigMotorModule + + 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/big_motor/multiple_motors.py b/examples/modules/big_motor/multiple_motors.py new file mode 100644 index 0000000..12ef3a6 --- /dev/null +++ b/examples/modules/big_motor/multiple_motors.py @@ -0,0 +1,99 @@ +import math +from pimoroni_yukon import Yukon +from pimoroni_yukon.modules import BigMotorModule +from pimoroni_yukon.timing import ticks_ms, ticks_add + +""" +How to drive up to 4 motors from a set of Big Motor + Encoder Modules connected to Slots. +A wave pattern will be played on the attached motors, and their speeds printed out. + +To use more motors, look at the all_motors_no_encoders.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 +WAVE_SCALE = 1.0 # A scale to apply to the phase calculation to expand or contract the wave + +# Variables +yukon = Yukon() # Create a new Yukon object +modules = [] # A list to store BigMotorModule 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 * WAVE_SCALE) / BigMotorModule.NUM_MOTORS) + offset) * math.pi * 2 + speed = math.sin(phase) * SPEED_EXTENT + return speed + + +# Generator to get the next PIO and State Machine numbers +def pio_and_sm_generator(): + pio = 0 + sm = 0 + while True: + yield (pio, sm) # Return the next pair of PIO and SM values + + sm += 1 # Advance by one SM + + # Wrap the SM and increment the PIO + if sm > 3: + sm -= 4 + pio += 1 + + +pio_and_sm = pio_and_sm_generator() # An instance of the generator + +# Wrap the code in a try block, to catch any exceptions (including KeyboardInterrupt) +try: + # Find out which slots of Yukon have BigMotorModule attached + for slot in yukon.find_slots_with(BigMotorModule): + pio, sm = next(pio_and_sm) # Get the next PIO and State Machine numbers + module = BigMotorModule(encoder_pio=pio, # Create a BigMotorModule object, with a specific PIO and SM for each encoder + encoder_sm=sm) + yukon.register_with_slot(module, slot) # Register the BigMotorModule 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) * BigMotorModule.NUM_MOTORS + print(f"Up to {NUM_MOTORS} motors available") + + yukon.verify_and_initialise() # Verify that BigMotorModules are attached to Yukon, and initialise them + yukon.enable_main_output() # Turn on power to the module slots + + for module in modules: + module.enable() # Enable the motor driver on the BigMotorModule + + 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(): + + # Read all the encoders and give all the motors new speeds + current_motor = 0 + for module in modules: + capture = module.encoder.capture() # Capture the state of the encoder + print(f"RPS{current_motor} = {capture.revolutions_per_second}", end=", ") # Print out the measured speed of the motor + + speed = speed_from_index(current_motor, phase_offset) + module.motor.speed(speed) + current_motor += 1 + print() + + # Advance the phase offset, wrapping if it exceeds 1.0 + phase_offset += SPEED + if phase_offset >= 1.0: + phase_offset -= 1.0 + + # 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/big_motor/position_control.py b/examples/modules/big_motor/position_control.py new file mode 100644 index 0000000..cf0053e --- /dev/null +++ b/examples/modules/big_motor/position_control.py @@ -0,0 +1,124 @@ +import math +import random +from pimoroni import PID, NORMAL_DIR # , REVERSED_DIR +from pimoroni_yukon import Yukon +from pimoroni_yukon import SLOT1 as SLOT +from pimoroni_yukon.modules import BigMotorModule +from pimoroni_yukon.timing import ticks_ms, ticks_add + +""" +How to drive a motor smoothly between random positions, with the help of it's attached encoder and PID control. +This uses a Big Motor + Encoder Module connected to Slot1. + +Press "Boot/User" to exit the program. +""" + +# Constants +GEAR_RATIO = 30 # The gear ratio of the motor +ENCODER_CPR = 12 # The number of counts a single encoder shaft revolution will produce +MOTOR_CPR = GEAR_RATIO * ENCODER_CPR # The number of counts a single motor shaft revolution will produce + +MOTOR_DIRECTION = NORMAL_DIR # The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1) +ENCODER_DIRECTION = NORMAL_DIR # The direction the encoder counts positive in. NORMAL_DIR (0), REVERSED_DIR (1) +SPEED_SCALE = 3.4 # The scaling to apply to the motor's speed to match its real-world speed + +UPDATES = 100 # How many times to update the motor per second +UPDATE_RATE = 1 / UPDATES +TIME_FOR_EACH_MOVE = 1 # The time to travel between each random value +UPDATES_PER_MOVE = TIME_FOR_EACH_MOVE * UPDATES +PRINT_DIVIDER = 4 # How many of the updates should be printed (i.e. 2 would be every other update) + +# Multipliers for the different printed values, so they appear nicely on the Thonny plotter +SPD_PRINT_SCALE = 20 # Driving Speed multipler + +POSITION_EXTENT = 180 # How far from zero to move the motor, in degrees +INTERP_MODE = 2 # The interpolating mode between setpoints. STEP (0), LINEAR (1), COSINE (2) + +# PID values +POS_KP = 0.14 # Position proportional (P) gain +POS_KI = 0.0 # Position integral (I) gain +POS_KD = 0.0022 # Position derivative (D) gain + + +# Variables +yukon = Yukon() # Create a new Yukon object +module = BigMotorModule(counts_per_rev=MOTOR_CPR) # Create a BigMotorModule object +pos_pid = PID(POS_KP, POS_KI, POS_KD, UPDATE_RATE) # Create a PID object for position control +update = 0 +print_count = 0 + +# Wrap the code in a try block, to catch any exceptions (including KeyboardInterrupt) +try: + yukon.register_with_slot(module, SLOT) # Register the BigMotorModule object with the slot + yukon.verify_and_initialise() # Verify that a BigMotorModule is attached to Yukon, and initialise it + yukon.enable_main_output() # Turn on power to the module slots + + module.motor.speed_scale(SPEED_SCALE) # Set the motor's speed scale + + # Set the motor and encoder's direction + module.motor.direction(MOTOR_DIRECTION) + module.encoder.direction(ENCODER_DIRECTION) + + module.enable() # Enable the motor driver on the BigMotorModule + module.motor.enable() # Enable the motor to get started + + # Set the initial value and create a random end value between the extents + start_value = 0.0 + end_value = random.uniform(-POSITION_EXTENT, POSITION_EXTENT) + + 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(): + + capture = module.encoder.capture() # Capture the state of the encoder + + # Calculate how far along this movement to be + percent_along = min(update / UPDATES_PER_MOVE, 1.0) + + if INTERP_MODE == 0: + # Move the motor instantly to the end value + pos_pid.setpoint = end_value + elif INTERP_MODE == 2: + # Move the motor between values using cosine + pos_pid.setpoint = (((-math.cos(percent_along * math.pi) + 1.0) / 2.0) * (end_value - start_value)) + start_value + else: + # Move the motor linearly between values + pos_pid.setpoint = (percent_along * (end_value - start_value)) + start_value + + # Calculate the velocity to move the motor closer to the position setpoint + vel = pos_pid.calculate(capture.degrees, capture.degrees_per_second) + + module.motor.speed(vel) # Set the new motor driving speed + + # Print out the current motor values and their setpoints, but only on every multiple + if print_count == 0: + print("Pos =", capture.degrees, end=", ") + print("Pos SP =", pos_pid.setpoint, end=", ") + print("Speed = ", module.motor.speed() * SPD_PRINT_SCALE) + + # Increment the print count, and wrap it + print_count = (print_count + 1) % PRINT_DIVIDER + + update += 1 # Move along in time + + # Have we reached the end of this movement? + if update >= UPDATES_PER_MOVE: + update = 0 # Reset the counter + + # Set the start as the last end and create a new random end value + start_value = end_value + end_value = random.uniform(-POSITION_EXTENT, POSITION_EXTENT) + + # Advance the current time by a number of seconds + current_time = ticks_add(current_time, int(1000 * UPDATE_RATE)) + + # 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) + + module.motor.disable() # Disable the motor + +finally: + # Put the board back into a safe state, regardless of how the program may have ended + yukon.reset() diff --git a/examples/modules/big_motor/position_on_velocity_control.py b/examples/modules/big_motor/position_on_velocity_control.py new file mode 100644 index 0000000..a872d26 --- /dev/null +++ b/examples/modules/big_motor/position_on_velocity_control.py @@ -0,0 +1,141 @@ +import math +import random +from pimoroni import PID, NORMAL_DIR # , REVERSED_DIR +from pimoroni_yukon import Yukon +from pimoroni_yukon import SLOT1 as SLOT +from pimoroni_yukon.modules import BigMotorModule +from pimoroni_yukon.timing import ticks_ms, ticks_add + +""" +How to move a motor smoothly between random positions, with velocity limits, with the help of it's attached encoder and PID control. +This uses a Big Motor + Encoder Module connected to Slot1. + +Press "Boot/User" to exit the program. +""" + +# Constants +GEAR_RATIO = 30 # The gear ratio of the motor +ENCODER_CPR = 12 # The number of counts a single encoder shaft revolution will produce +MOTOR_CPR = GEAR_RATIO * ENCODER_CPR # The number of counts a single motor shaft revolution will produce + +MOTOR_DIRECTION = NORMAL_DIR # The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1) +ENCODER_DIRECTION = NORMAL_DIR # The direction the encoder counts positive in. NORMAL_DIR (0), REVERSED_DIR (1) +SPEED_SCALE = 3.4 # The scaling to apply to the motor's speed to match its real-world speed + +UPDATES = 100 # How many times to update the motor per second +UPDATE_RATE = 1 / UPDATES +TIME_FOR_EACH_MOVE = 1 # The time to travel between each random value, in seconds +UPDATES_PER_MOVE = TIME_FOR_EACH_MOVE * UPDATES +PRINT_DIVIDER = 4 # How many of the updates should be printed (i.e. 2 would be every other update) + +# Multipliers for the different printed values, so they appear nicely on the Thonny plotter +ACC_PRINT_SCALE = 2 # Acceleration multiplier +SPD_PRINT_SCALE = 40 # Driving Speed multipler + +POSITION_EXTENT = 180 # How far from zero to move the motor, in degrees +MAX_SPEED = 1.0 # The maximum speed to move the motor at, in revolutions per second +INTERP_MODE = 0 # The interpolating mode between setpoints. STEP (0), LINEAR (1), COSINE (2) + +# PID values +POS_KP = 0.035 # Position proportional (P) gain +POS_KI = 0.0 # Position integral (I) gain +POS_KD = 0.002 # Position derivative (D) gain + +VEL_KP = 30.0 # Velocity proportional (P) gain +VEL_KI = 0.0 # Velocity integral (I) gain +VEL_KD = 0.4 # Velocity derivative (D) gain + + +# Variables +yukon = Yukon() # Create a new Yukon object +module = BigMotorModule(counts_per_rev=MOTOR_CPR) # Create a BigMotorModule object +pos_pid = PID(POS_KP, POS_KI, POS_KD, UPDATE_RATE) # Create a PID object for position control +vel_pid = PID(VEL_KP, VEL_KI, VEL_KD, UPDATE_RATE) # Create a PID object for velocity control +update = 0 +print_count = 0 + +# Wrap the code in a try block, to catch any exceptions (including KeyboardInterrupt) +try: + yukon.register_with_slot(module, SLOT) # Register the BigMotorModule object with the slot + yukon.verify_and_initialise() # Verify that a BigMotorModule is attached to Yukon, and initialise it + yukon.enable_main_output() # Turn on power to the module slots + + module.motor.speed_scale(SPEED_SCALE) # Set the motor's speed scale + + # Set the motor and encoder's direction + module.motor.direction(MOTOR_DIRECTION) + module.encoder.direction(ENCODER_DIRECTION) + + module.enable() # Enable the motor driver on the BigMotorModule + module.motor.enable() # Enable the motor to get started + + # Set the initial value and create a random end value between the extents + start_value = 0.0 + end_value = random.uniform(-POSITION_EXTENT, POSITION_EXTENT) + + 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(): + + capture = module.encoder.capture() # Capture the state of the encoder + + # Calculate how far along this movement to be + percent_along = min(update / UPDATES_PER_MOVE, 1.0) + + if INTERP_MODE == 0: + # Move the motor instantly to the end value + pos_pid.setpoint = end_value + elif INTERP_MODE == 2: + # Move the motor between values using cosine + pos_pid.setpoint = (((-math.cos(percent_along * math.pi) + 1.0) / 2.0) * (end_value - start_value)) + start_value + else: + # Move the motor linearly between values + pos_pid.setpoint = (percent_along * (end_value - start_value)) + start_value + + # Calculate the velocity to move the motor closer to the position setpoint + vel = pos_pid.calculate(capture.degrees, capture.degrees_per_second) + + # Limit the velocity between user defined limits, and set it as the new setpoint of the velocity PID + vel_pid.setpoint = max(min(vel, MAX_SPEED), -MAX_SPEED) + + # Calculate the acceleration to apply to the motor to move it closer to the velocity setpoint + accel = vel_pid.calculate(capture.revolutions_per_second) + + # Accelerate or decelerate the motor + module.motor.speed(module.motor.speed() + (accel * UPDATE_RATE)) + + # Print out the current motor values and their setpoints, but only on every multiple + if print_count == 0: + print("Pos =", capture.degrees, end=", ") + print("Pos SP =", pos_pid.setpoint, end=", ") + print("Vel =", capture.revolutions_per_second * SPD_PRINT_SCALE, end=", ") + print("Vel SP =", vel_pid.setpoint * SPD_PRINT_SCALE, end=", ") + print("Accel =", accel * ACC_PRINT_SCALE, end=", ") + print("Speed =", module.motor.speed() * SPD_PRINT_SCALE) + + # Increment the print count, and wrap it + print_count = (print_count + 1) % PRINT_DIVIDER + + update += 1 # Move along in time + + # Have we reached the end of this movement? + if update >= UPDATES_PER_MOVE: + update = 0 # Reset the counter + + # Set the start as the last end and create a new random end value + start_value = end_value + end_value = random.uniform(-POSITION_EXTENT, POSITION_EXTENT) + + # Advance the current time by a number of seconds + current_time = ticks_add(current_time, int(1000 * UPDATE_RATE)) + + # 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) + + module.motor.disable() # Disable the motor + +finally: + # Put the board back into a safe state, regardless of how the program may have ended + yukon.reset() diff --git a/examples/modules/big_motor/single_motor.py b/examples/modules/big_motor/single_motor.py index d2ae7d3..dc55299 100644 --- a/examples/modules/big_motor/single_motor.py +++ b/examples/modules/big_motor/single_motor.py @@ -6,19 +6,26 @@ """ How to drive a single motor from a Big Motor + Encoder Module connected to Slot1. -A wave pattern will be played on the attached motor. +A wave pattern will be played on the attached motor, and its speed printed out. """ # 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 +GEAR_RATIO = 30 # The gear ratio of the motor +ENCODER_CPR = 12 # The number of counts a single encoder shaft revolution will produce +MOTOR_CPR = GEAR_RATIO * ENCODER_CPR # The number of counts a single motor shaft revolution will produce +ENCODER_PIO = 0 # The PIO system to use (0 or 1) for the motor's encoder +ENCODER_SM = 0 # The State Machines (SM) to use for the motor's encoder + +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 # Variables -yukon = Yukon() # Create a new Yukon object -module = BigMotorModule() # Create a BigMotorModule object -phase_offset = 0 # The offset used to animate the motors +yukon = Yukon() # Create a new Yukon object +module = BigMotorModule(encoder_pio=ENCODER_PIO, # Create a BigMotorModule object, with details of the encoder + encoder_sm=ENCODER_SM, + counts_per_rev=MOTOR_CPR) +phase_offset = 0 # The offset used to animate the motor # Wrap the code in a try block, to catch any exceptions (including KeyboardInterrupt) try: @@ -33,6 +40,9 @@ # Loop until the BOOT/USER button is pressed while not yukon.is_boot_pressed(): + capture = module.encoder.capture() # Capture the state of the encoder + print(f"RPS = {capture.revolutions_per_second}") # Print out the measured speed of the motor + # Give the motor a new speed phase = phase_offset * math.pi * 2 speed = math.sin(phase) * SPEED_EXTENT @@ -43,8 +53,6 @@ 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)) diff --git a/examples/modules/big_motor/tuning/motor_profiler.py b/examples/modules/big_motor/tuning/motor_profiler.py index bab8d57..90bd2fe 100644 --- a/examples/modules/big_motor/tuning/motor_profiler.py +++ b/examples/modules/big_motor/tuning/motor_profiler.py @@ -6,6 +6,7 @@ """ A program that profiles the speed of a motor across its PWM duty cycle range using the attached encoder for feedback. Note that the returned readings will only be valid for a single input voltage. +This uses a Big Motor + Encoder Module connected to Slot1. """ # Constants diff --git a/examples/modules/big_motor/tuning/position_on_velocity_tuning.py b/examples/modules/big_motor/tuning/position_on_velocity_tuning.py new file mode 100644 index 0000000..44fa119 --- /dev/null +++ b/examples/modules/big_motor/tuning/position_on_velocity_tuning.py @@ -0,0 +1,125 @@ +from pimoroni import PID, NORMAL_DIR # , REVERSED_DIR +from pimoroni_yukon import Yukon +from pimoroni_yukon import SLOT1 as SLOT +from pimoroni_yukon.modules import BigMotorModule +from pimoroni_yukon.timing import ticks_ms, ticks_add + +""" +A program to aid in the discovery and tuning of motor PID values for position on velocity control. +It does this by commanding the motor to move repeatedly between two setpoint angles and plots +the measured response. +This uses a Big Motor + Encoder Module connected to Slot1. + +Press "Boot/User" to exit the program. +""" + +# Constants +GEAR_RATIO = 30 # The gear ratio of the motor +ENCODER_CPR = 12 # The number of counts a single encoder shaft revolution will produce +MOTOR_CPR = GEAR_RATIO * ENCODER_CPR # The number of counts a single motor shaft revolution will produce + +MOTOR_DIRECTION = NORMAL_DIR # The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1) +ENCODER_DIRECTION = NORMAL_DIR # The direction the encoder counts positive in. NORMAL_DIR (0), REVERSED_DIR (1) +SPEED_SCALE = 3.4 # The scaling to apply to the motor's speed to match its real-world speed + +UPDATES = 100 # How many times to update the motor per second +UPDATE_RATE = 1 / UPDATES +PRINT_WINDOW = 1.0 # The time (in seconds) after a new setpoint, to display print out motor values +MOVEMENT_WINDOW = 2.0 # The time (in seconds) between each new setpoint being set +PRINT_DIVIDER = 4 # How many of the updates should be printed (i.e. 2 would be every other update) + +# Multipliers for the different printed values, so they appear nicely on the Thonny plotter +ACC_PRINT_SCALE = 1 # Acceleration multiplier +SPD_PRINT_SCALE = 20 # Driving Speed multipler + +POSITION_EXTENT = 180 # How far from zero to move the motor, in degrees +MAX_SPEED = 2.0 # The maximum speed to move the motor at, in revolutions per second + +# PID values +POS_KP = 0.035 # Position proportional (P) gain +POS_KI = 0.0 # Position integral (I) gain +POS_KD = 0.002 # Position derivative (D) gain + +VEL_KP = 30.0 # Velocity proportional (P) gain +VEL_KI = 0.0 # Velocity integral (I) gain +VEL_KD = 0.4 # Velocity derivative (D) gain + + +# Variables +yukon = Yukon() # Create a new Yukon object +module = BigMotorModule(counts_per_rev=MOTOR_CPR) # Create a BigMotorModule object +pos_pid = PID(POS_KP, POS_KI, POS_KD, UPDATE_RATE) # Create a PID object for position control +vel_pid = PID(VEL_KP, VEL_KI, VEL_KD, UPDATE_RATE) # Create a PID object for velocity control +update = 0 +print_count = 0 + +# Wrap the code in a try block, to catch any exceptions (including KeyboardInterrupt) +try: + yukon.register_with_slot(module, SLOT) # Register the BigMotorModule object with the slot + yukon.verify_and_initialise() # Verify that a BigMotorModule is attached to Yukon, and initialise it + yukon.enable_main_output() # Turn on power to the module slots + + module.motor.speed_scale(SPEED_SCALE) # Set the motor's speed scale + + # Set the motor and encoder's direction + module.motor.direction(MOTOR_DIRECTION) + module.encoder.direction(ENCODER_DIRECTION) + + module.enable() # Enable the motor driver on the BigMotorModule + module.motor.enable() # Enable the motor to get started + + pos_pid.setpoint = POSITION_EXTENT # Set the initial setpoint position + + 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(): + + capture = module.encoder.capture() # Capture the state of the encoder + + # Calculate the velocity to move the motor closer to the position setpoint + vel = pos_pid.calculate(capture.degrees, capture.degrees_per_second) + + # Limit the velocity between user defined limits, and set it as the new setpoint of the velocity PID + vel_pid.setpoint = max(min(vel, MAX_SPEED), -MAX_SPEED) + + # Calculate the acceleration to apply to the motor to move it closer to the velocity setpoint + accel = vel_pid.calculate(capture.revolutions_per_second) + + # Accelerate or decelerate the motor + module.motor.speed(module.motor.speed() + (accel * UPDATE_RATE)) + + # Print out the current motor values and their setpoints, + # but only for the first few updates and only every multiple + if update < (PRINT_WINDOW * UPDATES) and print_count == 0: + print("Pos =", capture.degrees, end=", ") + print("Pos SP =", pos_pid.setpoint, end=", ") + print("Vel =", capture.revolutions_per_second * SPD_PRINT_SCALE, end=", ") + print("Vel SP =", vel_pid.setpoint * SPD_PRINT_SCALE, end=", ") + print("Accel =", accel * ACC_PRINT_SCALE, end=", ") + print("Speed =", module.motor.speed() * SPD_PRINT_SCALE) + + # Increment the print count, and wrap it + print_count = (print_count + 1) % PRINT_DIVIDER + + update += 1 # Move along in time + + # Have we reached the end of this time window? + if update >= (MOVEMENT_WINDOW * UPDATES): + update = 0 # Reset the counter + + # Set the new position setpoint to be the inverse of the current setpoint + pos_pid.setpoint = 0.0 - pos_pid.setpoint + + # Advance the current time by a number of seconds + current_time = ticks_add(current_time, int(1000 * UPDATE_RATE)) + + # 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) + + module.motor.disable() # Disable the motor + +finally: + # Put the board back into a safe state, regardless of how the program may have ended + yukon.reset() diff --git a/examples/modules/big_motor/tuning/position_tuning.py b/examples/modules/big_motor/tuning/position_tuning.py index 251b9b8..31e32d0 100644 --- a/examples/modules/big_motor/tuning/position_tuning.py +++ b/examples/modules/big_motor/tuning/position_tuning.py @@ -8,6 +8,9 @@ A program to aid in the discovery and tuning of motor PID values for position control. It does this by commanding the motor to move repeatedly between two setpoint angles and plots the measured response. +This uses a Big Motor + Encoder Module connected to Slot1. + +Press "Boot/User" to exit the program. """ # Constants @@ -15,7 +18,8 @@ ENCODER_CPR = 12 # The number of counts a single encoder shaft revolution will produce MOTOR_CPR = GEAR_RATIO * ENCODER_CPR # The number of counts a single motor shaft revolution will produce -DIRECTION = NORMAL_DIR # The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1) +MOTOR_DIRECTION = NORMAL_DIR # The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1) +ENCODER_DIRECTION = NORMAL_DIR # The direction the encoder counts positive in. NORMAL_DIR (0), REVERSED_DIR (1) SPEED_SCALE = 3.4 # The scaling to apply to the motor's speed to match its real-world speed UPDATES = 100 # How many times to update the motor per second @@ -38,6 +42,7 @@ # Variables yukon = Yukon() # Create a new Yukon object module = BigMotorModule(counts_per_rev=MOTOR_CPR) # Create a BigMotorModule object +pos_pid = PID(POS_KP, POS_KI, POS_KD, UPDATE_RATE) # Create a PID object for position control update = 0 print_count = 0 @@ -50,11 +55,8 @@ module.motor.speed_scale(SPEED_SCALE) # Set the motor's speed scale # Set the motor and encoder's direction - module.motor.direction(DIRECTION) - module.encoder.direction(DIRECTION) - - # Create PID object for position control - pos_pid = PID(POS_KP, POS_KI, POS_KD, UPDATE_RATE) + module.motor.direction(MOTOR_DIRECTION) + module.encoder.direction(ENCODER_DIRECTION) module.enable() # Enable the motor driver on the BigMotorModule module.motor.enable() # Enable the motor to get started @@ -66,21 +68,19 @@ # Loop until the BOOT/USER button is pressed while not yukon.is_boot_pressed(): - # Capture the state of the encoder - capture = module.encoder.capture() + capture = module.encoder.capture() # Capture the state of the encoder # Calculate the velocity to move the motor closer to the position setpoint vel = pos_pid.calculate(capture.degrees, capture.degrees_per_second) - # Set the new motor driving speed - module.motor.speed(vel) + module.motor.speed(vel) # Set the new motor driving speed # Print out the current motor values and their setpoints, # but only for the first few updates and only every multiple if update < (PRINT_WINDOW * UPDATES) and print_count == 0: print("Pos =", capture.degrees, end=", ") print("Pos SP =", pos_pid.setpoint, end=", ") - print("Speed = ", module.motor.speed() * SPD_PRINT_SCALE) + print("Speed =", module.motor.speed() * SPD_PRINT_SCALE) # Increment the print count, and wrap it print_count = (print_count + 1) % PRINT_DIVIDER @@ -101,8 +101,7 @@ # This approach accounts for the updating of the rainbows taking a non-zero amount of time to complete yukon.monitor_until_ms(current_time) - # Disable the motor - module.motor.disable() + module.motor.disable() # Disable the motor finally: # Put the board back into a safe state, regardless of how the program may have ended diff --git a/examples/modules/big_motor/tuning/velocity_tuning.py b/examples/modules/big_motor/tuning/velocity_tuning.py new file mode 100644 index 0000000..4940cdd --- /dev/null +++ b/examples/modules/big_motor/tuning/velocity_tuning.py @@ -0,0 +1,110 @@ +from pimoroni import PID, NORMAL_DIR # , REVERSED_DIR +from pimoroni_yukon import Yukon +from pimoroni_yukon import SLOT1 as SLOT +from pimoroni_yukon.modules import BigMotorModule +from pimoroni_yukon.timing import ticks_ms, ticks_add + +""" +A program to aid in the discovery and tuning of motor PID values for velocity control. +It does this by commanding the motor to drive repeatedly between two setpoint speeds and +plots the measured response. +This uses a Big Motor + Encoder Module connected to Slot1. + +Press "Boot/User" to exit the program. +""" + +# Constants +GEAR_RATIO = 30 # The gear ratio of the motor +ENCODER_CPR = 12 # The number of counts a single encoder shaft revolution will produce +MOTOR_CPR = GEAR_RATIO * ENCODER_CPR # The number of counts a single motor shaft revolution will produce + +MOTOR_DIRECTION = NORMAL_DIR # The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1) +ENCODER_DIRECTION = NORMAL_DIR # The direction the encoder counts positive in. NORMAL_DIR (0), REVERSED_DIR (1) +SPEED_SCALE = 3.4 # The scaling to apply to the motor's speed to match its real-world speed + +UPDATES = 100 # How many times to update the motor per second +UPDATE_RATE = 1 / UPDATES +PRINT_WINDOW = 0.25 # The time (in seconds) after a new setpoint, to display print out motor values +MOVEMENT_WINDOW = 2.0 # The time (in seconds) between each new setpoint being set +PRINT_DIVIDER = 1 # How many of the updates should be printed (i.e. 2 would be every other update) + +# Multipliers for the different printed values, so they appear nicely on the Thonny plotter +ACC_PRINT_SCALE = 0.01 # Acceleration multiplier + +VELOCITY_EXTENT = 1 # How far from zero to drive the motor at, in revolutions per second + +# PID values +VEL_KP = 30.0 # Velocity proportional (P) gain +VEL_KI = 0.0 # Velocity integral (I) gain +VEL_KD = 0.4 # Velocity derivative (D) gain + + +# Variables +yukon = Yukon() # Create a new Yukon object +module = BigMotorModule(counts_per_rev=MOTOR_CPR) # Create a BigMotorModule object +vel_pid = PID(VEL_KP, VEL_KI, VEL_KD, UPDATE_RATE) # Create a PID object for velocity control +update = 0 +print_count = 0 + +# Wrap the code in a try block, to catch any exceptions (including KeyboardInterrupt) +try: + yukon.register_with_slot(module, SLOT) # Register the BigMotorModule object with the slot + yukon.verify_and_initialise() # Verify that a BigMotorModule is attached to Yukon, and initialise it + yukon.enable_main_output() # Turn on power to the module slots + + module.motor.speed_scale(SPEED_SCALE) # Set the motor's speed scale + + # Set the motor and encoder's direction + module.motor.direction(MOTOR_DIRECTION) + module.encoder.direction(ENCODER_DIRECTION) + + module.enable() # Enable the motor driver on the BigMotorModule + module.motor.enable() # Enable the motor to get started + + vel_pid.setpoint = VELOCITY_EXTENT # Set the initial setpoint velocity + + 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(): + + capture = module.encoder.capture() # Capture the state of the encoder + + # Calculate the acceleration to apply to the motor to move it closer to the velocity setpoint + accel = vel_pid.calculate(capture.revolutions_per_second) + + # Accelerate or decelerate the motor + module.motor.speed(module.motor.speed() + (accel * UPDATE_RATE)) + + # Print out the current motor values and their setpoints, + # but only for the first few updates and only every multiple + if update < (PRINT_WINDOW * UPDATES) and print_count == 0: + print("Vel =", capture.revolutions_per_second, end=", ") + print("Vel SP =", vel_pid.setpoint, end=", ") + print("Accel =", accel * ACC_PRINT_SCALE, end=", ") + print("Speed =", module.motor.speed()) + + # Increment the print count, and wrap it + print_count = (print_count + 1) % PRINT_DIVIDER + + update += 1 # Move along in time + + # Have we reached the end of this time window? + if update >= (MOVEMENT_WINDOW * UPDATES): + update = 0 # Reset the counter + + # Set the new velocity setpoint to be the inverse of the current setpoint + vel_pid.setpoint = 0.0 - vel_pid.setpoint + + # Advance the current time by a number of seconds + current_time = ticks_add(current_time, int(1000 * UPDATE_RATE)) + + # 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) + + module.motor.disable() # Disable the motor + +finally: + # Put the board back into a safe state, regardless of how the program may have ended + yukon.reset() diff --git a/examples/modules/big_motor/velocity_control.py b/examples/modules/big_motor/velocity_control.py new file mode 100644 index 0000000..eba5ba5 --- /dev/null +++ b/examples/modules/big_motor/velocity_control.py @@ -0,0 +1,126 @@ +import math +import random +from pimoroni import PID, NORMAL_DIR # , REVERSED_DIR +from pimoroni_yukon import Yukon +from pimoroni_yukon import SLOT1 as SLOT +from pimoroni_yukon.modules import BigMotorModule +from pimoroni_yukon.timing import ticks_ms, ticks_add + +""" +How to drive a motor smoothly between random speeds, with the help of it's attached encoder and PID control. +This uses a Big Motor + Encoder Module connected to Slot1. + +Press "Boot/User" to exit the program. +""" + +# Constants +GEAR_RATIO = 30 # The gear ratio of the motor +ENCODER_CPR = 12 # The number of counts a single encoder shaft revolution will produce +MOTOR_CPR = GEAR_RATIO * ENCODER_CPR # The number of counts a single motor shaft revolution will produce + +MOTOR_DIRECTION = NORMAL_DIR # The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1) +ENCODER_DIRECTION = NORMAL_DIR # The direction the encoder counts positive in. NORMAL_DIR (0), REVERSED_DIR (1) +SPEED_SCALE = 3.4 # The scaling to apply to the motor's speed to match its real-world speed + +UPDATES = 100 # How many times to update the motor per second +UPDATE_RATE = 1 / UPDATES +TIME_FOR_EACH_MOVE = 1 # The time to travel between each random value, in seconds +UPDATES_PER_MOVE = TIME_FOR_EACH_MOVE * UPDATES +PRINT_DIVIDER = 4 # How many of the updates should be printed (i.e. 2 would be every other update) + +# Multipliers for the different printed values, so they appear nicely on the Thonny plotter +ACC_PRINT_SCALE = 0.05 # Acceleration multiplier + +VELOCITY_EXTENT = 3 # How far from zero to drive the motor at, in revolutions per second +INTERP_MODE = 2 # The interpolating mode between setpoints. STEP (0), LINEAR (1), COSINE (2) + +# PID values +VEL_KP = 30.0 # Velocity proportional (P) gain +VEL_KI = 0.0 # Velocity integral (I) gain +VEL_KD = 0.4 # Velocity derivative (D) gain + + +# Variables +yukon = Yukon() # Create a new Yukon object +module = BigMotorModule(counts_per_rev=MOTOR_CPR) # Create a BigMotorModule object +vel_pid = PID(VEL_KP, VEL_KI, VEL_KD, UPDATE_RATE) # Create a PID object for velocity control +update = 0 +print_count = 0 + +# Wrap the code in a try block, to catch any exceptions (including KeyboardInterrupt) +try: + yukon.register_with_slot(module, SLOT) # Register the BigMotorModule object with the slot + yukon.verify_and_initialise() # Verify that a BigMotorModule is attached to Yukon, and initialise it + yukon.enable_main_output() # Turn on power to the module slots + + module.motor.speed_scale(SPEED_SCALE) # Set the motor's speed scale + + # Set the motor and encoder's direction + module.motor.direction(MOTOR_DIRECTION) + module.encoder.direction(ENCODER_DIRECTION) + + module.enable() # Enable the motor driver on the BigMotorModule + module.motor.enable() # Enable the motor to get started + + # Set the initial value and create a random end value between the extents + start_value = 0.0 + end_value = random.uniform(-VELOCITY_EXTENT, VELOCITY_EXTENT) + + 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(): + + capture = module.encoder.capture() # Capture the state of the encoder + + # Calculate how far along this movement to be + percent_along = min(update / UPDATES_PER_MOVE, 1.0) + + if INTERP_MODE == 0: + # Move the motor instantly to the end value + vel_pid.setpoint = end_value + elif INTERP_MODE == 2: + # Move the motor between values using cosine + vel_pid.setpoint = (((-math.cos(percent_along * math.pi) + 1.0) / 2.0) * (end_value - start_value)) + start_value + else: + # Move the motor linearly between values + vel_pid.setpoint = (percent_along * (end_value - start_value)) + start_value + + # Calculate the acceleration to apply to the motor to move it closer to the velocity setpoint + accel = vel_pid.calculate(capture.revolutions_per_second) + + # Accelerate or decelerate the motor + module.motor.speed(module.motor.speed() + (accel * UPDATE_RATE)) + + # Print out the current motor values and their setpoints, but only on every multiple + if print_count == 0: + print("Vel =", capture.revolutions_per_second, end=", ") + print("Vel SP =", vel_pid.setpoint, end=", ") + print("Accel =", accel * ACC_PRINT_SCALE, end=", ") + print("Speed =", module.motor.speed()) + + # Increment the print count, and wrap it + print_count = (print_count + 1) % PRINT_DIVIDER + + update += 1 # Move along in time + + # Have we reached the end of this movement? + if update >= UPDATES_PER_MOVE: + update = 0 # Reset the counter + + # Set the start as the last end and create a new random end value + start_value = end_value + end_value = random.uniform(-VELOCITY_EXTENT, VELOCITY_EXTENT) + + # Advance the current time by a number of seconds + current_time = ticks_add(current_time, int(1000 * UPDATE_RATE)) + + # 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) + + module.motor.disable() # Disable the motor + +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/all_motors.py b/examples/modules/dual_motor/all_motors.py index 386e0c7..0f05874 100644 --- a/examples/modules/dual_motor/all_motors.py +++ b/examples/modules/dual_motor/all_motors.py @@ -9,7 +9,6 @@ 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