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..f15532e38 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,12 @@ 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); + // Opening lid slightly to check if latch is stuck. If so, raise an error. + constexpr static double LATCH_RELEASE_BACKOFF_DEGREES = + motor_util::LidStepper::angle_to_microsteps(8); // 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 = @@ -129,15 +135,19 @@ struct LidStepperState { 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; + constexpr static double LID_OPEN_LATCH_BACKOFF_RPM = 10.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.*/ + LATCH_RELEASE_BACKOFF, /**< Open lid slightly to make sure latch is not + stuck before fully opening.*/ + 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.*/ @@ -210,11 +220,11 @@ struct LidState { OPENING_RETRACT_SEAL, /**< Retracting seal before opening lid.*/ OPENING_RETRACT_SEAL_BACKOFF, /**< Extend seal to ease off of the limit switch.*/ - OPENING_OPEN_HINGE, /**< Opening lid hinge.*/ + OPENING_LID_HINGE, /**< Opening lid hinge.*/ CLOSING_RETRACT_SEAL, /**< Retracting seal before closing lid.*/ CLOSING_RETRACT_SEAL_BACKOFF, /**< Extend seal to ease off of the limit switch.*/ - CLOSING_CLOSE_HINGE, /**< Closing lid hinge.*/ + CLOSING_LID_HINGE, /**< Closing lid hinge.*/ CLOSING_EXTEND_SEAL, /**< Extending seal after closing lid hinge.*/ CLOSING_EXTEND_SEAL_BACKOFF, /**< Retract seal to ease off of the @@ -860,7 +870,7 @@ class MotorTask { error = errors::ErrorCode::SEAL_MOTOR_SWITCH; } else if (retract_switch) { // Seal is already retracted, so just open the hinge - error = handle_lid_state_enter(LidState::Status::OPENING_OPEN_HINGE, + error = handle_lid_state_enter(LidState::Status::OPENING_LID_HINGE, policy); } else { // Seal isn't retracted yet, so retract it @@ -912,8 +922,8 @@ class MotorTask { error = errors::ErrorCode::SEAL_MOTOR_SWITCH; } else if (retract_switch) { // Seal is already retracted, so just open the hinge - error = handle_lid_state_enter( - LidState::Status::CLOSING_CLOSE_HINGE, policy); + error = handle_lid_state_enter(LidState::Status::CLOSING_LID_HINGE, + policy); } else { // Always retract seal before closing error = handle_lid_state_enter( @@ -965,14 +975,41 @@ class MotorTask { } // First release the latch policy.lid_solenoid_engage(); + if (policy.lid_read_closed_switch()) { + return start_latch_release_overdrive(response_id, policy); + } + return start_latch_release_backoff(response_id, policy); + } + + 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); + LidStepperState::LID_OPEN_LATCH_BACKOFF_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_latch_release_backoff(uint32_t response_id, Policy& policy) + -> bool { + std::ignore = policy.lid_stepper_set_rpm( + LidStepperState::LID_OPEN_LATCH_BACKOFF_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); + policy.lid_stepper_start(LidStepperState::LATCH_RELEASE_BACKOFF_DEGREES, + false); // Store the new state, as well as the response ID - _lid_stepper_state.status = LidStepperState::Status::OPEN_TO_SWITCH; + _lid_stepper_state.status = + LidStepperState::Status::LATCH_RELEASE_BACKOFF; _lid_stepper_state.position = motor_util::LidStepper::Position::BETWEEN; _lid_stepper_state.response_id = response_id; return true; @@ -983,8 +1020,6 @@ class MotorTask { if (_lid_stepper_state.status != LidStepperState::Status::IDLE) { return false; } - // 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); @@ -1071,7 +1106,7 @@ class MotorTask { state_for_system_task = messages::UpdateMotorState::MotorState::OPENING_OR_CLOSING; break; - case LidState::Status::OPENING_OPEN_HINGE: + case LidState::Status::OPENING_LID_HINGE: if (!start_lid_hinge_open(INVALID_ID, policy)) { error = errors::ErrorCode::LID_MOTOR_BUSY; } @@ -1093,7 +1128,7 @@ class MotorTask { state_for_system_task = messages::UpdateMotorState::MotorState::OPENING_OR_CLOSING; break; - case LidState::Status::CLOSING_CLOSE_HINGE: + case LidState::Status::CLOSING_LID_HINGE: if (!start_lid_hinge_close(INVALID_ID, policy)) { error = errors::ErrorCode::LID_MOTOR_BUSY; } @@ -1166,7 +1201,7 @@ class MotorTask { auto next_state = shared_switches ? LidState::Status::OPENING_RETRACT_SEAL_BACKOFF - : LidState::Status::OPENING_OPEN_HINGE; + : LidState::Status::OPENING_LID_HINGE; error = handle_lid_state_enter(next_state, policy); break; } @@ -1174,12 +1209,12 @@ class MotorTask { _seal_position = motor_util::SealStepper::Status::RETRACTED; // Start lid motor movement error = handle_lid_state_enter( - LidState::Status::OPENING_OPEN_HINGE, policy); + LidState::Status::OPENING_LID_HINGE, policy); + break; + } + case LidState::Status::OPENING_LID_HINGE: { + error = handle_lid_state_enter(LidState::Status::IDLE, policy); break; - case LidState::Status::OPENING_OPEN_HINGE: - error = - handle_lid_state_enter(LidState::Status::IDLE, policy); - break; } case LidState::Status::CLOSING_RETRACT_SEAL: { _seal_position = @@ -1189,7 +1224,7 @@ class MotorTask { auto next_state = shared_switches ? LidState::Status::CLOSING_RETRACT_SEAL_BACKOFF - : LidState::Status::CLOSING_CLOSE_HINGE; + : LidState::Status::CLOSING_LID_HINGE; error = handle_lid_state_enter(next_state, policy); break; } @@ -1197,14 +1232,23 @@ class MotorTask { _seal_position = motor_util::SealStepper::Status::RETRACTED; // Start lid motor movement error = handle_lid_state_enter( - LidState::Status::CLOSING_CLOSE_HINGE, policy); + LidState::Status::CLOSING_LID_HINGE, policy); + break; + } + case LidState::Status::CLOSING_LID_HINGE: { + error = handle_lid_state_enter( + LidState::Status::CLOSING_EXTEND_SEAL, policy); break; - case LidState::Status::CLOSING_CLOSE_HINGE: - error = handle_lid_state_enter( - LidState::Status::CLOSING_EXTEND_SEAL, policy); - break; } case LidState::Status::CLOSING_EXTEND_SEAL: { + if (!policy.lid_read_closed_switch()) { + handle_lid_movement_error(policy); + auto response = messages::ErrorMessage{ + .code = errors::ErrorCode::UNEXPECTED_LID_STATE}; + static_cast( + _task_registry->comms->get_message_queue().try_send( + messages::HostCommsMessage(response))); + } _seal_position = shared_switches ? motor_util::SealStepper::Status::BETWEEN : motor_util::SealStepper::Status::ENGAGED; @@ -1233,6 +1277,26 @@ class MotorTask { return error; } + template + auto handle_lid_movement_error(Policy& policy) { + policy.lid_solenoid_disengage(); + // Turn off lid stepper current + policy.lid_stepper_set_dac(LID_STEPPER_HOLD_CURRENT); + + // update status + _lid_stepper_state.status = LidStepperState::Status::IDLE; + _state.status = LidState::Status::IDLE; + _lid_stepper_state.position = motor_util::LidStepper::Position::UNKNOWN; + + // TODO: issue an update task error state to system task + constexpr uint32_t system_msg_timeout_ticks = 100; + auto state_for_system_task = + messages::UpdateMotorState::MotorState::IDLE; + static_cast(_task_registry->system->get_message_queue().try_send( + messages::UpdateMotorState{.state = state_for_system_task}, + system_msg_timeout_ticks)); + } + /** * @brief Handler to transition between lid hinge motor states. Should be * called every time a lid motor movement complete callback is triggered. @@ -1251,48 +1315,103 @@ class MotorTask { _lid_stepper_state.position = motor_util::LidStepper::Position::BETWEEN; break; + case LidStepperState::Status::LATCH_RELEASE_OVERDRIVE: + start_latch_release_backoff(_lid_stepper_state.response_id, + policy); + break; + case LidStepperState::Status::LATCH_RELEASE_BACKOFF: + if (!policy.lid_read_closed_switch()) { + 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; + policy.lid_solenoid_disengage(); + } else { + // The latch is stuck, stop and raise error + handle_lid_movement_error(policy); + auto response = messages::ErrorMessage{ + .code = errors::ErrorCode::UNEXPECTED_LID_STATE}; + static_cast( + _task_registry->comms->get_message_queue().try_send( + messages::HostCommsMessage(response))); + } + break; case LidStepperState::Status::OPEN_TO_SWITCH: - // Now that the lid is at the open position, - // the solenoid can be safely turned off - policy.lid_solenoid_disengage(); - // Overdrive into switch - policy.lid_stepper_start( - LidStepperState::OPEN_OVERDRIVE_DEGREES, true); - _lid_stepper_state.status = - LidStepperState::Status::OPEN_OVERDRIVE; + if (!policy.lid_read_open_switch()) { + handle_lid_movement_error(policy); + auto response = messages::ErrorMessage{ + .code = errors::ErrorCode::UNEXPECTED_LID_STATE}; + static_cast( + _task_registry->comms->get_message_queue().try_send( + messages::HostCommsMessage(response))); + } else { + // Overdrive into switch + policy.lid_stepper_start( + LidStepperState::OPEN_OVERDRIVE_DEGREES, true); + _lid_stepper_state.status = + LidStepperState::Status::OPEN_OVERDRIVE; + } break; case LidStepperState::Status::OPEN_OVERDRIVE: - // Turn off lid stepper current - policy.lid_stepper_set_dac(LID_STEPPER_HOLD_CURRENT); - // Movement is done - _lid_stepper_state.status = LidStepperState::Status::IDLE; - _lid_stepper_state.position = - motor_util::LidStepper::Position::OPEN; - // The overall lid state machine can advance now - error = handle_lid_state_end(policy); + // lid open switch should no longer be triggered + if (policy.lid_read_open_switch()) { + handle_lid_movement_error(policy); + auto response = messages::ErrorMessage{ + .code = errors::ErrorCode::UNEXPECTED_LID_STATE}; + static_cast( + _task_registry->comms->get_message_queue().try_send( + messages::HostCommsMessage(response))); + } else { + // Turn off lid stepper current + policy.lid_stepper_set_dac(LID_STEPPER_HOLD_CURRENT); + // Movement is done + _lid_stepper_state.status = LidStepperState::Status::IDLE; + _lid_stepper_state.position = + motor_util::LidStepper::Position::OPEN; + // The overall lid state machine can advance now + error = handle_lid_state_end(policy); + } break; case LidStepperState::Status::CLOSE_TO_SWITCH: - // Overdrive the lid stepper into the switch - policy.lid_stepper_start( - LidStepperState::CLOSE_OVERDRIVE_DEGREES, true); - _lid_stepper_state.status = - LidStepperState::Status::CLOSE_OVERDRIVE; + if (!policy.lid_read_closed_switch()) { + handle_lid_movement_error(policy); + auto response = messages::ErrorMessage{ + .code = errors::ErrorCode::UNEXPECTED_LID_STATE}; + static_cast( + _task_registry->comms->get_message_queue().try_send( + messages::HostCommsMessage(response))); + } else { + // Overdrive the lid stepper into the switch + policy.lid_stepper_start( + LidStepperState::CLOSE_OVERDRIVE_DEGREES, true); + _lid_stepper_state.status = + LidStepperState::Status::CLOSE_OVERDRIVE; + } break; case LidStepperState::Status::CLOSE_OVERDRIVE: - // Now that the lid is at the closed position, - // the solenoid can be safely turned off - policy.lid_solenoid_disengage(); - // Turn off lid stepper current - policy.lid_stepper_set_dac(LID_STEPPER_HOLD_CURRENT); - // Movement is done - _lid_stepper_state.status = LidStepperState::Status::IDLE; - _lid_stepper_state.position = - motor_util::LidStepper::Position::CLOSED; - // The overall lid state machine can advance now - error = handle_lid_state_end(policy); - // if the lid isn't actually closed, overwrite error status if (!policy.lid_read_closed_switch()) { - error = errors::ErrorCode::UNEXPECTED_LID_STATE; + handle_lid_movement_error(policy); + auto response = messages::ErrorMessage{ + .code = errors::ErrorCode::UNEXPECTED_LID_STATE}; + static_cast( + _task_registry->comms->get_message_queue().try_send( + messages::HostCommsMessage(response))); + } else { + // Turn off lid stepper current + policy.lid_stepper_set_dac(LID_STEPPER_HOLD_CURRENT); + // Movement is done + _lid_stepper_state.status = LidStepperState::Status::IDLE; + _lid_stepper_state.position = + motor_util::LidStepper::Position::CLOSED; + // The overall lid state machine can advance now + error = handle_lid_state_end(policy); + // if the lid isn't actually closed, overwrite error status } break; case LidStepperState::Status::LIFT_NUDGE: diff --git a/stm32-modules/thermocycler-gen2/tests/test_motor_task.cpp b/stm32-modules/thermocycler-gen2/tests/test_motor_task.cpp index 9a4c546ac..767133a7a 100644 --- a/stm32-modules/thermocycler-gen2/tests/test_motor_task.cpp +++ b/stm32-modules/thermocycler-gen2/tests/test_motor_task.cpp @@ -607,6 +607,12 @@ SCENARIO("motor task message passing") { struct MotorStep { // Message to send on this step messages::MotorMessage msg; + + // Set the lid closed switch if there's a value before this message + std::optional lid_closed_switch_condition = std::nullopt; + // Set the lid open switch if there's a value before this message + std::optional lid_open_switch_condition = std::nullopt; + // If true, expect that the lid angle increased after this message bool lid_angle_increased = false; // If true, expect that the lid angle decreased after this message @@ -633,6 +639,8 @@ struct MotorStep { std::optional lid_rpm = std::nullopt; // If true, expect an ack in the host comms task std::optional ack = std::nullopt; + // If true, expect an error in the host comms task + std::optional error = std::nullopt; }; /** @@ -648,12 +656,34 @@ void test_motor_state_machine(std::shared_ptr tasks, for (size_t i = 0; i < steps.size(); ++i) { auto &step = steps[i]; + if (step.lid_open_switch_condition.has_value()) { + motor_policy.set_lid_open_switch( + step.lid_open_switch_condition.value()); + } + if (step.lid_closed_switch_condition.has_value()) { + motor_policy.set_lid_closed_switch( + step.lid_closed_switch_condition.value()); + } + auto lid_angle_before = motor_policy.get_angle(); motor_queue.backing_deque.push_back(step.msg); tasks->get_system_queue().backing_deque.clear(); tasks->run_motor_task(); DYNAMIC_SECTION("Step " << i) { + if (step.error.has_value()) { + THEN("an error is sent to host comms") { + auto error = step.error.value(); + REQUIRE(tasks->get_host_comms_queue().has_message()); + auto msg = + tasks->get_host_comms_queue().backing_deque.front(); + REQUIRE( + std::holds_alternative(msg)); + auto response = std::get(msg); + REQUIRE(response.code == error.code); + } + continue; + } if (step.lid_angle_increased) { THEN("the lid motor opened") { REQUIRE(motor_policy.get_angle() > lid_angle_before); @@ -692,6 +722,7 @@ void test_motor_state_machine(std::shared_ptr tasks, REQUIRE(response.with_error == ack.with_error); } } + if (step.motor_state.has_value()) { THEN("the motor state is updated correctly") { REQUIRE(tasks->get_system_queue().has_message()); @@ -730,21 +761,34 @@ SCENARIO("motor task lid state machine") { .seal_on = true, .seal_direction = false, .seal_switch_armed = false}, - // Third step opens hinge + // Third step overdrive lid close 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 + // Fourth step open lid to backoff from latch + { + .msg = messages::LidStepperComplete(), + .lid_angle_increased = true, + .lid_overdrive = false, + }, + // Fifth step fully opening lid {.msg = messages::LidStepperComplete(), + .lid_closed_switch_condition = false, + .lid_angle_increased = true, + .lid_overdrive = false}, + // Sixth step open overdrive + {.msg = messages::LidStepperComplete(), + .lid_open_switch_condition = true, .lid_angle_decreased = true, .lid_overdrive = true}, // Should send ACK now {.msg = messages::LidStepperComplete(), + .lid_open_switch_condition = false, .motor_state = MotorStep::MotorState::IDLE, .ack = messages::AcknowledgePrevious{ @@ -763,21 +807,32 @@ 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 overdrive 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 step do latch backoff + {.msg = messages::LidStepperComplete(), + .lid_angle_increased = true, + .lid_overdrive = false}, + // Fourth open fully if the close switch is not engaged {.msg = messages::LidStepperComplete(), + .lid_closed_switch_condition = false, + .lid_angle_increased = true, + .lid_overdrive = false}, + // Fifth open overdrive + {.msg = messages::LidStepperComplete(), + .lid_open_switch_condition = true, .lid_angle_decreased = true, .lid_overdrive = true}, - // Should send ACK now + // Fifth open overdrive {.msg = messages::LidStepperComplete(), + .lid_open_switch_condition = false, .motor_state = MotorStep::MotorState::IDLE, .ack = messages::AcknowledgePrevious{ @@ -809,10 +864,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 lid closes 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}, }; @@ -855,11 +910,10 @@ SCENARIO("motor task lid state machine") { std::vector steps = { // No action {.msg = messages::OpenLidMessage{.id = 123}, - .ack = - messages::AcknowledgePrevious{ - .responding_to_id = 123, - .with_error = errors::ErrorCode::NO_ERROR}}, - }; + .lid_open_switch_condition = true, + .ack = messages::AcknowledgePrevious{ + .responding_to_id = 123, + .with_error = errors::ErrorCode::NO_ERROR}}}; test_motor_state_machine(tasks, steps); } WHEN("sending close lid command") { @@ -883,56 +937,95 @@ SCENARIO("motor task lid state machine") { messages::SealStepperComplete{ .reason = messages::SealStepperComplete:: CompletionReason::DONE}, + .lid_closed_switch_condition = true, .lid_angle_decreased = true, .lid_overdrive = false, .lid_rpm = motor_task::LidStepperState::LID_DEFAULT_VELOCITY_RPM}, // Fourth step overdrives hinge - {.msg = messages::LidStepperComplete(), - .lid_angle_decreased = true, - .lid_overdrive = true}, - // Now extend seal to switch - {.msg = messages::LidStepperComplete(), - .seal_on = true, - .seal_direction = false, - .seal_switch_armed = true}, - // Retract seal from switch - {.msg = - messages::SealStepperComplete{ - .reason = messages::SealStepperComplete:: - CompletionReason::LIMIT}, - .seal_on = true, - .seal_direction = true, - .seal_switch_armed = false}, }; - AND_WHEN("the closed switch is triggered") { - motor_policy.set_lid_closed_switch(true); + AND_WHEN( + "the closed switch is not triggered after closing the lid to " + "the limit switch") { + steps.push_back(MotorStep{ + .msg = messages::LidStepperComplete(), + .lid_closed_switch_condition = false, + .error = + messages::ErrorMessage{ + .code = errors::ErrorCode::UNEXPECTED_LID_STATE}} + // Retract seal from switch + ); + test_motor_state_machine(tasks, steps); + } + AND_WHEN( + "the closed switch is not triggered after overdriving the " + "lid") { + steps.push_back(MotorStep{.msg = messages::LidStepperComplete(), + .lid_closed_switch_condition = true, + .lid_angle_decreased = true, + .lid_overdrive = true} + // Now extend seal to switch + ); + steps.push_back(MotorStep{ + .msg = messages::LidStepperComplete(), + .lid_closed_switch_condition = false, + .seal_on = true, + .seal_direction = false, + .seal_switch_armed = true, + .error = + messages::ErrorMessage{ + .code = errors::ErrorCode::UNEXPECTED_LID_STATE}} + // Retract seal from switch + ); + test_motor_state_machine(tasks, steps); + } + steps.push_back(MotorStep{.msg = messages::LidStepperComplete(), + .lid_angle_decreased = true, + .lid_overdrive = true} + // Now extend seal to switch + ); + steps.push_back(MotorStep{.msg = messages::LidStepperComplete(), + .seal_on = true, + .seal_direction = false, + .seal_switch_armed = true} + // Retract seal from switch + ); + + AND_WHEN( + "the closed switch is triggered after extending the seal") { + steps.push_back( + MotorStep{.msg = + messages::SealStepperComplete{ + .reason = messages::SealStepperComplete:: + CompletionReason::LIMIT}, + .lid_closed_switch_condition = true, + .seal_on = true, + .seal_direction = true, + .seal_switch_armed = false}); steps.push_back( // an ack with error code NO_ERROR should follow MotorStep{.msg = messages::SealStepperComplete{ .reason = messages::SealStepperComplete:: CompletionReason::DONE}, + .lid_closed_switch_condition = true, .motor_state = MotorStep::MotorState::IDLE, .ack = messages::AcknowledgePrevious{ .responding_to_id = 123, .with_error = errors::ErrorCode::NO_ERROR}}); test_motor_state_machine(tasks, steps); } - AND_WHEN("the closed switch is not triggered") { - motor_policy.set_lid_closed_switch(false); - steps.push_back( - // an ack with error code UNEXPECTED_LID_STATE should follow - MotorStep{ - .msg = - messages::SealStepperComplete{ - .reason = messages::SealStepperComplete:: - CompletionReason::DONE}, - .motor_state = MotorStep::MotorState::IDLE, - .ack = messages::AcknowledgePrevious{ - .responding_to_id = 0, - .with_error = - errors::ErrorCode::UNEXPECTED_LID_STATE}}); + AND_WHEN( + "the closed switch is not triggered after extending the seal") { + steps.push_back(MotorStep{ + .msg = + messages::SealStepperComplete{ + .reason = messages::SealStepperComplete:: + CompletionReason::DONE}, + .lid_closed_switch_condition = false, + .motor_state = MotorStep::MotorState::IDLE, + .error = messages::ErrorMessage{ + .code = errors::ErrorCode::UNEXPECTED_LID_STATE}}); test_motor_state_machine(tasks, steps); } } @@ -948,6 +1041,7 @@ SCENARIO("motor task lid state machine") { // Incremental nudging steps.push_back(MotorStep{ .msg = messages::LidStepperComplete(), + .lid_open_switch_condition = true, .lid_angle_increased = true, .lid_overdrive = true, .lid_rpm = @@ -958,12 +1052,12 @@ SCENARIO("motor task lid state machine") { .lid_angle_decreased = true, .lid_overdrive = true, .lid_rpm = - motor_task::LidStepperState::PLATE_LIFT_VELOCITY_RPM, - }); + motor_task::LidStepperState::PLATE_LIFT_VELOCITY_RPM}); } // Final open - all the way to the limit steps.push_back(MotorStep{ .msg = messages::LidStepperComplete(), + .lid_open_switch_condition = true, .lid_angle_increased = true, .lid_overdrive = true, .lid_rpm = @@ -979,6 +1073,7 @@ SCENARIO("motor task lid state machine") { // Return to the switch steps.push_back(MotorStep{ .msg = messages::LidStepperComplete(), + .lid_open_switch_condition = true, .lid_angle_increased = true, .lid_overdrive = false, .lid_rpm = @@ -986,6 +1081,7 @@ SCENARIO("motor task lid state machine") { // Now BACK OUT of the switch steps.push_back(MotorStep{ .msg = messages::LidStepperComplete(), + .lid_open_switch_condition = true, .lid_angle_increased = false, .lid_angle_decreased = true, .lid_overdrive = true, @@ -994,6 +1090,7 @@ SCENARIO("motor task lid state machine") { steps.push_back( // an ack with error code NO_ERROR should follow MotorStep{.msg = messages::LidStepperComplete(), + .lid_open_switch_condition = false, .motor_state = MotorStep::MotorState::IDLE, .ack = messages::AcknowledgePrevious{ .responding_to_id = 123, @@ -1008,12 +1105,14 @@ SCENARIO("motor task lid state machine") { std::vector steps = { // First step closes hinge - skip seal movements {.msg = messages::CloseLidMessage{.id = 123}, + .lid_closed_switch_condition = true, .lid_angle_decreased = true, .lid_overdrive = false, .lid_rpm = motor_task::LidStepperState::LID_DEFAULT_VELOCITY_RPM}, // Fourth step overdrives hinge {.msg = messages::LidStepperComplete(), + .lid_closed_switch_condition = true, .lid_angle_decreased = true, .lid_overdrive = true}, // Now extend seal to switch @@ -1022,16 +1121,16 @@ SCENARIO("motor task lid state machine") { .seal_direction = false, .seal_switch_armed = true}, // Retract seal from switch - {.msg = - messages::SealStepperComplete{ - .reason = messages::SealStepperComplete:: - CompletionReason::LIMIT}, - .seal_on = true, - .seal_direction = true, - .seal_switch_armed = false}, }; AND_WHEN("the closed switch is triggered") { - motor_policy.set_lid_closed_switch(true); + steps.push_back(MotorStep{ + .msg = + messages::SealStepperComplete{ + .reason = messages::SealStepperComplete:: + CompletionReason::LIMIT}, + .seal_on = true, + .seal_direction = true, + .seal_switch_armed = false}); steps.push_back( // an ack with error code NO_ERROR should follow MotorStep{ @@ -1039,6 +1138,7 @@ SCENARIO("motor task lid state machine") { messages::SealStepperComplete{ .reason = messages::SealStepperComplete:: CompletionReason::DONE}, + .lid_closed_switch_condition = true, .motor_state = MotorStep::MotorState::IDLE, .ack = messages::AcknowledgePrevious{ .responding_to_id = 123, @@ -1046,20 +1146,16 @@ SCENARIO("motor task lid state machine") { test_motor_state_machine(tasks, steps); } AND_WHEN("the closed switch is not triggered") { - motor_policy.set_lid_closed_switch(false); - steps.push_back( - // an ack with error code UNEXPECTED_LID_STATE should - // follow - MotorStep{ - .msg = - messages::SealStepperComplete{ - .reason = messages::SealStepperComplete:: - CompletionReason::DONE}, - .motor_state = MotorStep::MotorState::IDLE, - .ack = messages::AcknowledgePrevious{ - .responding_to_id = 0, - .with_error = - errors::ErrorCode::UNEXPECTED_LID_STATE}}); + // An error message with UNEXPECTED_LID_STATE should follow + steps.push_back(MotorStep{ + .msg = + messages::SealStepperComplete{ + .reason = messages::SealStepperComplete:: + CompletionReason::DONE}, + .lid_closed_switch_condition = false, + .motor_state = MotorStep::MotorState::IDLE, + .error = messages::ErrorMessage{ + .code = errors::ErrorCode::UNEXPECTED_LID_STATE}}); test_motor_state_machine(tasks, steps); } } @@ -1080,22 +1176,23 @@ SCENARIO("motor task lid state machine") { messages::SealStepperComplete{ .reason = messages::SealStepperComplete:: CompletionReason::DONE}, + .lid_closed_switch_condition = true, .lid_angle_decreased = true, .lid_overdrive = false, .lid_rpm = motor_task::LidStepperState::LID_DEFAULT_VELOCITY_RPM}, // Fourth step overdrives hinge {.msg = messages::LidStepperComplete(), + .lid_closed_switch_condition = true, .lid_angle_decreased = true, .lid_overdrive = true}, - // Now extend seal to switch {.msg = messages::LidStepperComplete(), .seal_on = true, .seal_direction = false, .seal_switch_armed = true}, }; + // Now extend seal to switch AND_WHEN("the closed switch is triggered") { - motor_policy.set_lid_closed_switch(true); steps.push_back( // an ack with error code NO_ERROR should follow MotorStep{ @@ -1103,6 +1200,7 @@ SCENARIO("motor task lid state machine") { messages::SealStepperComplete{ .reason = messages::SealStepperComplete:: CompletionReason::DONE}, + .lid_closed_switch_condition = true, .motor_state = MotorStep::MotorState::IDLE, .ack = messages::AcknowledgePrevious{ .responding_to_id = 123, @@ -1110,7 +1208,6 @@ SCENARIO("motor task lid state machine") { test_motor_state_machine(tasks, steps); } AND_WHEN("the closed switch is not triggered") { - motor_policy.set_lid_closed_switch(false); steps.push_back( // an ack with error code UNEXPECTED_LID_STATE should // follow @@ -1119,11 +1216,12 @@ SCENARIO("motor task lid state machine") { messages::SealStepperComplete{ .reason = messages::SealStepperComplete:: CompletionReason::DONE}, + .lid_closed_switch_condition = false, .motor_state = MotorStep::MotorState::IDLE, - .ack = messages::AcknowledgePrevious{ - .responding_to_id = 0, - .with_error = + .error = messages::ErrorMessage{ + .code = errors::ErrorCode::UNEXPECTED_LID_STATE}}); + test_motor_state_machine(tasks, steps); } } @@ -1139,7 +1237,7 @@ SCENARIO("motor task lid state machine") { .seal_on = true, .seal_direction = true, .seal_switch_armed = true}, - // Second step extends seeal switch + // Second step extends seal switch {.msg = messages::SealStepperComplete{ .reason = messages::SealStepperComplete:: @@ -1147,7 +1245,8 @@ SCENARIO("motor task lid state machine") { .seal_on = true, .seal_direction = false, .seal_switch_armed = false}, - // Third step opens hinge + // Third step closed switch is not engaged, we can go directly + // latch backoff {.msg = messages::SealStepperComplete{ .reason = messages::SealStepperComplete:: @@ -1155,19 +1254,37 @@ SCENARIO("motor task lid state machine") { .lid_angle_increased = true, .lid_overdrive = false, .lid_rpm = - motor_task::LidStepperState::LID_DEFAULT_VELOCITY_RPM}, - // Fourth step overdrives hinge - {.msg = messages::LidStepperComplete(), - .lid_angle_decreased = true, - .lid_overdrive = true}, + motor_task::LidStepperState::LID_DEFAULT_VELOCITY_RPM}}; + + WHEN("lid backoff from the latch successfully") { + // Fourth step fully opening lid + steps.push_back(MotorStep{.msg = messages::LidStepperComplete(), + .lid_closed_switch_condition = false, + .lid_angle_increased = true, + .lid_overdrive = false}); + // Fifth step open overdrive + steps.push_back(MotorStep{.msg = messages::LidStepperComplete(), + .lid_open_switch_condition = true, + .lid_angle_decreased = true, + .lid_overdrive = true}); // Should send ACK now - {.msg = messages::LidStepperComplete(), - .ack = - messages::AcknowledgePrevious{ - .responding_to_id = 123, - .with_error = errors::ErrorCode::NO_ERROR}}, - }; - test_motor_state_machine(tasks, steps); + steps.push_back( + MotorStep{.msg = messages::LidStepperComplete(), + .lid_open_switch_condition = false, + .ack = messages::AcknowledgePrevious{ + .responding_to_id = 123, + .with_error = errors::ErrorCode::NO_ERROR}}); + test_motor_state_machine(tasks, steps); + } + WHEN("lid is stuck on latch") { + // Fourth step fully opening lid + steps.push_back(MotorStep{ + .msg = messages::LidStepperComplete(), + .lid_closed_switch_condition = true, + .error = messages::ErrorMessage{ + .code = errors::ErrorCode::UNEXPECTED_LID_STATE}}); + test_motor_state_machine(tasks, steps); + } } WHEN("sending close lid command") { std::vector steps = { @@ -1198,14 +1315,18 @@ SCENARIO("motor task lid state machine") { AND_WHEN("the closed switch is triggered") { motor_policy.set_lid_closed_switch(true); // Fourth step overdrives hinge - steps.push_back(MotorStep{.msg = messages::LidStepperComplete(), - .lid_angle_decreased = true, - .lid_overdrive = true}); + steps.push_back(MotorStep{ + .msg = messages::LidStepperComplete(), + .lid_angle_decreased = true, + .lid_overdrive = true, + }); // Now extend seal to switch - steps.push_back(MotorStep{.msg = messages::LidStepperComplete(), - .seal_on = true, - .seal_direction = false, - .seal_switch_armed = true}); + steps.push_back(MotorStep{ + .msg = messages::LidStepperComplete(), + .seal_on = true, + .seal_direction = false, + .seal_switch_armed = true, + }); // Retract seal from switch steps.push_back( MotorStep{.msg = @@ -1229,34 +1350,10 @@ SCENARIO("motor task lid state machine") { AND_WHEN("the closed switch is not triggered") { motor_policy.set_lid_closed_switch(false); // Fourth step overdrives hinge - steps.push_back(MotorStep{.msg = messages::LidStepperComplete(), - .lid_angle_decreased = true, - .lid_overdrive = true}); - // Now extend seal to switch - steps.push_back(MotorStep{.msg = messages::LidStepperComplete(), - .seal_on = true, - .seal_direction = false, - .seal_switch_armed = true}); - // Retract seal from switch - steps.push_back( - MotorStep{.msg = - messages::SealStepperComplete{ - .reason = messages::SealStepperComplete:: - CompletionReason::LIMIT}, - .seal_on = true, - .seal_direction = true, - .seal_switch_armed = false}); - steps.push_back( - // an ack with error code UNEXPECTED_LID_STATE should follow - MotorStep{ - .msg = - messages::SealStepperComplete{ - .reason = messages::SealStepperComplete:: - CompletionReason::DONE}, - .ack = messages::AcknowledgePrevious{ - .responding_to_id = 0, - .with_error = - errors::ErrorCode::UNEXPECTED_LID_STATE}}); + steps.push_back(MotorStep{ + .msg = messages::LidStepperComplete(), + .error = messages::ErrorMessage{ + .code = errors::ErrorCode::UNEXPECTED_LID_STATE}}); test_motor_state_machine(tasks, steps); } }