Finished MP examples

motor-and-encoder
ZodiusInfuser 2022-04-26 12:52:28 +01:00
rodzic 0efe210c4c
commit 56451bff30
8 zmienionych plików z 210 dodań i 194 usunięć

Wyświetl plik

@ -6,13 +6,10 @@
- [Motor Cluster](#motor-cluster)
- [Motor Wave](#motor-wave)
- [Stop Motors](#stop-motors)
- [Encoder Examples](#encoder-examples)
- [Read Encoders](#read-encoders)
- [Motor Profiler](#motor-profiler)
- [Function Examples](#function-examples)
- [Read Sensors](#read-sensors)
- [Sensor Feedback](#sensor-feedback)
- [Current Meter](#current-meter)
- [Read Encoders](#read-encoders)
- [Motor Profiler](#motor-profiler)
- [LED Rainbow](#led-rainbow)
- [Turn Off LEDs](#turn-off-leds)
- [Control Examples](#control-examples)
@ -21,6 +18,7 @@
- [Position on Velocity Control](#position-on-velocity-control)
- [Reactive Encoder](#reactive-encoder)
- [Quad Position Wave](#quad-position-wave)
- [Quad Velocity Sequence](#quad-velocity-sequence)
- [Tuning Examples](#tuning-examples)
- [Position Tuning](#position-tuning)
- [Velocity Tuning](#velocity-tuning)
@ -59,7 +57,13 @@ An example of applying a wave pattern to a group of motors and the LEDs.
A simple program that stops the motors.
## Encoder Examples
## Function Examples
### Read Sensors
[read_sensors.py](read_sensors.py)
Shows how to initialise and read the 2 external and 6 internal sensors of Motor 2040.
### Read Encoders
[read_encoders.py](read_encoders.py)
@ -74,26 +78,6 @@ A program that profiles the speed of a motor across its PWM
duty cycle range using the attached encoder for feedback.
## Function Examples
### Read Sensors
[read_sensors.py](read_sensors.py)
TODO
Shows how to initialise and read the 6 external and 2 internal sensors of Motor 2040.
### Sensor Feedback
[sensor_feedback.py](sensor_feedback.py)
TODO
Show how to read the 6 external sensors and display their values on the neighbouring LEDs.
### Current Meter
[current_meter.py](current_meter.py)
TODO
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)
@ -135,7 +119,12 @@ A demonstration of how a motor with an encoder can be used as a programmable rot
### Quad Position Wave
[quad_position_wave.py](quad_position_wave.py)
An demonstration of driving all four of Motor 2040's motor outputs between positions, with the help of their attached encoders and PID control.
A demonstration of driving all four of Motor 2040's motor outputs between positions, with the help of their attached encoders and PID control.
### Quad Velocity Sequence
[quad_velocity_sequence.py](quad_velocity_sequence.py)
A demonstration of driving all four of Motor 2040's motor outputs through a sequence of velocities, with the help of their attached encoders and PID control.
## Tuning Examples

Wyświetl plik

@ -1,90 +0,0 @@
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()

Wyświetl plik

@ -7,7 +7,7 @@ from encoder import Encoder, MMME_CPR
from pimoroni import Button, PID, REVERSED_DIR
"""
An demonstration of driving all four of Motor 2040's motor outputs between
A demonstration of driving all four of Motor 2040's motor outputs between
positions, with the help of their attached encoders and PID control.
Press "Boot" to exit the program.

Wyświetl plik

@ -0,0 +1,162 @@
import gc
import time
from motor import Motor, motor2040
from encoder import Encoder, MMME_CPR
from pimoroni import Button, PID, REVERSED_DIR
"""
A demonstration of driving all four of Motor 2040's motor outputs through a
sequence of velocities, with the help of their attached encoders and PID control.
Press "Boot" to exit the program.
"""
GEAR_RATIO = 50 # The gear ratio of the motors
COUNTS_PER_REV = MMME_CPR * GEAR_RATIO # The counts per revolution of each motor's output shaft
SPEED_SCALE = 5.4 # The scaling to apply to each 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 = 2 # The time to travel between each 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)
# 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
# Free up hardware resources ahead of creating a new Encoder
gc.collect()
# Create a list of motors with a given speed scale
MOTOR_PINS = [motor2040.MOTOR_A, motor2040.MOTOR_B, motor2040.MOTOR_C, motor2040.MOTOR_D]
motors = [Motor(pins, speed_scale=SPEED_SCALE) for pins in MOTOR_PINS]
# Create a list of encoders, using PIO 0, with the given counts per revolution
ENCODER_PINS = [motor2040.ENCODER_A, motor2040.ENCODER_B, motor2040.ENCODER_C, motor2040.ENCODER_D]
ENCODER_NAMES = ["RR", "RL", "FL", "FR"]
encoders = [Encoder(0, i, ENCODER_PINS[i], counts_per_rev=COUNTS_PER_REV, count_microsteps=True) for i in range(motor2040.NUM_MOTORS)]
# Wheel friendly names
FL = 2
FR = 3
RL = 1
RR = 0
# Reverse the direction of the B and D motors and encoders
motors[FL].direction(REVERSED_DIR)
motors[RL].direction(REVERSED_DIR)
encoders[FL].direction(REVERSED_DIR)
encoders[RL].direction(REVERSED_DIR)
# Create the user button
user_sw = Button(motor2040.USER_SW)
# Create PID objects for position control
vel_pids = [PID(VEL_KP, VEL_KI, VEL_KD, UPDATE_RATE) for i in range(motor2040.NUM_MOTORS)]
# Enable the motor to get started
for m in motors:
m.enable()
update = 0
print_count = 0
# Helper functions for driving in common directions
def drive_forward(speed):
vel_pids[FL].setpoint = speed
vel_pids[FR].setpoint = speed
vel_pids[RL].setpoint = speed
vel_pids[RR].setpoint = speed
def turn_right(speed):
vel_pids[FL].setpoint = speed
vel_pids[FR].setpoint = -speed
vel_pids[RL].setpoint = speed
vel_pids[RR].setpoint = -speed
def strafe_right(speed):
vel_pids[FL].setpoint = speed
vel_pids[FR].setpoint = -speed
vel_pids[RL].setpoint = -speed
vel_pids[RR].setpoint = speed
def stop():
vel_pids[FL].setpoint = 0
vel_pids[FR].setpoint = 0
vel_pids[RL].setpoint = 0
vel_pids[RR].setpoint = 0
sequence = 0
captures = [None] * motor2040.NUM_MOTORS
# Continually move the motor until the user button is pressed
while user_sw.raw() is not True:
# Capture the state of all the encoders
for i in range(motor2040.NUM_MOTORS):
captures[i] = encoders[i].capture()
# Calculate how far along this movement to be
percent_along = min(update / UPDATES_PER_MOVE, 1.0)
for i in range(motor2040.NUM_MOTORS):
# Calculate the acceleration to apply to the motor to move it closer to the velocity setpoint
accel = vel_pids[i].calculate(captures[i].revolutions_per_second)
# Accelerate or decelerate the motor
motors[i].speed(motors[i].speed() + (accel * UPDATE_RATE))
# Print out the current motor values, but only on every multiple
if print_count == 0:
for i in range(len(motors)):
print(ENCODER_NAMES[i], "=", captures[i].revolutions_per_second, end=", ")
print()
# 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
# Move on to the next part of the sequence
sequence += 1
# Loop the sequence back around
if sequence >= 7:
sequence = 0
# Set the motor speeds, based on the sequence
if sequence == 0:
drive_forward(1.0)
elif sequence == 1:
drive_forward(-1.0)
elif sequence == 2:
turn_right(1.0)
elif sequence == 3:
turn_right(-1.0)
elif sequence == 4:
strafe_right(1.0)
elif sequence == 5:
strafe_right(-1.0)
elif sequence == 6:
stop()
time.sleep(UPDATE_RATE)
# Stop all the motors
for m in motors:
m.disable()

Wyświetl plik

@ -1,32 +1,35 @@
import time
from machine import Pin
from pimoroni import Analog, AnalogMux, Button
from servo import servo2040
from motor import motor2040
"""
Shows how to initialise and read the 6 external
and 2 internal sensors of Servo 2040.
Shows how to initialise and read the 2 external
and 6 internal sensors of Motor 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)
sen_adc = Analog(motor2040.SHARED_ADC)
vol_adc = Analog(motor2040.SHARED_ADC, motor2040.VOLTAGE_GAIN)
cur_adc = Analog(motor2040.SHARED_ADC, motor2040.CURRENT_GAIN,
motor2040.SHUNT_RESISTOR, motor2040.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))
mux = AnalogMux(motor2040.ADC_ADDR_0, motor2040.ADC_ADDR_1, motor2040.ADC_ADDR_2,
muxed_pin=Pin(motor2040.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))
sensor_addrs = list(range(motor2040.SENSOR_1_ADDR, motor2040.SENSOR_2_ADDR + 1))
for addr in sensor_addrs:
mux.configure_pull(addr, Pin.PULL_DOWN)
# Set up the pull-up for the fault sense
mux.configure_pull(motor2040.FAULT_SENSE_ADDR, Pin.PULL_UP)
# Create the user button
user_sw = Button(servo2040.USER_SW)
user_sw = Button(motor2040.USER_SW)
# Read sensors until the user button is pressed
@ -38,11 +41,16 @@ while user_sw.raw() is not True:
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)
mux.select(motor2040.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))
# Read the current sense's of each motor and print the value
for i in range(motor2040.NUM_MOTORS):
mux.select(i + motor2040.CURRENT_SENSE_A_ADDR)
print("C", i + 1, "=", cur_adc.read_current(), sep="", end=", ")
# Read the fault sense and print the value
mux.select(motor2040.FAULT_SENSE_ADDR)
print("Fault =", not mux.read())
time.sleep(0.5)

Wyświetl plik

@ -1,58 +0,0 @@
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)

Wyświetl plik

@ -177,10 +177,9 @@ typedef struct _mp_obj_float_t {
mp_obj_base_t base;
mp_float_t value;
} mp_obj_float_t;
//TODO confirm below numbers are correct
mp_obj_float_t motor2040_shunt_resistor = {{&mp_type_float}, 0.47f};
mp_obj_float_t motor2040_voltage_gain = {{&mp_type_float}, 3.9f / 13.9f};
mp_obj_float_t motor2040_current_offset = {{&mp_type_float}, -0.02f};
mp_obj_float_t motor2040_current_offset = {{&mp_type_float}, -0.005f};
/***** Globals Table *****/
@ -231,7 +230,7 @@ STATIC const mp_rom_map_elem_t motor2040_globals_table[] = {
{ MP_ROM_QSTR(MP_QSTR_SENSOR_2_ADDR), MP_ROM_INT(0b111) },
{ MP_ROM_QSTR(MP_QSTR_NUM_SENSORS), MP_ROM_INT(2) },
{ MP_ROM_QSTR(MP_QSTR_SHUNT_RESISTOR), MP_ROM_PTR(&motor2040_shunt_resistor) },
{ MP_ROM_QSTR(MP_QSTR_CURRENT_GAIN), MP_ROM_INT(5) },
{ MP_ROM_QSTR(MP_QSTR_CURRENT_GAIN), MP_ROM_INT(1) },
{ MP_ROM_QSTR(MP_QSTR_VOLTAGE_GAIN), MP_ROM_PTR(&motor2040_voltage_gain) },
{ MP_ROM_QSTR(MP_QSTR_CURRENT_OFFSET), MP_ROM_PTR(&motor2040_current_offset) },
};

Wyświetl plik

@ -79,6 +79,12 @@ class AnalogMux:
else:
self.pulls[address] = pull
def read(self):
if self.muxed_pin is not None:
return self.muxed_pin.value()
else:
raise RuntimeError("there is no muxed pin assigned to this mux")
class Button:
def __init__(self, button, invert=True, repeat_time=200, hold_time=1000):