feat(thermocycler-gen2): slower plate lift (#376)

stm32-modules/include/thermocycler-gen2/firmware/motor_hardware.h

@@ -81,6 +81,8 @@ bool motor_hardware_lid_stepper_check_fault(void);
  */
 bool motor_hardware_lid_stepper_reset(void);
 
+bool motor_hardware_lid_stepper_set_rpm(double rpm);
+
 /**
  * @brief Read the Lid Closed switch
  *

stm32-modules/include/thermocycler-gen2/firmware/motor_policy.hpp

@@ -74,6 +74,13 @@ class MotorPolicy {
      * @return true if a fault is seen \e after reset, false otherwise
      */
     auto lid_stepper_reset() -> bool;
+    /**
+     * @brief Set the velocity of the lid stepper in RPM. If the lid stepper
+     * is in motion, this will fail.
+     *
+     * @return true if the rpm could be updated
+     */
+    auto lid_stepper_set_rpm(double rpm) -> bool;
     /**
      * @brief Disengage the lid solenoid
      */

stm32-modules/include/thermocycler-gen2/test/test_motor_policy.hpp

@@ -35,6 +35,11 @@ class TestMotorPolicy : public TestTMC2130Policy {
         return true;
     }
 
+    auto lid_stepper_set_rpm(double rpm) -> bool {
+        _lid_rpm = rpm;
+        return true;
+    }
+
     auto lid_solenoid_disengage() -> void { _solenoid_engaged = false; }
     auto lid_solenoid_engage() -> void { _solenoid_engaged = true; }
 
@@ -109,6 +114,8 @@ class TestMotorPolicy : public TestTMC2130Policy {
         _retraction_switch_triggered = val;
     }
 
+    auto get_lid_rpm() -> double { return _lid_rpm; }
+
   private:
     // Solenoid is engaged when unpowered
     bool _solenoid_engaged = true;
@@ -124,5 +131,6 @@ class TestMotorPolicy : public TestTMC2130Policy {
     bool _retraction_switch_triggered = false;
     bool _extension_switch_armed = false;
     bool _retraction_switch_armed = false;
+    double _lid_rpm = 0;
     Callback _callback;
 };

stm32-modules/include/thermocycler-gen2/thermocycler-gen2/motor_task.hpp

@@ -58,6 +58,9 @@ concept MotorExecutionPolicy = requires(Policy& p,
     { p.lid_stepper_check_fault() } -> std::same_as<bool>;
     // A function to reset the stepper driver
     {p.lid_stepper_reset()};
+    // A function to set the RPM of the lid stepper driver
+    // NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
+    { p.lid_stepper_set_rpm(123.0) } -> std::same_as<bool>;
     // A function to disengage the solenoid
     {p.lid_solenoid_disengage()};
     // A function to engage the solenoid
@@ -110,6 +113,11 @@ struct LidStepperState {
         motor_util::LidStepper::angle_to_microsteps(20);
     constexpr static double PLATE_LIFT_LOWER_DEGREES =
         motor_util::LidStepper::angle_to_microsteps(-30);
+    // 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;
+    // Velocity for all lid movements other than plate lift
+    constexpr static double LID_DEFAULT_VELOCITY_RPM = 125.0F;
     // States for lid stepper
     enum Status {
         IDLE,            /**< Not moving.*/
@@ -919,6 +927,9 @@ class MotorTask {
         }
         // First release the latch
         policy.lid_solenoid_engage();
+        // Update velocity for this movement
+        std::ignore = policy.lid_stepper_set_rpm(
+            LidStepperState::LID_DEFAULT_VELOCITY_RPM);
         // Now start a lid motor movement to the endstop
         policy.lid_stepper_set_dac(LID_STEPPER_RUN_CURRENT);
         policy.lid_stepper_start(LidStepperState::FULL_OPEN_DEGREES, false);
@@ -936,6 +947,9 @@ class MotorTask {
         }
         // First release the latch
         policy.lid_solenoid_engage();
+        // Update velocity for this movement
+        std::ignore = policy.lid_stepper_set_rpm(
+            LidStepperState::LID_DEFAULT_VELOCITY_RPM);
         // Now start a lid motor movement to closed position
         policy.lid_stepper_set_dac(LID_STEPPER_RUN_CURRENT);
         policy.lid_stepper_start(LidStepperState::FULL_CLOSE_DEGREES, false);
@@ -952,6 +966,9 @@ class MotorTask {
         if (_lid_stepper_state.status != LidStepperState::Status::IDLE) {
             return false;
         }
+        // Update velocity for this movement
+        std::ignore = policy.lid_stepper_set_rpm(
+            LidStepperState::PLATE_LIFT_VELOCITY_RPM);
         // Now start a lid motor movement to closed position
         policy.lid_stepper_set_dac(LID_STEPPER_RUN_CURRENT);
         policy.lid_stepper_start(LidStepperState::PLATE_LIFT_RAISE_DEGREES,

stm32-modules/thermocycler-gen2/firmware/motor_task/motor_hardware.c

@@ -101,6 +101,11 @@ extern "C" {
  */
 #define LID_RPM_TO_FREQ(rpm) ((rpm * 1280) / 3)
 
+/** Above this speed, will get skipping */
+#define LID_RPM_MAX (125)
+/** Below this speed reduces torque */
+#define LID_RPM_MIN (75)
+
 // ----------------------------------------------------------------------------
 // Local typedefs
 
@@ -187,6 +192,7 @@ void HAL_TIM_OC_MspInit(TIM_HandleTypeDef* htim);
 static void init_dac1(DAC_HandleTypeDef* hdac);
 static void init_tim2(TIM_HandleTypeDef* htim);
 static void init_tim6(TIM_HandleTypeDef* htim);
+static bool lid_active();
 
 // ----------------------------------------------------------------------------
 // Public function implementation
@@ -288,6 +294,25 @@ bool motor_hardware_lid_stepper_reset(void) {
     return (motor_hardware_lid_stepper_check_fault() == true) ? false : true;
 }
 
+bool motor_hardware_lid_stepper_set_rpm(double rpm) {
+    if(lid_active()) {
+        return false;
+    }
+    if(rpm < LID_RPM_MIN || rpm > LID_RPM_MAX) {
+        return false;
+    }
+
+    /* Compute TIM2 clock */
+    uint32_t uwTimClock = HAL_RCC_GetPCLK1Freq();
+    uint32_t uwPeriodValue = __HAL_TIM_CALC_PERIOD(
+                uwTimClock, 
+                _motor_hardware.lid_stepper.timer.Init.Prescaler, 
+                LID_RPM_TO_FREQ(rpm));
+    __HAL_TIM_SET_AUTORELOAD(&_motor_hardware.lid_stepper.timer, uwPeriodValue);
+
+    return true;
+}
+
 bool motor_hardware_lid_read_closed(void) {
     return (HAL_GPIO_ReadPin(LID_CLOSED_SWITCH_PORT, LID_CLOSED_SWITCH_PIN) == GPIO_PIN_RESET) ? true : false;
 }
@@ -589,6 +614,12 @@ static void init_tim6(TIM_HandleTypeDef* htim) {
     configASSERT(hal_ret == HAL_OK);
 }
 
+static bool lid_active() {
+    return TIM_CHANNEL_STATE_GET
+        (&(_motor_hardware.lid_stepper.timer), LID_STEPPER_STEP_Channel) 
+        != HAL_TIM_CHANNEL_STATE_READY;
+}
+
 // ----------------------------------------------------------------------------
 // Overwritten HAL functions
 

stm32-modules/thermocycler-gen2/firmware/motor_task/motor_policy.cpp

@@ -52,6 +52,11 @@ auto MotorPolicy::lid_stepper_reset() -> bool {
     return motor_hardware_lid_stepper_reset();
 }
 
+// NOLINTNEXTLINE(readability-convert-member-functions-to-static)
+auto MotorPolicy::lid_stepper_set_rpm(double rpm) -> bool {
+    return motor_hardware_lid_stepper_set_rpm(rpm);
+}
+
 // NOLINTNEXTLINE(readability-convert-member-functions-to-static)
 auto MotorPolicy::lid_solenoid_disengage() -> void {
     motor_hardware_solenoid_release();

stm32-modules/thermocycler-gen2/simulator/motor_thread.cpp

@@ -74,6 +74,8 @@ class SimMotorPolicy : public SimTMC2130Policy {
         return true;
     }
 
+    auto lid_stepper_set_rpm(double rpm) -> bool { return true; }
+
     auto lid_solenoid_disengage() -> void { _solenoid_engaged = false; }
     auto lid_solenoid_engage() -> void { _solenoid_engaged = true; }
 

stm32-modules/thermocycler-gen2/tests/test_motor_task.cpp

@@ -546,6 +546,11 @@ SCENARIO("motor task message passing") {
                     REQUIRE(motor_task.get_lid_state() ==
                             motor_task::LidState::Status::PLATE_LIFTING);
                 }
+                THEN("the lid rpm is updated") {
+                    REQUIRE(
+                        tasks->get_motor_policy().get_lid_rpm() ==
+                        motor_task::LidStepperState::PLATE_LIFT_VELOCITY_RPM);
+                }
             }
             WHEN("sending a GetLidSwitches message") {
                 auto message = messages::GetLidSwitchesMessage{.id = 123};
@@ -624,6 +629,8 @@ struct MotorStep {
     using SealPos = motor_util::SealStepper::Status;
     // If this variable is set, expect seal in a specific position
     std::optional<SealPos> seal_pos = std::nullopt;
+    // If this variable is set, check the lid velocity
+    std::optional<double> lid_rpm = std::nullopt;
     // If true, expect an ack in the host comms task
     std::optional<messages::AcknowledgePrevious> ack = std::nullopt;
 };
@@ -729,7 +736,9 @@ SCENARIO("motor task lid state machine") {
                          .reason = messages::SealStepperComplete::
                              CompletionReason::DONE},
                  .lid_angle_increased = true,
-                 .lid_overdrive = false},
+                 .lid_overdrive = false,
+                 .lid_rpm =
+                     motor_task::LidStepperState::LID_DEFAULT_VELOCITY_RPM},
                 // Fourth step overdrives hinge
                 {.msg = messages::LidStepperComplete(),
                  .lid_angle_decreased = true,
@@ -810,7 +819,9 @@ SCENARIO("motor task lid state machine") {
                          .reason = messages::SealStepperComplete::
                              CompletionReason::DONE},
                  .lid_angle_decreased = true,
-                 .lid_overdrive = false},
+                 .lid_overdrive = false,
+                 .lid_rpm =
+                     motor_task::LidStepperState::LID_DEFAULT_VELOCITY_RPM},
                 // Fourth step overdrives hinge
                 {.msg = messages::LidStepperComplete(),
                  .lid_angle_decreased = true,
@@ -847,7 +858,9 @@ SCENARIO("motor task lid state machine") {
                 {.msg = messages::PlateLiftMessage{.id = 123},
                  .lid_angle_increased = true,
                  .lid_overdrive = true,
-                 .motor_state = MotorStep::MotorState::PLATE_LIFT},
+                 .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,
@@ -895,7 +908,9 @@ SCENARIO("motor task lid state machine") {
                          .reason = messages::SealStepperComplete::
                              CompletionReason::DONE},
                  .lid_angle_increased = true,
-                 .lid_overdrive = false},
+                 .lid_overdrive = false,
+                 .lid_rpm =
+                     motor_task::LidStepperState::LID_DEFAULT_VELOCITY_RPM},
                 // Fourth step overdrives hinge
                 {.msg = messages::LidStepperComplete(),
                  .lid_angle_decreased = true,
@@ -930,7 +945,9 @@ SCENARIO("motor task lid state machine") {
                          .reason = messages::SealStepperComplete::
                              CompletionReason::DONE},
                  .lid_angle_decreased = true,
-                 .lid_overdrive = false},
+                 .lid_overdrive = false,
+                 .lid_rpm =
+                     motor_task::LidStepperState::LID_DEFAULT_VELOCITY_RPM},
                 // Fourth step overdrives hinge
                 {.msg = messages::LidStepperComplete(),
                  .lid_angle_decreased = true,