Added attrtuple for capture, and some position control examples

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="")

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()

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()

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);

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);
+}
 }

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);
+extern mp_obj_t Encoder_take_snapshot(mp_obj_t self_in);
+extern mp_obj_t Encoder_capture(mp_obj_t self_in);