diff --git a/stm32-modules/include/thermocycler-gen2/thermocycler-gen2/motor_task.hpp b/stm32-modules/include/thermocycler-gen2/thermocycler-gen2/motor_task.hpp index adb11e44c..b641604e9 100644 --- a/stm32-modules/include/thermocycler-gen2/thermocycler-gen2/motor_task.hpp +++ b/stm32-modules/include/thermocycler-gen2/thermocycler-gen2/motor_task.hpp @@ -95,6 +95,9 @@ concept MotorExecutionPolicy = requires(Policy& p, // Structure to encapsulate state of the lid stepper struct LidStepperState { + // Further closing the lid to ensure the latch is not stuck. + constexpr static double LATCH_RELEASE_OVERDRIVE_DEGREES = + motor_util::LidStepper::angle_to_microsteps(-1); // Full open/close movements run until they hit an endstop switch, so the // distance is 120 degrees which is far wider than the actual travel angle. constexpr static double FULL_OPEN_DEGREES = @@ -131,13 +134,14 @@ struct LidStepperState { constexpr static double LID_DEFAULT_VELOCITY_RPM = 125.0F; // States for lid stepper enum Status { - IDLE, /**< Not moving.*/ - SIMPLE_MOVEMENT, /**< Single stage movement.*/ - OPEN_TO_SWITCH, /**< Open until the open switch is hit.*/ - 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.*/ + IDLE, /**< Not moving.*/ + SIMPLE_MOVEMENT, /**< Single stage movement.*/ + LATCH_RELEASE_OVERDRIVE, /**< Close lid to ease off latch.*/ + OPEN_TO_SWITCH, /**< Open until the open switch is hit.*/ + 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.*/ @@ -965,6 +969,11 @@ class MotorTask { } // First release the latch policy.lid_solenoid_engage(); + // If the move is starting at the bottom of the lid's axis, + // overdrive the lid first to release the solenoid latch + if (policy.lid_read_closed_switch()) { + return start_latch_release_overdrive(response_id, policy); + } // Update velocity for this movement std::ignore = policy.lid_stepper_set_rpm( LidStepperState::LID_DEFAULT_VELOCITY_RPM); @@ -978,6 +987,23 @@ class MotorTask { return true; } + template + auto start_latch_release_overdrive(uint32_t response_id, Policy& policy) + -> bool { + // 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::LATCH_RELEASE_OVERDRIVE_DEGREES, true); + // Store the new state, as well as the response ID + _lid_stepper_state.status = + LidStepperState::Status::LATCH_RELEASE_OVERDRIVE; + _lid_stepper_state.response_id = response_id; + return true; + } + template auto start_lid_hinge_close(uint32_t response_id, Policy& policy) -> bool { if (_lid_stepper_state.status != LidStepperState::Status::IDLE) { @@ -1342,6 +1368,18 @@ class MotorTask { _lid_stepper_state.status = LidStepperState::Status::OPEN_TO_SWITCH; break; + case LidStepperState::Status::LATCH_RELEASE_OVERDRIVE: + std::ignore = policy.lid_stepper_set_rpm( + LidStepperState::LID_DEFAULT_VELOCITY_RPM); + // The latch is not holding the lid down, continue to open + policy.lid_stepper_start(LidStepperState::FULL_OPEN_DEGREES, + false); + // Store the new state, as well as the response ID + _lid_stepper_state.status = + LidStepperState::Status::OPEN_TO_SWITCH; + _lid_stepper_state.position = + motor_util::LidStepper::Position::BETWEEN; + break; case LidStepperState::Status::IDLE: [[fallthrough]]; default: diff --git a/stm32-modules/thermocycler-gen2/tests/test_motor_task.cpp b/stm32-modules/thermocycler-gen2/tests/test_motor_task.cpp index 9a4c546ac..9cb9041ad 100644 --- a/stm32-modules/thermocycler-gen2/tests/test_motor_task.cpp +++ b/stm32-modules/thermocycler-gen2/tests/test_motor_task.cpp @@ -735,11 +735,15 @@ SCENARIO("motor task lid state machine") { messages::SealStepperComplete{ .reason = messages::SealStepperComplete:: CompletionReason::DONE}, - .lid_angle_increased = true, - .lid_overdrive = false, + .lid_angle_decreased = true, + .lid_overdrive = true, .lid_rpm = motor_task::LidStepperState::LID_DEFAULT_VELOCITY_RPM}, - // Fourth step overdrives hinge + // Fourth step fully opening lid + {.msg = messages::LidStepperComplete(), + .lid_angle_increased = true, + .lid_overdrive = false}, + // Sixth step open overdrive {.msg = messages::LidStepperComplete(), .lid_angle_decreased = true, .lid_overdrive = true}, @@ -763,16 +767,20 @@ SCENARIO("motor task lid state machine") { .seal_direction = true, .seal_switch_armed = true, .motor_state = MotorStep::MotorState::OPENING_OR_CLOSING}, - // Second step opens hinge + // Second step overdrives lid to ease off the latch {.msg = messages::SealStepperComplete{ .reason = messages::SealStepperComplete:: CompletionReason::DONE}, - .lid_angle_increased = true, - .lid_overdrive = false, + .lid_angle_decreased = true, + .lid_overdrive = true, .lid_rpm = motor_task::LidStepperState::LID_DEFAULT_VELOCITY_RPM}, - // Fourth step overdrives hinge + // Third open fully if the close switch is not engaged + {.msg = messages::LidStepperComplete(), + .lid_angle_increased = true, + .lid_overdrive = false}, + // Fifth open overdrive {.msg = messages::LidStepperComplete(), .lid_angle_decreased = true, .lid_overdrive = true}, @@ -809,10 +817,10 @@ SCENARIO("motor task lid state machine") { motor_policy.set_retraction_switch_triggered(true); WHEN("sending open lid command") { std::vector steps = { - // First step retracts seal switch + // First step closes lid to ease off latch {.msg = messages::OpenLidMessage{.id = 123}, - .lid_angle_increased = true, - .lid_overdrive = false, + .lid_angle_decreased = true, + .lid_overdrive = true, .lid_rpm = motor_task::LidStepperState::LID_DEFAULT_VELOCITY_RPM}, };