feat(thermocycler-gen2): add plate lift wiggling (#440)

fsinapi · web-flow · commit 3aabe310a3fc · 2023-04-28T13:37:49.000-04:00
* Change minimum hardware RPM
* Wiggle plate during plate lift
* Fixed tests for plate wiggling
diff --git a/stm32-modules/include/thermocycler-gen2/thermocycler-gen2/motor_task.hpp b/stm32-modules/include/thermocycler-gen2/thermocycler-gen2/motor_task.hpp
@@ -112,13 +112,21 @@ struct LidStepperState {
     // for this movement.
     constexpr static double CLOSE_OVERDRIVE_DEGREES =
         motor_util::LidStepper::angle_to_microsteps(-5);
+    constexpr static double PLATE_LIFT_NUDGE_START_DEGREES =
+        motor_util::LidStepper::angle_to_microsteps(11);
+    constexpr static double PLATE_LIFT_NUDGE_FINAL_DEGREES =
+        motor_util::LidStepper::angle_to_microsteps(15);
+    constexpr static double PLATE_LIFT_NUDGE_INCREMENT =
+        motor_util::LidStepper::angle_to_microsteps(0.5);
+    constexpr static double PLATE_LIFT_RETURN_DEGREES =
+        motor_util::LidStepper::angle_to_microsteps(5);
     constexpr static double PLATE_LIFT_RAISE_DEGREES =
         motor_util::LidStepper::angle_to_microsteps(20);
     constexpr static double PLATE_LIFT_LOWER_DEGREES =
-        motor_util::LidStepper::angle_to_microsteps(-30);
+        motor_util::LidStepper::angle_to_microsteps(-23);
     // Velocity for plate lift actions. This provides a smoother lifting
     // action than the default open/close velocity.
-    constexpr static double PLATE_LIFT_VELOCITY_RPM = 80.0F;
+    constexpr static double PLATE_LIFT_VELOCITY_RPM = 40.0F;
     // Velocity for all lid movements other than plate lift
     constexpr static double LID_DEFAULT_VELOCITY_RPM = 125.0F;
     // States for lid stepper
@@ -129,6 +137,8 @@ struct LidStepperState {
         OPEN_OVERDRIVE,  /**< Close from switch back to 90º position.*/
         CLOSE_TO_SWITCH, /**< Close lid until it hits the switch.*/
         CLOSE_OVERDRIVE, /**< Close lid a few degrees into the switch.*/
+        LIFT_NUDGE,      /**< Nudge the plate up with one pin.*/
+        LIFT_NUDGE_DOWN, /**< Move back to the "open" position after nudging.*/
         LIFT_RAISE,      /**< Open lid to raise the plate lift.*/
         LIFT_LOWER,      /**< Close lid to lower the plate lift.*/
     };
@@ -267,6 +277,7 @@ class MotorTask {
                         motor_util::MovementType::OpenLoop, 0),
           _seal_velocity(SealStepperState::DEFAULT_VELOCITY),
           _seal_acceleration(SealStepperState::DEFAULT_ACCEL),
+          _nudge_degrees(0),
           _seal_position(motor_util::SealStepper::Status::UNKNOWN) {}
     MotorTask(const MotorTask& other) = delete;
     auto operator=(const MotorTask& other) -> MotorTask& = delete;
@@ -996,11 +1007,11 @@ class MotorTask {
         std::ignore = policy.lid_stepper_set_rpm(
             LidStepperState::PLATE_LIFT_VELOCITY_RPM);
         // Now start a lid motor movement to closed position
+        _nudge_degrees = LidStepperState::PLATE_LIFT_NUDGE_START_DEGREES;
         policy.lid_stepper_set_dac(LID_STEPPER_RUN_CURRENT);
-        policy.lid_stepper_start(LidStepperState::PLATE_LIFT_RAISE_DEGREES,
-                                 true);
+        policy.lid_stepper_start(_nudge_degrees, true);
         // Store the new state, as well as the response ID
-        _lid_stepper_state.status = LidStepperState::Status::LIFT_RAISE;
+        _lid_stepper_state.status = LidStepperState::Status::LIFT_NUDGE;
         _lid_stepper_state.position = motor_util::LidStepper::Position::BETWEEN;
         _lid_stepper_state.response_id = response_id;
         return true;
@@ -1281,7 +1292,39 @@ class MotorTask {
                 // TODO(Frank, Mar-7-2022) check if the lid didn't make it in
                 // all the way
                 break;
+            case LidStepperState::Status::LIFT_NUDGE:
+                policy.lid_stepper_start(
+                    -1 * LidStepperState::PLATE_LIFT_RETURN_DEGREES, true);
+                _lid_stepper_state.status =
+                    LidStepperState::Status::LIFT_NUDGE_DOWN;
+                break;
+            case LidStepperState::Status::LIFT_NUDGE_DOWN:
+                // Slow speed for both repeat nudge and for final lift
+                std::ignore = policy.lid_stepper_set_rpm(
+                    LidStepperState::PLATE_LIFT_VELOCITY_RPM);
+                if (_nudge_degrees <
+                    LidStepperState::PLATE_LIFT_NUDGE_FINAL_DEGREES) {
+                    _nudge_degrees +=
+                        LidStepperState::PLATE_LIFT_NUDGE_INCREMENT;
+                    policy.lid_stepper_start(
+                        LidStepperState::PLATE_LIFT_RETURN_DEGREES +
+                            LidStepperState::PLATE_LIFT_NUDGE_INCREMENT,
+                        true);
+                    _lid_stepper_state.status =
+                        LidStepperState::Status::LIFT_NUDGE;
+                } else {
+                    policy.lid_stepper_start(
+                        LidStepperState::PLATE_LIFT_RAISE_DEGREES +
+                            LidStepperState::PLATE_LIFT_RETURN_DEGREES -
+                            _nudge_degrees,
+                        true);
+                    _lid_stepper_state.status =
+                        LidStepperState::Status::LIFT_RAISE;
+                }
+                break;
             case LidStepperState::Status::LIFT_RAISE:
+                std::ignore = policy.lid_stepper_set_rpm(
+                    LidStepperState::LID_DEFAULT_VELOCITY_RPM);
                 // Lower the plate lift mechanism and move the lid far enough
                 // that it will go PAST the switch.
                 policy.lid_stepper_start(
@@ -1315,6 +1358,13 @@ class MotorTask {
     motor_util::MovementProfile _seal_profile;
     double _seal_velocity;
     double _seal_acceleration;
+    /**
+     * When performing a plate lift, a "nudge" is used to dislodge the plate
+     * while avoiding a sudden pop off the plate. The nudge increases in
+     * amplitude over multiple iterations; this variable tracks the current
+     * distance being set.
+     */
+    double _nudge_degrees;
     /**
      * @brief  We need to cache the position of the seal motor in addition to
      * the state in _seal_stepper_state due to the lack of limit switches.
diff --git a/stm32-modules/thermocycler-gen2/firmware/motor_task/motor_hardware.c b/stm32-modules/thermocycler-gen2/firmware/motor_task/motor_hardware.c
@@ -104,7 +104,7 @@ extern "C" {
 /** Above this speed, will get skipping */
 #define LID_RPM_MAX (125)
 /** Below this speed reduces torque */
-#define LID_RPM_MIN (75)
+#define LID_RPM_MIN (10)
 
 // ----------------------------------------------------------------------------
 // Local typedefs
diff --git a/stm32-modules/thermocycler-gen2/scripts/start_dfu.py b/stm32-modules/thermocycler-gen2/scripts/start_dfu.py
@@ -0,0 +1,7 @@
+from test_utils import *
+
+
+if __name__ == '__main__':
+    ser = build_serial()
+    ser.write('dfu\n'.encode())
+    exit(0)
diff --git a/stm32-modules/thermocycler-gen2/tests/test_motor_task.cpp b/stm32-modules/thermocycler-gen2/tests/test_motor_task.cpp
@@ -918,34 +918,68 @@ SCENARIO("motor task lid state machine") {
             test_motor_state_machine(tasks, steps);
         }
         WHEN("sending plate lift command") {
-            std::vector<MotorStep> steps = {
-                // First open past the switch
-                {.msg = messages::PlateLiftMessage{.id = 123},
-                 .lid_angle_increased = true,
-                 .lid_overdrive = true,
-                 .motor_state = MotorStep::MotorState::PLATE_LIFT,
-                 .lid_rpm =
-                     motor_task::LidStepperState::PLATE_LIFT_VELOCITY_RPM},
-                // Now close back below the switch
-                {.msg = messages::LidStepperComplete(),
-                 .lid_angle_decreased = true,
-                 .lid_overdrive = true},
-                // Now open back to the switch
-                {.msg = messages::LidStepperComplete(),
-                 .lid_angle_increased = true,
-                 .lid_overdrive = false},
-                // Now overdrive into the switch
-                {.msg = messages::LidStepperComplete(),
-                 .lid_angle_decreased = true,
-                 .lid_overdrive = true},
-                // Should send ACK now
-                {.msg = messages::LidStepperComplete(),
-                 .motor_state = MotorStep::MotorState::IDLE,
-                 .ack =
-                     messages::AcknowledgePrevious{
-                         .responding_to_id = 123,
-                         .with_error = errors::ErrorCode::NO_ERROR}},
-            };
+            std::vector<MotorStep> steps;
+            for (auto angle = motor_task::LidStepperState::
+                     PLATE_LIFT_NUDGE_START_DEGREES;
+                 angle <
+                 motor_task::LidStepperState::PLATE_LIFT_NUDGE_FINAL_DEGREES +
+                     motor_task::LidStepperState::PLATE_LIFT_NUDGE_INCREMENT;
+                 angle +=
+                 motor_task::LidStepperState::PLATE_LIFT_NUDGE_INCREMENT) {
+                // Incremental nudging
+                steps.push_back(MotorStep{
+                    .msg = messages::LidStepperComplete(),
+                    .lid_angle_increased = true,
+                    .lid_overdrive = true,
+                    .lid_rpm =
+                        motor_task::LidStepperState::PLATE_LIFT_VELOCITY_RPM,
+                });
+                steps.push_back(MotorStep{
+                    .msg = messages::LidStepperComplete(),
+                    .lid_angle_decreased = true,
+                    .lid_overdrive = true,
+                    .lid_rpm =
+                        motor_task::LidStepperState::PLATE_LIFT_VELOCITY_RPM,
+                });
+            }
+            // Final open - all the way to the limit
+            steps.push_back(MotorStep{
+                .msg = messages::LidStepperComplete(),
+                .lid_angle_increased = true,
+                .lid_overdrive = true,
+                .lid_rpm =
+                    motor_task::LidStepperState::PLATE_LIFT_VELOCITY_RPM});
+            // Return below switch
+            steps.push_back(MotorStep{
+                .msg = messages::LidStepperComplete(),
+                .lid_angle_increased = false,
+                .lid_angle_decreased = true,
+                .lid_overdrive = true,
+                .lid_rpm =
+                    motor_task::LidStepperState::LID_DEFAULT_VELOCITY_RPM});
+            // Return to the switch
+            steps.push_back(MotorStep{
+                .msg = messages::LidStepperComplete(),
+                .lid_angle_increased = true,
+                .lid_overdrive = false,
+                .lid_rpm =
+                    motor_task::LidStepperState::LID_DEFAULT_VELOCITY_RPM});
+            // Now BACK OUT of the switch
+            steps.push_back(MotorStep{
+                .msg = messages::LidStepperComplete(),
+                .lid_angle_increased = false,
+                .lid_angle_decreased = true,
+                .lid_overdrive = true,
+                .lid_rpm =
+                    motor_task::LidStepperState::LID_DEFAULT_VELOCITY_RPM});
+            steps.push_back(
+                MotorStep{.msg = messages::LidStepperComplete(),
+                          .motor_state = MotorStep::MotorState::IDLE,
+                          .ack = messages::AcknowledgePrevious{
+                              .responding_to_id = 123,
+                              .with_error = errors::ErrorCode::NO_ERROR}});
+            steps[0].msg = messages::PlateLiftMessage{.id = 123};
+            steps[0].motor_state = MotorStep::MotorState::PLATE_LIFT;
             test_motor_state_machine(tasks, steps);
         }
         GIVEN("seal retraction switch is triggered") {
