diff --git a/micropython/examples/motor2040/motor_profiler.py b/micropython/examples/motor2040/motor_profiler.py index d930e0d4..e25ec6fb 100644 --- a/micropython/examples/motor2040/motor_profiler.py +++ b/micropython/examples/motor2040/motor_profiler.py @@ -41,19 +41,19 @@ def profile_at_duty(duty): time.sleep(SETTLE_TIME) # Perform a dummy capture to clear the encoder - enc.take_snapshot() + enc.capture() # Wait for the capture time to pass time.sleep(CAPTURE_TIME) # Perform a capture and read the measured speed - capture = enc.take_snapshot() - measured_speed = capture.revolutions_per_second() + capture = enc.capture() + measured_speed = capture.revolutions_per_second # These are some alternate speed measurements from the encoder - # measured_speed = capture.revolutions_per_minute() - # measured_speed = capture.degrees_per_second() - # measured_speed = capture.radians_per_second() + # measured_speed = capture.revolutions_per_minute + # measured_speed = capture.degrees_per_second + # measured_speed = capture.radians_per_second # Print out the expected and measured speeds, as well as their difference print("Duty = ", m.duty(), ", Expected = ", m.speed(), ", Measured = ", measured_speed, ", Diff = ", m.speed() - measured_speed, sep="") diff --git a/micropython/examples/motor2040/position_control.py b/micropython/examples/motor2040/position_control.py new file mode 100644 index 00000000..4fc99859 --- /dev/null +++ b/micropython/examples/motor2040/position_control.py @@ -0,0 +1,106 @@ +import gc +import time +import math +import random +from pimoroni import Button +from motor import Motor, motor2040 +from encoder import Encoder, MMME_CPR + +""" +A program that profiles the speed of a motor across its PWM +duty cycle range using the attached encoder for feedback. +""" + +MOTOR_PINS = motor2040.MOTOR_A # The pins of the motor being profiled +ENCODER_PINS = motor2040.ENCODER_A # The pins of the encoder attached to the profiled motor +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 + +UPDATES = 100 # How many times to update the motor per second +TIME_FOR_EACH_MOVE = 1 # The time to travel between each random value +UPDATES_PER_MOVE = TIME_FOR_EACH_MOVE * UPDATES + +MOTOR_EXTENT = 180 # How far from zero to move the motor +USE_COSINE = True # Whether or not to use a cosine path between values + +# Free up hardware resources ahead of creating a new Encoder +gc.collect() + +# Create a motor and set its speed scale +m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE, deadzone=0.05) + +# Create an encoder, using PIO 0 and State Machine 0 +enc = Encoder(0, 0, ENCODER_PINS, direction=DIRECTION, counts_per_rev=COUNTS_PER_REV, count_microsteps=True) + + +# Enable the motor to get started +m.enable() + + +# Set th initial value and create a random end value between the extents +start_value = 0.0 +end_value = 180.0#random.uniform(-MOTOR_EXTENT, MOTOR_EXTENT) + +# Create the user button +user_sw = Button(motor2040.USER_SW) + + +update = 0 +target = 0 +error_sum = 0 + +# Values for 50Hz update +# kP = 0.08 +# kI = 0.000 +# kD = 0.06 + +# Values for 100Hz update +kP = 0.14 +kI = 0.000 +kD = 0.2 + +# 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 = min(update / UPDATES_PER_MOVE, 1.0) + + if USE_COSINE: + # Move the motor between values using cosine + target = (((-math.cos(percent_along * math.pi) + 1.0) / 2.0) * (end_value - start_value)) + start_value; + else: + # Move the motor linearly between values + target = (percent_along * (end_value - start_value)) + start_value; + + capture = enc.capture() + error = target - capture.degrees + error_sum += (kI * error) + motor_speed = (error * kP) + error_sum - (kD * capture.degrees_delta) + + m.speed(motor_speed) + + # Print out the target value the motor will move towards + print("C=", round(capture.degrees, 3), ", T=", round(target, 3), ", S=", round(motor_speed, 3),sep="") + + # Move along in time + update += 1 + + # Have we reached the end of this movement? + if update >= UPDATES_PER_MOVE: + + #if update >= UPDATES_PER_MOVE + 10: + # 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(-MOTOR_EXTENT, MOTOR_EXTENT) + + time.sleep(1.0 / UPDATES) + +# Disable the servo +m.disable() \ No newline at end of file diff --git a/micropython/examples/motor2040/tuning.py b/micropython/examples/motor2040/tuning.py new file mode 100644 index 00000000..1d53b95b --- /dev/null +++ b/micropython/examples/motor2040/tuning.py @@ -0,0 +1,89 @@ +import gc +import time +import math +from pimoroni import Button +from motor import Motor, motor2040 +from encoder import Encoder, MMME_CPR + +""" +A program that profiles the speed of a motor across its PWM +duty cycle range using the attached encoder for feedback. +""" + +MOTOR_PINS = motor2040.MOTOR_A # The pins of the motor being profiled +ENCODER_PINS = motor2040.ENCODER_A # The pins of the encoder attached to the profiled motor +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 + +UPDATES = 100 # How many times to update the motor per second +TIME_FOR_EACH_MOVE = 0.25 # The time to travel between each random value +UPDATES_PER_MOVE = TIME_FOR_EACH_MOVE * UPDATES +FURTHER_DELAY = 2 # The time to travel between each random value +FURTHER_UPDATES = FURTHER_DELAY * UPDATES + +MOTOR_EXTENT = 90 # How far from zero to move the motor + +# Free up hardware resources ahead of creating a new Encoder +gc.collect() + +# Create a motor and set its speed scale +m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE, deadzone=0.1) + +# Create an encoder, using PIO 0 and State Machine 0 +enc = Encoder(0, 0, ENCODER_PINS, direction=DIRECTION, counts_per_rev=COUNTS_PER_REV, count_microsteps=True) + +# Create the user button +user_sw = Button(motor2040.USER_SW) + + +update = 0 +target = MOTOR_EXTENT +error_sum = 0 + +# Values for 50Hz update +# kP = 0.08 +# kI = 0.000 +# kD = 0.06 + +# Values for 100Hz update +kP = 0.14 +kI = 0.000 +kD = 0.2 + +# Enable the motor to get started +m.enable() + +last_angle = 0 + +# Continually move the servo until the user button is pressed +while user_sw.raw() is not True: + + capture = enc.capture() + error = target - capture.degrees + error_sum += (kI * error) + motor_speed = (error * kP) + error_sum - (kD * capture.degrees_delta) + + m.speed(motor_speed) + + if update < UPDATES_PER_MOVE: + # Print out the target value the motor will move towards + print("Current = ", round(capture.degrees, 3), ", Target = ", round(target, 3), ", Speed x10= ", round(m.speed() * 10, 3),sep="") + + # Move along in time + update += 1 + + # Have we reached the end of this movement? + if update >= UPDATES_PER_MOVE + FURTHER_UPDATES: + # Reset the counter + update = 0 + + target = -target + + time.sleep(1.0 / UPDATES) + +# Disable the servo +m.disable() \ No newline at end of file diff --git a/micropython/modules/encoder/encoder.c b/micropython/modules/encoder/encoder.c index 2cd19741..1a2afdc1 100644 --- a/micropython/modules/encoder/encoder.c +++ b/micropython/modules/encoder/encoder.c @@ -32,6 +32,7 @@ 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_1(Encoder_take_snapshot_obj, Encoder_take_snapshot); +MP_DEFINE_CONST_FUN_OBJ_1(Encoder_capture_obj, Encoder_capture); /***** Binding of Methods *****/ STATIC const mp_rom_map_elem_t Snapshot_locals_dict_table[] = { @@ -67,6 +68,7 @@ STATIC const mp_rom_map_elem_t Encoder_locals_dict_table[] = { { 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_take_snapshot), MP_ROM_PTR(&Encoder_take_snapshot_obj) }, + { MP_ROM_QSTR(MP_QSTR_capture), MP_ROM_PTR(&Encoder_capture_obj) }, }; STATIC MP_DEFINE_CONST_DICT(Snapshot_locals_dict, Snapshot_locals_dict_table); diff --git a/micropython/modules/encoder/encoder.cpp b/micropython/modules/encoder/encoder.cpp index ee36d433..5ee3ea0d 100644 --- a/micropython/modules/encoder/encoder.cpp +++ b/micropython/modules/encoder/encoder.cpp @@ -416,4 +416,44 @@ extern mp_obj_t Encoder_take_snapshot(mp_obj_t self_in) { snap->snapshot = self->encoder->take_snapshot(); return MP_OBJ_FROM_PTR(snap); } + +extern mp_obj_t Encoder_capture(mp_obj_t self_in) { + _Encoder_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Encoder_obj_t); + + Encoder::Snapshot snapshot = self->encoder->take_snapshot(); + + mp_obj_t tuple[] = { + mp_obj_new_int(snapshot.count()), + mp_obj_new_int(snapshot.delta()), + mp_obj_new_float(snapshot.frequency()), + mp_obj_new_float(snapshot.revolutions()), + mp_obj_new_float(snapshot.degrees()), + mp_obj_new_float(snapshot.radians()), + mp_obj_new_float(snapshot.revolutions_delta()), + mp_obj_new_float(snapshot.degrees_delta()), + mp_obj_new_float(snapshot.radians_delta()), + mp_obj_new_float(snapshot.revolutions_per_second()), + mp_obj_new_float(snapshot.revolutions_per_minute()), + mp_obj_new_float(snapshot.degrees_per_second()), + mp_obj_new_float(snapshot.radians_per_second()), + }; + + STATIC const qstr tuple_fields[] = { + MP_QSTR_count, + MP_QSTR_delta, + MP_QSTR_frequency, + MP_QSTR_revolutions, + MP_QSTR_degrees, + MP_QSTR_radians, + MP_QSTR_revolutions_delta, + MP_QSTR_degrees_delta, + MP_QSTR_radians_delta, + MP_QSTR_revolutions_per_second, + MP_QSTR_revolutions_per_minute, + MP_QSTR_degrees_per_second, + MP_QSTR_radians_per_second, + }; + + return mp_obj_new_attrtuple(tuple_fields, sizeof(tuple) / sizeof(mp_obj_t), tuple); +} } diff --git a/micropython/modules/encoder/encoder.h b/micropython/modules/encoder/encoder.h index 80bbc4d7..257a5bd3 100644 --- a/micropython/modules/encoder/encoder.h +++ b/micropython/modules/encoder/encoder.h @@ -39,4 +39,5 @@ 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_take_snapshot(mp_obj_t self_in); \ No newline at end of file +extern mp_obj_t Encoder_take_snapshot(mp_obj_t self_in); +extern mp_obj_t Encoder_capture(mp_obj_t self_in); \ No newline at end of file