diff --git a/drivers/motor/motor_state.cpp b/drivers/motor/motor_state.cpp index 6ae19373..ce66db91 100644 --- a/drivers/motor/motor_state.cpp +++ b/drivers/motor/motor_state.cpp @@ -62,7 +62,7 @@ namespace motor { speed = 0.0f - speed; // Clamp the speed between the hard limits - motor_speed = CLAMP(speed, -motor_scale, motor_scale); + motor_speed = CLAMP(speed, 0.0f - motor_scale, motor_scale); last_enabled_duty = motor_speed / motor_scale; return enable_with_return(); @@ -81,7 +81,7 @@ namespace motor { } float MotorState::to_percent_with_return(float in, float in_min, float in_max) { - float speed = MotorState::map_float(in, in_min, in_max, 0.0f - motor_speed, motor_speed); + float speed = MotorState::map_float(in, in_min, in_max, 0.0f - motor_scale, motor_scale); return set_speed_with_return(speed); } diff --git a/libraries/motor2040/motor2040.hpp b/libraries/motor2040/motor2040.hpp index 17dc22d4..acb8f17a 100644 --- a/libraries/motor2040/motor2040.hpp +++ b/libraries/motor2040/motor2040.hpp @@ -31,10 +31,13 @@ namespace motor { const uint ENCODER_D_A = 14; const uint ENCODER_D_B = 15; - const pin_pair ENCODER_A(ENCODER_A_A, ENCODER_A_B); - const pin_pair ENCODER_B(ENCODER_B_A, ENCODER_B_B); - const pin_pair ENCODER_C(ENCODER_C_A, ENCODER_C_B); - const pin_pair ENCODER_D(ENCODER_D_A, ENCODER_D_B); + // Although encoder A and B channels are arbitrary, our MMME Encoders + // that accompany Motor2040 count down when the motors are diving in a + // positive direction, so these pin pairs are set as B and A instead + const pin_pair ENCODER_A(ENCODER_A_B, ENCODER_A_A); + const pin_pair ENCODER_B(ENCODER_B_B, ENCODER_B_A); + const pin_pair ENCODER_C(ENCODER_C_B, ENCODER_C_A); + const pin_pair ENCODER_D(ENCODER_D_B, ENCODER_D_A); const uint NUM_ENCODERS = 4; const uint TX_TRIG = 16; diff --git a/micropython/examples/motor2040/motor_cluster.py b/micropython/examples/motor2040/motor_cluster.py index 400e364d..b06a8685 100644 --- a/micropython/examples/motor2040/motor_cluster.py +++ b/micropython/examples/motor2040/motor_cluster.py @@ -1,60 +1,64 @@ import gc import time import math -from servo import ServoCluster, servo2040 +from motor import MotorCluster, motor2040 """ -Demonstrates how to create a ServoCluster object to control multiple servos at once. +Demonstrates how to create a MotorCluster object to control multiple motors at once. -NOTE: ServoCluster uses the RP2040's PIO system, and as +NOTE: MotorCluster uses the RP2040's PIO system, and as such may have problems when running code multiple times. If you encounter issues, try resetting your board. """ -# Free up hardware resources ahead of creating a new ServoCluster +# Free up hardware resources ahead of creating a new MotorCluster gc.collect() -# Create a servo cluster for pins 0 to 3, using PIO 0 and State Machine 0 -START_PIN = servo2040.SERVO_1 -END_PIN = servo2040.SERVO_4 -servos = ServoCluster(pio=0, sm=0, pins=list(range(START_PIN, END_PIN + 1))) +# Create a motor cluster, using PIO 0 and State Machine 0 +MOTOR_PINS = [motor2040.MOTOR_A, motor2040.MOTOR_B, motor2040.MOTOR_C, motor2040.MOTOR_D] +motors = MotorCluster(pio=0, sm=0, pins=MOTOR_PINS) -# Enable all servos (this puts them at the middle) -servos.enable_all() +# Enable all motors +motors.enable_all() time.sleep(2) -# Go to min -servos.all_to_min() +# Drive at full positive +motors.all_full_positive() time.sleep(2) -# Go to max -servos.all_to_max() +# Stop all moving +motors.stop_all() time.sleep(2) -# Go back to mid -servos.all_to_mid() +# Drive at full negative +motors.all_full_negative() time.sleep(2) -SWEEPS = 3 # How many sweeps of the servo to perform +# Coast all to a gradual stop +motors.coast_all() +time.sleep(2) + + +SWEEPS = 2 # How many sweeps of the motors to perform STEPS = 10 # The number of discrete sweep steps STEPS_INTERVAL = 0.5 # The time in seconds between each step of the sequence -SWEEP_EXTENT = 90.0 # How far from zero to move the servos when sweeping +SPEED_EXTENT = 1.0 # How far from zero to drive the motors when sweeping -# Do a sine sweep +# Do a sine speed sweep for j in range(SWEEPS): for i in range(360): - value = math.sin(math.radians(i)) * SWEEP_EXTENT - servos.all_to_value(value) + speed = math.sin(math.radians(i)) * SPEED_EXTENT + motors.all_to_speed(speed) time.sleep(0.02) -# Do a stepped sweep +# Do a stepped speed sweep for j in range(SWEEPS): for i in range(0, STEPS): - servos.all_to_percent(i, 0, STEPS, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT) + motors.all_to_percent(i, 0, STEPS, 0.0 - SPEED_EXTENT, SPEED_EXTENT) time.sleep(STEPS_INTERVAL) for i in range(0, STEPS): - servos.all_to_percent(i, STEPS, 0, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT) + motors.all_to_percent(i, STEPS, 0, 0.0 - SPEED_EXTENT, SPEED_EXTENT) time.sleep(STEPS_INTERVAL) -# Disable the servos -servos.disable_all() +# Disable the motors +motors.disable_all() diff --git a/micropython/examples/motor2040/motor_profiler.py b/micropython/examples/motor2040/motor_profiler.py index 28cc875d..d930e0d4 100644 --- a/micropython/examples/motor2040/motor_profiler.py +++ b/micropython/examples/motor2040/motor_profiler.py @@ -2,7 +2,6 @@ import gc import time from motor import Motor, motor2040 from encoder import Encoder, MMME_CPR -from encoder import REVERSED """ A program that profiles the speed of a motor across its PWM @@ -14,6 +13,7 @@ ENCODER_PINS = motor2040.ENCODER_A # The pins of the encoder attached to th GEAR_RATIO = 50 # The gear ratio of the motor COUNTS_PER_REV = MMME_CPR * GEAR_RATIO # The counts per revolution of the motor's output shaft +DIRECTION = 0 # The direction to spin the motor in. NORMAL (0), REVERSED (1) SPEED_SCALE = 5.4 # The scaling to apply to the motor's speed # Set this to the maximum measured speed @@ -25,19 +25,19 @@ CAPTURE_TIME = 0.2 # How long to capture the motor's speed gc.collect() # Create a motor and set its speed scale, and give it a zero deadzone -m = Motor(MOTOR_PINS, speed_scale=SPEED_SCALE, deadzone=0.0) +m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE, deadzone=0.0) # Create an encoder, using PIO 0 and State Machine 0 -enc = Encoder(0, 0, ENCODER_PINS, counts_per_rev=COUNTS_PER_REV, count_microsteps=True) - -# Uncomment the below line (and the top import) to reverse the counting direction -enc.direction(REVERSED) +enc = Encoder(0, 0, ENCODER_PINS, direction=DIRECTION, counts_per_rev=COUNTS_PER_REV, count_microsteps=True) # Function that performs a single profiling step def profile_at_duty(duty): # Set the motor to a new duty cycle and wait for it to settle - m.duty(duty) + if DIRECTION == 1: + m.duty(0.0 - duty) + else: + m.duty(duty) time.sleep(SETTLE_TIME) # Perform a dummy capture to clear the encoder diff --git a/micropython/examples/motor2040/motor_wave.py b/micropython/examples/motor2040/motor_wave.py index 7d5c228e..369f326c 100644 --- a/micropython/examples/motor2040/motor_wave.py +++ b/micropython/examples/motor2040/motor_wave.py @@ -3,29 +3,28 @@ import time import math from pimoroni import Button from plasma import WS2812 -from motor import Motor, MotorCluster, motor2040 +from motor import Motor, motor2040 """ An example of applying a wave pattern to a group of motors and the LED. Press "Boot" to exit the program. -NOTE: MotorCluster and Plasma WS2812 use the RP2040's PIO system, -and as such may have problems when running code multiple times. +NOTE: Plasma WS2812 uses the RP2040's PIO system, and as +such may have problems when running code multiple times. If you encounter issues, try resetting your board. """ -SPEED = 5 # The speed that the LEDs will cycle at +SPEED = 5 # The speed that the LEDs and motors will cycle at BRIGHTNESS = 0.4 # The brightness of the LEDs -UPDATES = 50 # How many times to update LEDs and Motors per second -MOTOR_EXTENT = 1.0 # How far from zero to move the motors +UPDATES = 50 # How many times to update LEDs and motors per second +SPEED_EXTENT = 1.0 # How far from zero to drive the motors # Free up hardware resources ahead of creating a new MotorCluster gc.collect() -# Create a motor cluster for pins 0 to 7, using PIO 0 and State Machine 0 -# motors = MotorCluster(pio=0, sm=0, pins=list(range(START_PIN, END_PIN + 1))) -MOTOR_PINS = [ motor2040.MOTOR_1, motor2040.MOTOR_2, motor2040.MOTOR_3, motor2040.MOTOR_4] +# Create a list of motors +MOTOR_PINS = [motor2040.MOTOR_A, motor2040.MOTOR_B, motor2040.MOTOR_C, motor2040.MOTOR_D] motors = [Motor(pins) for pins in MOTOR_PINS] # Create the LED, using PIO 1 and State Machine 0 @@ -49,11 +48,10 @@ while user_sw.raw() is not True: led.set_hsv(0, offset / 2, 1.0, BRIGHTNESS) # Update all the MOTORs - #for i in range(motors.count()): for i in range(len(motors)): angle = ((i / len(motors)) + offset) * math.pi - motors[i].speed(math.sin(angle) * MOTOR_EXTENT) - + motors[i].speed(math.sin(angle) * SPEED_EXTENT) + time.sleep(1.0 / UPDATES) # Stop all the motors diff --git a/micropython/examples/motor2040/multiple_motors.py b/micropython/examples/motor2040/multiple_motors.py index 32696607..19ebacd5 100644 --- a/micropython/examples/motor2040/multiple_motors.py +++ b/micropython/examples/motor2040/multiple_motors.py @@ -7,35 +7,41 @@ Demonstrates how to create multiple Motor objects and control them together. """ # Create a list of motors -MOTOR_PINS = [ motor2040.MOTOR_1, motor2040.MOTOR_2, motor2040.MOTOR_3, motor2040.MOTOR_4] +MOTOR_PINS = [motor2040.MOTOR_A, motor2040.MOTOR_B, motor2040.MOTOR_C, motor2040.MOTOR_D] motors = [Motor(pins) for pins in MOTOR_PINS] -# Enable all motors (this puts them at the middle) +# Enable all motors for m in motors: m.enable() time.sleep(2) -# Go to min +# Drive at full positive for m in motors: m.full_positive() time.sleep(2) -# Go to max -for m in motors: - m.full_negative() -time.sleep(2) - -# Go back to mid +# Stop moving for m in motors: m.stop() time.sleep(2) -SWEEPS = 3 # How many sweeps of the motor to perform +# Drive at full negative +for m in motors: + m.full_negative() +time.sleep(2) + +# Coast to a gradual stop +for m in motors: + m.coast() +time.sleep(2) + + +SWEEPS = 2 # How many speed sweeps of the motor to perform STEPS = 10 # The number of discrete sweep steps STEPS_INTERVAL = 0.5 # The time in seconds between each step of the sequence -SPEED_EXTENT = 1.0 # How far from zero to move the motor when sweeping +SPEED_EXTENT = 1.0 # How far from zero to drive the motors when sweeping -# Do a sine sweep +# Do a sine speed sweep for j in range(SWEEPS): for i in range(360): speed = math.sin(math.radians(i)) * SPEED_EXTENT @@ -43,7 +49,7 @@ for j in range(SWEEPS): s.speed(speed) time.sleep(0.02) -# Do a stepped sweep +# Do a stepped speed sweep for j in range(SWEEPS): for i in range(0, STEPS): for m in motors: diff --git a/micropython/examples/motor2040/single_motor.py b/micropython/examples/motor2040/single_motor.py index 3c7e81d4..6f6b1bf2 100644 --- a/micropython/examples/motor2040/single_motor.py +++ b/micropython/examples/motor2040/single_motor.py @@ -6,43 +6,48 @@ from motor import Motor, motor2040 Demonstrates how to create a Motor object and control it. """ -# Create a motor on pins 4 and 5 -m = Motor(motor2040.MOTOR_1) +# Create a motor +m = Motor(motor2040.MOTOR_A) -# Enable the motor (this puts it at the middle) +# Enable the motor m.enable() time.sleep(2) -# Go to full positive +# Drive at full positive m.full_positive() time.sleep(2) -# Go to full negative -m.full_negative() -time.sleep(2) - # Stop moving m.stop() time.sleep(2) +# Drive at full negative +m.full_negative() +time.sleep(2) -SWEEPS = 3 # How many sweeps of the motor to perform +# Coast to a gradual stop +m.coast() +time.sleep(2) + + +SWEEPS = 2 # How many speed sweeps of the motor to perform STEPS = 10 # The number of discrete sweep steps STEPS_INTERVAL = 0.5 # The time in seconds between each step of the sequence +SPEED_EXTENT = 1.0 # How far from zero to drive the motor when sweeping -# Do a sine sweep +# Do a sine speed sweep for j in range(SWEEPS): for i in range(360): - m.speed(math.sin(math.radians(i))) + m.speed(math.sin(math.radians(i)) * SPEED_EXTENT) time.sleep(0.02) -# Do a stepped sweep +# Do a stepped speed sweep for j in range(SWEEPS): for i in range(0, STEPS): - m.to_percent(i, 0, STEPS) + m.to_percent(i, 0, STEPS, 0.0 - SPEED_EXTENT, SPEED_EXTENT) time.sleep(STEPS_INTERVAL) for i in range(0, STEPS): - m.to_percent(i, STEPS, 0) + m.to_percent(i, STEPS, 0, 0.0 - SPEED_EXTENT, SPEED_EXTENT) time.sleep(STEPS_INTERVAL) # Disable the motor diff --git a/micropython/examples/motor2040/turn_off_motors.py b/micropython/examples/motor2040/turn_off_motors.py index 1a33dd9c..bed8fcc2 100644 --- a/micropython/examples/motor2040/turn_off_motors.py +++ b/micropython/examples/motor2040/turn_off_motors.py @@ -7,7 +7,7 @@ A simple program that turns off the motors. # Create four motor objects. # This will initialise the pins, stopping any # previous movement that may be stuck on -m1 = Motor(motor2040.MOTOR_1) -m2 = Motor(motor2040.MOTOR_2) -m3 = Motor(motor2040.MOTOR_3) -m4 = Motor(motor2040.MOTOR_4) +mA = Motor(motor2040.MOTOR_A) +mB = Motor(motor2040.MOTOR_B) +mC = Motor(motor2040.MOTOR_C) +mD = Motor(motor2040.MOTOR_D) diff --git a/micropython/modules/motor/motor.c b/micropython/modules/motor/motor.c index 87f40ff7..a79ddb11 100644 --- a/micropython/modules/motor/motor.c +++ b/micropython/modules/motor/motor.c @@ -160,16 +160,16 @@ const mp_rom_obj_tuple_t motor2040_motorD_pins = { {&mp_type_tuple}, 2, { MP_ROM_INT(10), MP_ROM_INT(11) }, }; const mp_rom_obj_tuple_t motor2040_encoderA_pins = { - {&mp_type_tuple}, 2, { MP_ROM_INT(0), MP_ROM_INT(1) }, + {&mp_type_tuple}, 2, { MP_ROM_INT(1), MP_ROM_INT(0) }, }; const mp_rom_obj_tuple_t motor2040_encoderB_pins = { - {&mp_type_tuple}, 2, { MP_ROM_INT(2), MP_ROM_INT(3) }, + {&mp_type_tuple}, 2, { MP_ROM_INT(3), MP_ROM_INT(2) }, }; const mp_rom_obj_tuple_t motor2040_encoderC_pins = { - {&mp_type_tuple}, 2, { MP_ROM_INT(12), MP_ROM_INT(13) }, + {&mp_type_tuple}, 2, { MP_ROM_INT(13), MP_ROM_INT(12) }, }; const mp_rom_obj_tuple_t motor2040_encoderD_pins = { - {&mp_type_tuple}, 2, { MP_ROM_INT(14), MP_ROM_INT(15) }, + {&mp_type_tuple}, 2, { MP_ROM_INT(15), MP_ROM_INT(14) }, }; typedef struct _mp_obj_float_t {