Encoder driver finalising

motor-and-encoder
ZodiusInfuser 2022-04-25 12:28:42 +01:00
rodzic 2eb6a0cf3e
commit 7608e3f293
19 zmienionych plików z 185 dodań i 47 usunięć

Wyświetl plik

@ -281,11 +281,11 @@ void Encoder::direction(Direction direction) {
enc_direction = direction;
}
float Encoder::counts_per_revolution() const {
float Encoder::counts_per_rev() const {
return enc_counts_per_rev;
}
void Encoder::counts_per_revolution(float counts_per_rev) {
void Encoder::counts_per_rev(float counts_per_rev) {
enc_counts_per_rev = MAX(counts_per_rev, FLT_EPSILON);
}

Wyświetl plik

@ -125,7 +125,7 @@ namespace encoder {
volatile StepDir step_dir = NO_DIR;
int32_t last_count = 0;
int32_t last_capture_count = 0;
int32_t last_capture_count = 0;
bool initialised = false;
@ -178,8 +178,8 @@ namespace encoder {
Direction direction() const;
void direction(Direction direction);
float counts_per_revolution() const;
void counts_per_revolution(float counts_per_rev);
float counts_per_rev() const;
void counts_per_rev(float counts_per_rev);
Capture capture();

Wyświetl plik

@ -5,9 +5,9 @@
- [Multiple Motors](#multiple-motors)
- [Motor Cluster](#motor-cluster)
- [Motor Wave](#motor-wave)
- [Turn Off Motors](#turn-off-motors)
- [Stop Motors](#stop-motors)
- [Encoder Examples](#encoder-examples)
- [Motor Angles](#motor-angles)
- [Read Encoders](#read-encoders)
- [Motor Profiler](#motor-profiler)
- [Function Examples](#function-examples)
- [Read Sensors](#read-sensors)
@ -20,6 +20,7 @@
- [Velocity Control](#velocity-control)
- [Position on Velocity Control](#position-on-velocity-control)
- [Reactive Encoder](#reactive-encoder)
- [Quad Position Wave](#quad-position-wave)
- [Tuning Examples](#tuning-examples)
- [Position Tuning](#position-tuning)
- [Velocity Tuning](#velocity-tuning)
@ -52,16 +53,16 @@ Demonstrates how to create a MotorCluster object to control multiple motors at o
An example of applying a wave pattern to a group of motors and the LEDs.
### Turn Off Motors
[turn_off_motors.py](turn_off_motors.py)
### Stop Motors
[stop_motors.py](stop_motors.py)
A simple program that turns off the motors.
A simple program that stops the motors.
## Encoder Examples
### Motor Angles
[motor_angles.py](motor_angles.py)
### Read Encoders
[read_encoders.py](read_encoders.py)
Demonstrates how to read the angles of Motor 2040's four encoders.
@ -131,6 +132,12 @@ An example of how to move a motor smoothly between random positions, with veloci
A demonstration of how a motor with an encoder can be used as a programmable rotary encoder for user input, with force-feedback for arbitrary detents and end stops.
### 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.
## Tuning Examples
### Position Tuning

Wyświetl plik

@ -2,6 +2,7 @@ import gc
import time
from motor import Motor, motor2040
from encoder import Encoder, MMME_CPR
from pimoroni import NORMAL_DIR # , REVERSED_DIR
"""
A program that profiles the speed of a motor across its PWM
@ -13,9 +14,8 @@ 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
DIRECTION = NORMAL_DIR # The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1)
SPEED_SCALE = 5.4 # The scaling to apply to the motor's speed. Set this to the maximum measured speed
DUTY_STEPS = 100 # How many duty cycle steps to sample the speed of
SETTLE_TIME = 0.1 # How long to wait after changing motor duty cycle

Wyświetl plik

@ -2,9 +2,9 @@ import gc
import time
import math
import random
from pimoroni import Button, PID
from motor import Motor, motor2040
from encoder import Encoder, MMME_CPR
from pimoroni import Button, PID, NORMAL_DIR # , REVERSED_DIR
"""
An example of how to move a motor smoothly between random positions,
@ -18,7 +18,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)
DIRECTION = NORMAL_DIR # The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1)
SPEED_SCALE = 5.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

Wyświetl plik

@ -2,9 +2,9 @@ import gc
import time
import math
import random
from pimoroni import Button, PID
from motor import Motor, motor2040
from encoder import Encoder, MMME_CPR
from pimoroni import Button, PID, NORMAL_DIR # , REVERSED_DIR
"""
An example of how to move a motor smoothly between random positions,
@ -18,7 +18,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)
DIRECTION = NORMAL_DIR # The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1)
SPEED_SCALE = 5.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

Wyświetl plik

@ -1,8 +1,8 @@
import gc
import time
from pimoroni import Button, PID
from motor import Motor, motor2040
from encoder import Encoder, MMME_CPR
from pimoroni import Button, PID, NORMAL_DIR # , REVERSED_DIR
"""
A program to aid in the discovery and tuning of motor PID
@ -18,7 +18,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)
DIRECTION = NORMAL_DIR # The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1)
SPEED_SCALE = 5.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

Wyświetl plik

@ -1,8 +1,8 @@
import gc
import time
from pimoroni import Button, PID
from motor import Motor, motor2040
from encoder import Encoder, MMME_CPR
from pimoroni import Button, PID, NORMAL_DIR # , REVERSED_DIR
"""
A program to aid in the discovery and tuning of motor PID
@ -18,7 +18,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)
DIRECTION = NORMAL_DIR # The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1)
SPEED_SCALE = 5.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

Wyświetl plik

@ -0,0 +1,130 @@
import gc
import time
import math
from plasma import WS2812
from motor import Motor, motor2040
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
positions, 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)
# LED constant
BRIGHTNESS = 0.4 # The brightness of the LEDs
# 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
# 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, deadzone=0.05) 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 = ["A", "B", "C", "D"]
encoders = [Encoder(0, i, ENCODER_PINS[i], counts_per_rev=COUNTS_PER_REV, count_microsteps=True) for i in range(motor2040.NUM_MOTORS)]
# Reverse the direction of the B and D motors and encoders
motors[1].direction(REVERSED_DIR)
motors[3].direction(REVERSED_DIR)
encoders[1].direction(REVERSED_DIR)
encoders[3].direction(REVERSED_DIR)
# 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)
# Create PID objects for position control
pos_pids = [PID(POS_KP, POS_KI, POS_KD, UPDATE_RATE) for i in range(motor2040.NUM_MOTORS)]
# Start updating the LED
led.start()
# Enable the motor to get started
for m in motors:
m.enable()
update = 0
print_count = 0
# Set the initial and end values
start_value = 0.0
end_value = 270.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):
# Move the motor between values using cosine
pos_pids[i].setpoint = (((-math.cos(percent_along * math.pi) + 1.0) / 2.0) * (end_value - start_value)) + start_value
# Calculate the velocity to move the motor closer to the position setpoint
vel = pos_pids[i].calculate(captures[i].degrees, captures[i].degrees_per_second)
# Set the new motor driving speed
motors[i].speed(vel)
# Update the LED
led.set_hsv(0, percent_along, 1.0, BRIGHTNESS)
# Print out the current motor values and their setpoints, but only on every multiple
if print_count == 0:
for i in range(len(motors)):
print(ENCODER_NAMES[i], "=", captures[i].degrees, 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
# Swap the start and end values
temp = start_value
start_value = end_value
end_value = temp
time.sleep(UPDATE_RATE)
# Stop all the motors
for m in motors:
m.disable()
# Turn off the LED bar
led.clear()

Wyświetl plik

@ -1,9 +1,9 @@
import gc
import time
from pimoroni import Button, PID
from plasma import WS2812
from motor import Motor, motor2040
from encoder import Encoder, MMME_CPR
from pimoroni import Button, PID, NORMAL_DIR # , REVERSED_DIR
"""
A demonstration of how a motor with an encoder can be used
@ -22,7 +22,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)
DIRECTION = NORMAL_DIR # The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1)
SPEED_SCALE = 5.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

Wyświetl plik

@ -1,9 +1,8 @@
import gc
import time
from pimoroni import Button
from motor import motor2040
from encoder import Encoder, MMME_CPR
# from encoder import REVERSED
from pimoroni import Button # , REVERSED_DIR
"""
Demonstrates how to read the angles of Motor 2040's four encoders.
@ -26,10 +25,10 @@ encoders = [Encoder(0, i, ENCODER_PINS[i], counts_per_rev=COUNTS_PER_REV, count_
# Uncomment the below lines (and the top import) to
# reverse the counting direction of an encoder
# encoders[0].direction(REVERSED)
# encoders[1].direction(REVERSED)
# encoders[2].direction(REVERSED)
# encoders[3].direction(REVERSED)
# encoders[0].direction(REVERSED_DIR)
# encoders[1].direction(REVERSED_DIR)
# encoders[2].direction(REVERSED_DIR)
# encoders[3].direction(REVERSED_DIR)
# Create the user button
user_sw = Button(motor2040.USER_SW)

Wyświetl plik

@ -1,7 +1,7 @@
from motor import Motor, motor2040
"""
A simple program that turns off the motors.
A simple program that stops the motors.
"""
# Create four motor objects.

Wyświetl plik

@ -2,9 +2,9 @@ import gc
import time
import math
import random
from pimoroni import Button, PID
from motor import Motor, motor2040
from encoder import Encoder, MMME_CPR
from pimoroni import Button, PID, NORMAL_DIR # , REVERSED_DIR
"""
An example of how to drive a motor smoothly between random speeds,
@ -18,7 +18,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)
DIRECTION = NORMAL_DIR # The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1)
SPEED_SCALE = 5.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

Wyświetl plik

@ -1,8 +1,8 @@
import gc
import time
from pimoroni import Button, PID
from motor import Motor, motor2040
from encoder import Encoder, MMME_CPR
from pimoroni import Button, PID, NORMAL_DIR # , REVERSED_DIR
"""
A program to aid in the discovery and tuning of motor PID
@ -18,7 +18,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)
DIRECTION = NORMAL_DIR # The direction to spin the motor in. NORMAL_DIR (0), REVERSED_DIR (1)
SPEED_SCALE = 5.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

Wyświetl plik

@ -15,7 +15,7 @@ MP_DEFINE_CONST_FUN_OBJ_1(Encoder_revolutions_obj, Encoder_revolutions);
MP_DEFINE_CONST_FUN_OBJ_1(Encoder_degrees_obj, Encoder_degrees);
MP_DEFINE_CONST_FUN_OBJ_1(Encoder_radians_obj, Encoder_radians);
MP_DEFINE_CONST_FUN_OBJ_KW(Encoder_direction_obj, 1, Encoder_direction);
MP_DEFINE_CONST_FUN_OBJ_KW(Encoder_counts_per_revolution_obj, 1, Encoder_counts_per_revolution);
MP_DEFINE_CONST_FUN_OBJ_KW(Encoder_counts_per_rev_obj, 1, Encoder_counts_per_rev);
MP_DEFINE_CONST_FUN_OBJ_1(Encoder_capture_obj, Encoder_capture);
/***** Binding of Methods *****/
@ -33,7 +33,7 @@ STATIC const mp_rom_map_elem_t Encoder_locals_dict_table[] = {
{ MP_ROM_QSTR(MP_QSTR_degrees), MP_ROM_PTR(&Encoder_degrees_obj) },
{ MP_ROM_QSTR(MP_QSTR_radians), MP_ROM_PTR(&Encoder_radians_obj) },
{ MP_ROM_QSTR(MP_QSTR_direction), MP_ROM_PTR(&Encoder_direction_obj) },
{ MP_ROM_QSTR(MP_QSTR_counts_per_revolution), MP_ROM_PTR(&Encoder_counts_per_revolution_obj) },
{ MP_ROM_QSTR(MP_QSTR_counts_per_rev), MP_ROM_PTR(&Encoder_counts_per_rev_obj) },
{ MP_ROM_QSTR(MP_QSTR_capture), MP_ROM_PTR(&Encoder_capture_obj) },
};
@ -53,8 +53,6 @@ STATIC const mp_map_elem_t encoder_globals_table[] = {
{ MP_OBJ_NEW_QSTR(MP_QSTR___name__), MP_OBJ_NEW_QSTR(MP_QSTR_encoder) },
{ MP_OBJ_NEW_QSTR(MP_QSTR_Encoder), (mp_obj_t)&Encoder_type },
{ 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_MMME_CPR), MP_ROM_INT(12) },
{ MP_ROM_QSTR(MP_QSTR_ROTARY_CPR), MP_ROM_INT(24) },
};

Wyświetl plik

@ -46,7 +46,7 @@ void Encoder_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t ki
mp_print_str(print, ", direction = REVERSED");
mp_print_str(print, ", counts_per_rev = ");
mp_obj_print_helper(print, mp_obj_new_float(self->encoder->counts_per_revolution()), PRINT_REPR);
mp_obj_print_helper(print, mp_obj_new_float(self->encoder->counts_per_rev()), PRINT_REPR);
mp_print_str(print, ")");
}
@ -75,7 +75,7 @@ mp_obj_t Encoder_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw,
if(pio_int < 0 || pio_int > (int)NUM_PIOS) {
mp_raise_ValueError("pio out of range. Expected 0 to 1");
}
PIO pio = pio_int ? pio0 : pio1;
PIO pio = pio_int == 0 ? pio0 : pio1;
int sm = args[ARG_sm].u_int;
if(sm < 0 || sm > (int)NUM_PIO_STATE_MACHINES) {
@ -264,7 +264,7 @@ extern mp_obj_t Encoder_direction(size_t n_args, const mp_obj_t *pos_args, mp_ma
}
}
extern mp_obj_t Encoder_counts_per_revolution(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
extern mp_obj_t Encoder_counts_per_rev(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
if(n_args <= 1) {
enum { ARG_self };
static const mp_arg_t allowed_args[] = {
@ -277,7 +277,7 @@ extern mp_obj_t Encoder_counts_per_revolution(size_t n_args, const mp_obj_t *pos
_Encoder_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Encoder_obj_t);
return mp_obj_new_float(self->encoder->counts_per_revolution());
return mp_obj_new_float(self->encoder->counts_per_rev());
}
else {
enum { ARG_self, ARG_counts_per_rev };
@ -296,7 +296,7 @@ extern mp_obj_t Encoder_counts_per_revolution(size_t n_args, const mp_obj_t *pos
if(counts_per_rev < FLT_EPSILON) {
mp_raise_ValueError("counts_per_rev out of range. Expected greater than 0.0");
}
self->encoder->counts_per_revolution(counts_per_rev);
self->encoder->counts_per_rev(counts_per_rev);
return mp_const_none;
}
}

Wyświetl plik

@ -20,5 +20,5 @@ extern mp_obj_t Encoder_revolutions(mp_obj_t self_in);
extern mp_obj_t Encoder_degrees(mp_obj_t self_in);
extern mp_obj_t Encoder_radians(mp_obj_t self_in);
extern mp_obj_t Encoder_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t Encoder_counts_per_revolution(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t Encoder_counts_per_rev(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t Encoder_capture(mp_obj_t self_in);

Wyświetl plik

@ -618,7 +618,7 @@ mp_obj_t MotorCluster_make_new(const mp_obj_type_t *type, size_t n_args, size_t
if(pio_int < 0 || pio_int > (int)NUM_PIOS) {
mp_raise_ValueError("pio out of range. Expected 0 to 1");
}
PIO pio = pio_int ? pio0 : pio1;
PIO pio = pio_int == 0 ? pio0 : pio1;
int sm = args[ARG_sm].u_int;
if(sm < 0 || sm > (int)NUM_PIO_STATE_MACHINES) {

Wyświetl plik

@ -6,6 +6,10 @@ BREAKOUT_GARDEN_I2C_PINS = {"sda": 4, "scl": 5}
PICO_EXPLORER_I2C_PINS = {"sda": 20, "scl": 21}
HEADER_I2C_PINS = {"sda": 20, "scl": 21}
# Motor and encoder directions
NORMAL_DIR = 0x00
REVERSED_DIR = 0x01
class Analog:
def __init__(self, pin, amplifier_gain=1, resistor=0, offset=0):