diff --git a/micropython/examples/motor2040/README.md b/micropython/examples/motor2040/README.md new file mode 100644 index 00000000..4bb84e0a --- /dev/null +++ b/micropython/examples/motor2040/README.md @@ -0,0 +1,85 @@ +# Motor 2040 Micropython Examples + +- [Motor Examples](#motor-examples) + - [Single Motor](#single-motor) + - [Multiple Motors](#multiple-motors) + - [Motor Cluster](#motor-cluster) + - [Simple Easing](#simple-easing) + - [Motor Wave](#motor-wave) + - [Calibration](#calibration) +- [Function Examples](#function-examples) + - [Read Sensors](#read-sensors) + - [Sensor Feedback](#sensor-feedback) + - [Current Meter](#current-meter) + - [LED Rainbow](#led-rainbow) + - [Turn Off LEDs](#turn-off-leds) + + +## Motor Examples + +### Single Motor +[single_motor.py](single_motor.py) + +Demonstrates how to create a Motor object and control it. + + +### Multiple Motors +[multiple_motors.py](multiple_motors.py) + +Demonstrates how to create multiple Motor objects and control them together. + + +### Motor Cluster +[motor_cluster.py](motor_cluster.py) + +Demonstrates how to create a MotorCluster object to control multiple motors at once. + + +### Simple Easing +[simple_easing.py](simple_easing.py) + +An example of how to move a motor smoothly between random positions. + + +### Motor Wave +[motor_wave.py](motor_wave.py) + +An example of applying a wave pattern to a group of motors and the LEDs. + + +### Calibration +[calibration.py](calibration.py) + +Shows how to create motors with different common calibrations, modify a motor's existing calibration, and create a motor with a custom calibration. + + +## Function Examples + +### Read Sensors +[read_sensors.py](read_sensors.py) + +Shows how to initialise and read the 6 external and 2 internal sensors of Motor 2040. + + +### Sensor Feedback +[sensor_feedback.py](sensor_feedback.py) + +Show how to read the 6 external sensors and display their values on the neighbouring LEDs. + + +### Current Meter +[current_meter.py](current_meter.py) + +An example of how to use Motor 2040's current measuring ability and display the value on the onboard LED bar. + + +### LED Rainbow +[led_rainbow.py](led_rainbow.py) + +Displays a rotating rainbow pattern on the Motor 2040's onboard LED. + + +### Turn Off LED +[turn_off_led.py](turn_off_led.py) + +A simple program that turns off the onboard LED. diff --git a/micropython/examples/motor2040/current_meter.py b/micropython/examples/motor2040/current_meter.py new file mode 100644 index 00000000..5e4f4220 --- /dev/null +++ b/micropython/examples/motor2040/current_meter.py @@ -0,0 +1,90 @@ +import gc +import time +from machine import Pin +from pimoroni import Analog, AnalogMux, Button +from plasma import WS2812 +from servo import ServoCluster, servo2040 + +""" +An example of how to use Servo 2040's current measuring +ability and display the value on the onboard LED bar. + +Press "Boot" to exit the program. + +NOTE: ServoCluster and Plasma WS2812 use the RP2040's PIO system, +and as such may have problems when running code multiple times. +If you encounter issues, try resetting your board. +""" + +BRIGHTNESS = 0.4 # The brightness of the LEDs +UPDATES = 50 # How many times to update LEDs and Servos per second +MAX_CURRENT = 3.0 # The maximum current, in amps, to show on the meter +SAMPLES = 50 # The number of current measurements to take per reading +TIME_BETWEEN = 0.001 # The time between each current measurement + +# Free up hardware resources ahead of creating a new ServoCluster +gc.collect() + +# Create a servo cluster for pins 0 to 7, using PIO 0 and State Machine 0 +START_PIN = servo2040.SERVO_1 +END_PIN = servo2040.SERVO_8 +servos = ServoCluster(pio=0, sm=0, pins=list(range(START_PIN, END_PIN + 1))) + +# Set up the shared analog inputs +cur_adc = Analog(servo2040.SHARED_ADC, servo2040.CURRENT_GAIN, + servo2040.SHUNT_RESISTOR, servo2040.CURRENT_OFFSET) + +# Set up the analog multiplexer, including the pin for controlling pull-up/pull-down +mux = AnalogMux(servo2040.ADC_ADDR_0, servo2040.ADC_ADDR_1, servo2040.ADC_ADDR_2, + muxed_pin=Pin(servo2040.SHARED_ADC)) + +# Create the LED bar, using PIO 1 and State Machine 0 +led_bar = WS2812(servo2040.NUM_LEDS, 1, 0, servo2040.LED_DATA) + +# Create the user button +user_sw = Button(servo2040.USER_SW) + +# Start updating the LED bar +led_bar.start() + +# Enable all servos (this puts them at the middle). +# The servos are not going to be moved, but are activated to give a current draw +servos.enable_all() + +# Read sensors until the user button is pressed +while user_sw.raw() is not True: + + # Select the current sense + mux.select(servo2040.CURRENT_SENSE_ADDR) + + # Read the current sense several times and average the result + current = 0 + for i in range(SAMPLES): + current += cur_adc.read_current() + time.sleep(TIME_BETWEEN) + current /= SAMPLES + + # Print out the current sense value + print("Current =", round(current, 4)) + + # Convert the current to a percentage of the maximum we want to show + percent = (current / MAX_CURRENT) + + # Update all the LEDs + for i in range(servo2040.NUM_LEDS): + # Calculate the LED's hue, with Red for high currents and Green for low + hue = (1.0 - i / (servo2040.NUM_LEDS - 1)) * 0.333 + + # Calculate the current level the LED represents + level = (i + 0.5) / servo2040.NUM_LEDS + # If the percent is above the level, light the LED, otherwise turn it off + if percent >= level: + led_bar.set_hsv(i, hue, 1.0, BRIGHTNESS) + else: + led_bar.set_hsv(i, hue, 1.0, 0.0) + +# Disable the servos +servos.disable_all() + +# Turn off the LED bar +led_bar.clear() diff --git a/micropython/examples/motor2040/led_rainbow.py b/micropython/examples/motor2040/led_rainbow.py new file mode 100644 index 00000000..bb3db6b6 --- /dev/null +++ b/micropython/examples/motor2040/led_rainbow.py @@ -0,0 +1,43 @@ +import time +from pimoroni import Button +from plasma import WS2812 +from motor import motor2040 + +""" +Displays a rotating rainbow pattern on the Motor 2040's onboard LED. + +Press "Boot" to exit the program. + +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 +BRIGHTNESS = 0.4 # The brightness of the LEDs +UPDATES = 50 # How many times the LEDs will be updated per second + +# Create the LED, using PIO 1 and State Machine 0 +led = WS2812(motor2040.NUM_LEDS, 1, 0, motor2040.LED_DATA) + +# Create the user button +user_sw = Button(motor2040.USER_SW) + +# Start updating the LED +led.start() + + +hue = 0.0 + +# Make rainbows until the user button is pressed +while user_sw.raw() is not True: + + hue += SPEED / 1000.0 + + # Update the LED + led.set_hsv(0, hue, 1.0, BRIGHTNESS) + + time.sleep(1.0 / UPDATES) + +# Turn off the LED +led.clear() diff --git a/micropython/examples/motor2040/motor_cluster.py b/micropython/examples/motor2040/motor_cluster.py new file mode 100644 index 00000000..400e364d --- /dev/null +++ b/micropython/examples/motor2040/motor_cluster.py @@ -0,0 +1,60 @@ +import gc +import time +import math +from servo import ServoCluster, servo2040 + +""" +Demonstrates how to create a ServoCluster object to control multiple servos at once. + +NOTE: ServoCluster 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 +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))) + +# Enable all servos (this puts them at the middle) +servos.enable_all() +time.sleep(2) + +# Go to min +servos.all_to_min() +time.sleep(2) + +# Go to max +servos.all_to_max() +time.sleep(2) + +# Go back to mid +servos.all_to_mid() +time.sleep(2) + +SWEEPS = 3 # How many sweeps of the servo 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 + +# Do a sine sweep +for j in range(SWEEPS): + for i in range(360): + value = math.sin(math.radians(i)) * SWEEP_EXTENT + servos.all_to_value(value) + time.sleep(0.02) + +# Do a stepped 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) + time.sleep(STEPS_INTERVAL) + for i in range(0, STEPS): + servos.all_to_percent(i, STEPS, 0, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT) + time.sleep(STEPS_INTERVAL) + +# Disable the servos +servos.disable_all() diff --git a/micropython/examples/motor2040/motor_wave.py b/micropython/examples/motor2040/motor_wave.py new file mode 100644 index 00000000..7d5c228e --- /dev/null +++ b/micropython/examples/motor2040/motor_wave.py @@ -0,0 +1,64 @@ +import gc +import time +import math +from pimoroni import Button +from plasma import WS2812 +from motor import Motor, MotorCluster, 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. +If you encounter issues, try resetting your board. +""" + +SPEED = 5 # The speed that the LEDs 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 + +# 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] +motors = [Motor(pins) for pins in MOTOR_PINS] + +# Create the LED, using PIO 1 and State Machine 0 +led = WS2812(motor2040.NUM_LEDS, 1, 0, motor2040.LED_DATA) + +# Create the user button +user_sw = Button(motor2040.USER_SW) + +# Start updating the LED +led.start() + + +offset = 0.0 + +# Make waves until the user button is pressed +while user_sw.raw() is not True: + + offset += SPEED / 1000.0 + + # Update the LED + 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) + + time.sleep(1.0 / UPDATES) + +# Stop all the motors +for m in motors: + m.disable() + +# Turn off the LED bar +led.clear() diff --git a/micropython/examples/motor2040/multiple_motors.py b/micropython/examples/motor2040/multiple_motors.py new file mode 100644 index 00000000..32696607 --- /dev/null +++ b/micropython/examples/motor2040/multiple_motors.py @@ -0,0 +1,59 @@ +import time +import math +from motor import Motor, motor2040 + +""" +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] +motors = [Motor(pins) for pins in MOTOR_PINS] + +# Enable all motors (this puts them at the middle) +for m in motors: + m.enable() +time.sleep(2) + +# Go to min +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 +for m in motors: + m.stop() +time.sleep(2) + +SWEEPS = 3 # How many 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 + +# Do a sine sweep +for j in range(SWEEPS): + for i in range(360): + speed = math.sin(math.radians(i)) * SPEED_EXTENT + for s in motors: + s.speed(speed) + time.sleep(0.02) + +# Do a stepped sweep +for j in range(SWEEPS): + for i in range(0, STEPS): + for m in motors: + m.to_percent(i, 0, STEPS, 0.0 - SPEED_EXTENT, SPEED_EXTENT) + time.sleep(STEPS_INTERVAL) + for i in range(0, STEPS): + for m in motors: + m.to_percent(i, STEPS, 0, 0.0 - SPEED_EXTENT, SPEED_EXTENT) + time.sleep(STEPS_INTERVAL) + +# Disable the motors +for m in motors: + m.disable() diff --git a/micropython/examples/motor2040/read_sensors.py b/micropython/examples/motor2040/read_sensors.py new file mode 100644 index 00000000..e6c64c66 --- /dev/null +++ b/micropython/examples/motor2040/read_sensors.py @@ -0,0 +1,48 @@ +import time +from machine import Pin +from pimoroni import Analog, AnalogMux, Button +from servo import servo2040 + +""" +Shows how to initialise and read the 6 external +and 2 internal sensors of Servo 2040. + +Press "Boot" to exit the program. +""" + +# Set up the shared analog inputs +sen_adc = Analog(servo2040.SHARED_ADC) +vol_adc = Analog(servo2040.SHARED_ADC, servo2040.VOLTAGE_GAIN) +cur_adc = Analog(servo2040.SHARED_ADC, servo2040.CURRENT_GAIN, + servo2040.SHUNT_RESISTOR, servo2040.CURRENT_OFFSET) + +# Set up the analog multiplexer, including the pin for controlling pull-up/pull-down +mux = AnalogMux(servo2040.ADC_ADDR_0, servo2040.ADC_ADDR_1, servo2040.ADC_ADDR_2, + muxed_pin=Pin(servo2040.SHARED_ADC)) + +# Set up the sensor addresses and have them pulled down by default +sensor_addrs = list(range(servo2040.SENSOR_1_ADDR, servo2040.SENSOR_6_ADDR + 1)) +for addr in sensor_addrs: + mux.configure_pull(addr, Pin.PULL_DOWN) + +# Create the user button +user_sw = Button(servo2040.USER_SW) + + +# Read sensors until the user button is pressed +while user_sw.raw() is not True: + + # Read each sensor in turn and print its voltage + for i in range(len(sensor_addrs)): + mux.select(sensor_addrs[i]) + print("S", i + 1, " = ", round(sen_adc.read_voltage(), 3), sep="", end=", ") + + # Read the voltage sense and print the value + mux.select(servo2040.VOLTAGE_SENSE_ADDR) + print("Voltage =", round(vol_adc.read_voltage(), 4), end=", ") + + # Read the current sense and print the value + mux.select(servo2040.CURRENT_SENSE_ADDR) + print("Current =", round(cur_adc.read_current(), 4)) + + time.sleep(0.5) diff --git a/micropython/examples/motor2040/sensor_feedback.py b/micropython/examples/motor2040/sensor_feedback.py new file mode 100644 index 00000000..c781cc92 --- /dev/null +++ b/micropython/examples/motor2040/sensor_feedback.py @@ -0,0 +1,58 @@ +import time +from machine import Pin +from pimoroni import Analog, AnalogMux, Button +from plasma import WS2812 +from servo import servo2040 + +""" +Show how to read the 6 external sensors and +display their values on the neighbouring LEDs. + +Press "Boot" to exit the program. + +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. +""" + +BRIGHTNESS = 0.4 # The brightness of the LEDs +UPDATES = 50 # How many times to update LEDs and Servos per second + +# Set up the shared analog inputs +sen_adc = Analog(servo2040.SHARED_ADC) + +# Set up the analog multiplexer, including the pin for controlling pull-up/pull-down +mux = AnalogMux(servo2040.ADC_ADDR_0, servo2040.ADC_ADDR_1, servo2040.ADC_ADDR_2, + muxed_pin=Pin(servo2040.SHARED_ADC)) + +# Set up the sensor addresses and have them pulled down by default +sensor_addrs = list(range(servo2040.SENSOR_1_ADDR, servo2040.SENSOR_6_ADDR + 1)) +for addr in sensor_addrs: + mux.configure_pull(addr, Pin.PULL_DOWN) + +# Create the LED bar, using PIO 1 and State Machine 0 +led_bar = WS2812(servo2040.NUM_LEDS, 1, 0, servo2040.LED_DATA) + +# Create the user button +user_sw = Button(servo2040.USER_SW) + +# Start updating the LED bar +led_bar.start() + + +# Read sensors until the user button is pressed +while user_sw.raw() is not True: + + # Read each sensor in turn and print its voltage + for i in range(len(sensor_addrs)): + mux.select(sensor_addrs[i]) + sensor_voltage = sen_adc.read_voltage() + + # Calculate the LED's hue, with Green for high voltages and Blue for low + hue = (2.0 - (sensor_voltage / 3.3)) * 0.333 + led_bar.set_hsv(i, hue, 1.0, BRIGHTNESS) + + print("S", i + 1, " = ", round(sensor_voltage, 3), sep="", end=", ") + print() + + time.sleep(1.0 / UPDATES) diff --git a/micropython/examples/motor2040/simple_easing.py b/micropython/examples/motor2040/simple_easing.py new file mode 100644 index 00000000..54647ebc --- /dev/null +++ b/micropython/examples/motor2040/simple_easing.py @@ -0,0 +1,64 @@ +import time +import math +import random +from pimoroni import Button +from servo import Servo, servo2040 + +""" +An example of how to move a servo smoothly between random positions. + +Press "Boot" to exit the program. +""" + +UPDATES = 50 # How many times to update Servos per second +TIME_FOR_EACH_MOVE = 2 # The time to travel between each random value +UPDATES_PER_MOVE = TIME_FOR_EACH_MOVE * UPDATES + +SERVO_EXTENT = 80 # How far from zero to move the servo +USE_COSINE = True # Whether or not to use a cosine path between values + +# Create a servo on pin 0 +s = Servo(servo2040.SERVO_1) + +# Get the initial value and create a random end value between the extents +start_value = s.mid_value() +end_value = random.uniform(-SERVO_EXTENT, SERVO_EXTENT) + +# Create the user button +user_sw = Button(servo2040.USER_SW) + + +update = 0 + +# Continually move the servo until the user button is pressed +while user_sw.raw() is not True: + + # Calculate how far along this movement to be + percent_along = update / UPDATES_PER_MOVE + + if USE_COSINE: + # Move the servo between values using cosine + s.to_percent(math.cos(percent_along * math.pi), 1.0, -1.0, start_value, end_value) + else: + # Move the servo linearly between values + s.to_percent(percent_along, 0.0, 1.0, start_value, end_value) + + # Print out the value the servo is now at + print("Value = ", round(s.value(), 3), sep="") + + # Move along in time + update += 1 + + # Have we reached the end of this movement? + if update >= UPDATES_PER_MOVE: + # Reset the counter + update = 0 + + # Set the start as the last end and create a new random end value + start_value = end_value + end_value = random.uniform(-SERVO_EXTENT, SERVO_EXTENT) + + time.sleep(1.0 / UPDATES) + +# Disable the servo +s.disable() diff --git a/micropython/examples/motor2040/single_motor.py b/micropython/examples/motor2040/single_motor.py new file mode 100644 index 00000000..3c7e81d4 --- /dev/null +++ b/micropython/examples/motor2040/single_motor.py @@ -0,0 +1,49 @@ +import time +import math +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) + +# Enable the motor (this puts it at the middle) +m.enable() +time.sleep(2) + +# Go to 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) + + +SWEEPS = 3 # How many 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 + +# Do a sine sweep +for j in range(SWEEPS): + for i in range(360): + m.speed(math.sin(math.radians(i))) + time.sleep(0.02) + +# Do a stepped sweep +for j in range(SWEEPS): + for i in range(0, STEPS): + m.to_percent(i, 0, STEPS) + time.sleep(STEPS_INTERVAL) + for i in range(0, STEPS): + m.to_percent(i, STEPS, 0) + time.sleep(STEPS_INTERVAL) + +# Disable the motor +m.disable() diff --git a/micropython/examples/motor2040/turn_off_led.py b/micropython/examples/motor2040/turn_off_led.py new file mode 100644 index 00000000..a52af0d1 --- /dev/null +++ b/micropython/examples/motor2040/turn_off_led.py @@ -0,0 +1,16 @@ +from plasma import WS2812 +from motor import motor2040 + +""" +A simple program that turns off the onboard LED. + +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. +""" + +# Create the LED, using PIO 1 and State Machine 0 +led = WS2812(motor2040.NUM_LEDS, 1, 0, motor2040.LED_DATA) + +# Start updating the LED +led.start() diff --git a/micropython/examples/motor2040/turn_off_motors.py b/micropython/examples/motor2040/turn_off_motors.py new file mode 100644 index 00000000..1a33dd9c --- /dev/null +++ b/micropython/examples/motor2040/turn_off_motors.py @@ -0,0 +1,13 @@ +from motor import Motor, motor2040 + +""" +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) diff --git a/micropython/modules/motor/motor.c b/micropython/modules/motor/motor.c index f5d34174..2a0acc40 100644 --- a/micropython/modules/motor/motor.c +++ b/micropython/modules/motor/motor.c @@ -248,10 +248,10 @@ STATIC const mp_map_elem_t motor_globals_table[] = { { MP_OBJ_NEW_QSTR(MP_QSTR_pico_motor_shim), (mp_obj_t)&pico_motor_shim_user_cmodule }, { MP_OBJ_NEW_QSTR(MP_QSTR_motor2040), (mp_obj_t)&motor2040_user_cmodule }, - // TODO - //{ MP_ROM_QSTR(MP_QSTR_ANGULAR), MP_ROM_INT(0x00) }, - //{ MP_ROM_QSTR(MP_QSTR_LINEAR), MP_ROM_INT(0x01) }, - //{ MP_ROM_QSTR(MP_QSTR_CONTINUOUS), MP_ROM_INT(0x02) }, + { MP_ROM_QSTR(MP_QSTR_NORMAL), MP_ROM_INT(0x00) }, + { MP_ROM_QSTR(MP_QSTR_REVERSED), MP_ROM_INT(0x01) }, + { MP_ROM_QSTR(MP_QSTR_FAST_DECAY), MP_ROM_INT(0x00) }, + { MP_ROM_QSTR(MP_QSTR_SLOW_DECAY), MP_ROM_INT(0x01) }, }; STATIC MP_DEFINE_CONST_DICT(mp_module_motor_globals, motor_globals_table); diff --git a/micropython/modules/motor/motor.cpp b/micropython/modules/motor/motor.cpp index d7a7fd84..88bceecd 100644 --- a/micropython/modules/motor/motor.cpp +++ b/micropython/modules/motor/motor.cpp @@ -28,9 +28,12 @@ void Motor_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind _Motor_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Motor_obj_t); mp_print_str(print, "Motor("); - mp_print_str(print, "pins = "); - mp_obj_print_helper(print, mp_obj_new_int(self->motor->pins().positive), PRINT_REPR); - mp_print_str(print, ", enabled = "); + mp_print_str(print, "pins = ("); + pin_pair pins = self->motor->pins(); + mp_obj_print_helper(print, mp_obj_new_int(pins.positive), PRINT_REPR); + mp_print_str(print, ", "); + mp_obj_print_helper(print, mp_obj_new_int(pins.negative), PRINT_REPR); + mp_print_str(print, "), enabled = "); mp_obj_print_helper(print, self->motor->is_enabled() ? mp_const_true : mp_const_false, PRINT_REPR); mp_print_str(print, ", duty = "); mp_obj_print_helper(print, mp_obj_new_float(self->motor->duty()), PRINT_REPR); @@ -38,6 +41,18 @@ void Motor_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind mp_obj_print_helper(print, mp_obj_new_float(self->motor->speed()), PRINT_REPR); mp_print_str(print, ", freq = "); mp_obj_print_helper(print, mp_obj_new_float(self->motor->frequency()), PRINT_REPR); + if(self->motor->direction() == MotorState::NORMAL) + mp_print_str(print, ", direction = NORMAL"); + else + mp_print_str(print, ", direction = REVERSED"); + mp_print_str(print, ", speed_scale = "); + mp_obj_print_helper(print, mp_obj_new_float(self->motor->speed_scale()), PRINT_REPR); + mp_print_str(print, ", deadzone_percent = "); + mp_obj_print_helper(print, mp_obj_new_float(self->motor->deadzone_percent()), PRINT_REPR); + if(self->motor->decay_mode() == MotorState::SLOW_DECAY) + mp_print_str(print, ", decay_mode = SLOW_DECAY"); + else + mp_print_str(print, ", decay_mode = FAST_DECAY"); mp_print_str(print, ")"); } @@ -47,9 +62,9 @@ void Motor_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind mp_obj_t Motor_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args) { _Motor_obj_t *self = nullptr; - enum { ARG_pin, ARG_calibration, ARG_freq }; + enum { ARG_pins, ARG_calibration, ARG_freq }; static const mp_arg_t allowed_args[] = { - { MP_QSTR_pin, MP_ARG_REQUIRED | MP_ARG_INT }, + { MP_QSTR_pins, MP_ARG_REQUIRED | MP_ARG_OBJ }, { MP_QSTR_calibration, MP_ARG_OBJ, {.u_obj = mp_const_none} }, { MP_QSTR_freq, MP_ARG_OBJ, {.u_obj = mp_const_none} }, }; @@ -58,7 +73,41 @@ mp_obj_t Motor_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, c mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; mp_arg_parse_all_kw_array(n_args, n_kw, all_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); - int pin = args[ARG_pin].u_int; + size_t pin_count = 0; + pin_pair pins; + + // Determine what pair of pins this motor will use + const mp_obj_t object = args[ARG_pins].u_obj; + mp_obj_t *items = nullptr; + if(mp_obj_is_type(object, &mp_type_list)) { + mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); + pin_count = list->len; + items = list->items; + } + else if(mp_obj_is_type(object, &mp_type_tuple)) { + mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); + pin_count = tuple->len; + items = tuple->items; + } + + if(items == nullptr) + mp_raise_TypeError("cannot convert object to a list or tuple of pins"); + else if(pin_count != 2) + mp_raise_TypeError("list or tuple must only contain two integers"); + else { + int pos = mp_obj_get_int(items[0]); + int neg = mp_obj_get_int(items[1]); + if((pos < 0 || pos >= (int)NUM_BANK0_GPIOS) || + (neg < 0 || neg >= (int)NUM_BANK0_GPIOS)) { + mp_raise_ValueError("a pin in the list or tuple is out of range. Expected 0 to 29"); + } + else if(pos == neg) { + mp_raise_ValueError("cannot use the same pin for motor positive and negative"); + } + + pins.positive = (uint8_t)pos; + pins.negative = (uint8_t)neg; + } //motor::Calibration *calib = nullptr; //motor::CalibrationType calibration_type = motor::CalibrationType::ANGULAR; @@ -90,7 +139,7 @@ mp_obj_t Motor_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, c //if(calib != nullptr) // self->motor = new Motor(pin, *calib, freq); //else - self->motor = new Motor2(pin_pair(pin, pin));//TODO, calibration_type, freq); + self->motor = new Motor2(pins);//TODO, calibration_type, freq); self->motor->init(); return MP_OBJ_FROM_PTR(self); @@ -233,6 +282,7 @@ extern mp_obj_t Motor_frequency(size_t n_args, const mp_obj_t *pos_args, mp_map_ float freq = mp_obj_get_float(args[ARG_freq].u_obj); + // TODO confirm frequency range if(!self->motor->frequency(freq)) { mp_raise_ValueError("freq out of range. Expected 10Hz to 350Hz"); //TODO } @@ -494,9 +544,12 @@ void MotorCluster_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind uint8_t motor_count = self->cluster->count(); for(uint8_t motor = 0; motor < motor_count; motor++) { - mp_print_str(print, "\n\t{pins = "); - mp_obj_print_helper(print, mp_obj_new_int(self->cluster->pins(motor).positive), PRINT_REPR); - mp_print_str(print, ", enabled = "); + mp_print_str(print, "\n\t{pins = ("); + pin_pair pins = self->cluster->pins(motor); + mp_obj_print_helper(print, mp_obj_new_int(pins.positive), PRINT_REPR); + mp_print_str(print, ", "); + mp_obj_print_helper(print, mp_obj_new_int(pins.negative), PRINT_REPR); + mp_print_str(print, "), enabled = "); mp_obj_print_helper(print, self->cluster->is_enabled(motor) ? mp_const_true : mp_const_false, PRINT_REPR); mp_print_str(print, ", duty = "); mp_obj_print_helper(print, mp_obj_new_float(self->cluster->duty(motor)), PRINT_REPR); @@ -539,47 +592,79 @@ mp_obj_t MotorCluster_make_new(const mp_obj_type_t *type, size_t n_args, size_t PIO pio = args[ARG_pio].u_int == 0 ? pio0 : pio1; int sm = args[ARG_sm].u_int; - //uint pin_mask = 0; - //bool mask_provided = true; - uint32_t pair_count = 0; - pin_pair* pins = nullptr; + size_t pair_count = 0; + pin_pair *pins = nullptr; - // Determine what pins this cluster will use + // Determine what pair of pins this motor will use const mp_obj_t object = args[ARG_pins].u_obj; - if(mp_obj_is_int(object)) { - //pin_mask = (uint)mp_obj_get_int(object); + mp_obj_t *items = nullptr; + if(mp_obj_is_type(object, &mp_type_list)) { + mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); + pair_count = list->len; + items = list->items; + } + else if(mp_obj_is_type(object, &mp_type_tuple)) { + mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); + pair_count = tuple->len; + items = tuple->items; } - else { - mp_obj_t *items = nullptr; - if(mp_obj_is_type(object, &mp_type_list)) { - mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); - pair_count = list->len; - items = list->items; - } - else if(mp_obj_is_type(object, &mp_type_tuple)) { - mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); - pair_count = tuple->len; - items = tuple->items; - } - if(items == nullptr) - mp_raise_TypeError("cannot convert object to a list or tuple of pins, or a pin mask integer"); - else if(pair_count == 0) - mp_raise_TypeError("list or tuple must contain at least one integer"); + if(items == nullptr) + mp_raise_TypeError("cannot convert object to a list or tuple of pins"); + else if(pair_count == 0) + mp_raise_TypeError("list or tuple must contain at least one pair tuple"); + else { + // Specific check for is a single 2 pin list/tuple was provided + if(pair_count == 2 && mp_obj_is_int(items[0]) && mp_obj_is_int(items[1])) { + pins = new pin_pair[1]; + pair_count = 1; + + int pos = mp_obj_get_int(items[0]); + int neg = mp_obj_get_int(items[1]); + if((pos < 0 || pos >= (int)NUM_BANK0_GPIOS) || + (neg < 0 || neg >= (int)NUM_BANK0_GPIOS)) { + delete[] pins; + mp_raise_ValueError("a pin in the list or tuple is out of range. Expected 0 to 29"); + } + else if(pos == neg) { + delete[] pins; + mp_raise_ValueError("cannot use the same pin for motor positive and negative"); + } + + pins[0].positive = (uint8_t)pos; + pins[0].negative = (uint8_t)neg; + } else { // Create and populate a local array of pins pins = new pin_pair[pair_count]; for(size_t i = 0; i < pair_count; i++) { - int pin = mp_obj_get_int(items[i]); - if(pin < 0 || pin >= (int)NUM_BANK0_GPIOS) { + mp_obj_t obj = items[i]; + if(!mp_obj_is_type(obj, &mp_type_tuple)) { delete[] pins; - mp_raise_ValueError("a pin in the list or tuple is out of range. Expected 0 to 29"); + mp_raise_ValueError("cannot convert item to a pair tuple"); } else { - pins[i] = pin_pair((uint8_t)pin, (uint8_t)pin); //TODO + mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(obj, mp_obj_tuple_t); + if(tuple->len != 2) { + delete[] pins; + mp_raise_ValueError("pair tuple must only contain two integers"); + } + int pos = mp_obj_get_int(tuple->items[0]); + int neg = mp_obj_get_int(tuple->items[1]); + if((pos < 0 || pos >= (int)NUM_BANK0_GPIOS) || + (neg < 0 || neg >= (int)NUM_BANK0_GPIOS)) { + delete[] pins; + mp_raise_ValueError("a pin in the pair tuple is out of range. Expected 0 to 29"); + } + else if(pos == neg) { + delete[] pins; + mp_raise_ValueError("cannot use the same pin for motor positive and negative"); + } + + pins[i].positive = (uint8_t)pos; + pins[i].negative = (uint8_t)neg; } } - //mask_provided = false; } } diff --git a/micropython/modules/motor/motor.h b/micropython/modules/motor/motor.h index 08b499c8..45dd0987 100644 --- a/micropython/modules/motor/motor.h +++ b/micropython/modules/motor/motor.h @@ -1,14 +1,6 @@ // Include MicroPython API. #include "py/runtime.h" -enum { - PICO_MOTOR_SHIM_BUTTON_A = 2, - PICO_MOTOR_SHIM_MOTOR_1P = 6, - PICO_MOTOR_SHIM_MOTOR_1N = 7, - PICO_MOTOR_SHIM_MOTOR_2P = 27, - PICO_MOTOR_SHIM_MOTOR_2N = 26, -}; - /***** Extern of Class Definition *****/ extern const mp_obj_type_t Motor_type; extern const mp_obj_type_t MotorCluster_type;