Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Revert "feat(thermocycler): make sure the lid ease off the latch before attempting to open" #477

Merged
merged 1 commit into from
Nov 14, 2024
Merged

Revert "feat(thermocycler): make sure the lid ease off the latch before attempting to open" #477

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
249 changes: 65 additions & 184 deletions stm32-modules/include/thermocycler-gen2/thermocycler-gen2/motor_task.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -95,12 +95,6 @@ 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 =
Expand Down Expand Up @@ -135,19 +129,15 @@ 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.*/
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.*/
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.*/
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.*/
Expand Down Expand Up @@ -220,11 +210,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_LID_HINGE, /**< Opening lid hinge.*/
OPENING_OPEN_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_LID_HINGE, /**< Closing lid hinge.*/
CLOSING_CLOSE_HINGE, /**< Closing lid hinge.*/
CLOSING_EXTEND_SEAL, /**< Extending seal after closing
lid hinge.*/
CLOSING_EXTEND_SEAL_BACKOFF, /**< Retract seal to ease off of the
Expand Down Expand Up @@ -870,7 +860,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_LID_HINGE,
error = handle_lid_state_enter(LidState::Status::OPENING_OPEN_HINGE,
policy);
} else {
// Seal isn't retracted yet, so retract it
Expand Down Expand Up @@ -922,8 +912,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_LID_HINGE,
policy);
error = handle_lid_state_enter(
LidState::Status::CLOSING_CLOSE_HINGE, policy);
} else {
// Always retract seal before closing
error = handle_lid_state_enter(
Expand Down Expand Up @@ -975,41 +965,14 @@ 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 <MotorExecutionPolicy Policy>
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_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 <MotorExecutionPolicy Policy>
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);
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_BACKOFF_DEGREES,
false);
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::LATCH_RELEASE_BACKOFF;
_lid_stepper_state.status = LidStepperState::Status::OPEN_TO_SWITCH;
_lid_stepper_state.position = motor_util::LidStepper::Position::BETWEEN;
_lid_stepper_state.response_id = response_id;
return true;
Expand All @@ -1020,6 +983,8 @@ 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);
Expand Down Expand Up @@ -1106,7 +1071,7 @@ class MotorTask {
state_for_system_task =
messages::UpdateMotorState::MotorState::OPENING_OR_CLOSING;
break;
case LidState::Status::OPENING_LID_HINGE:
case LidState::Status::OPENING_OPEN_HINGE:
if (!start_lid_hinge_open(INVALID_ID, policy)) {
error = errors::ErrorCode::LID_MOTOR_BUSY;
}
Expand All @@ -1128,7 +1093,7 @@ class MotorTask {
state_for_system_task =
messages::UpdateMotorState::MotorState::OPENING_OR_CLOSING;
break;
case LidState::Status::CLOSING_LID_HINGE:
case LidState::Status::CLOSING_CLOSE_HINGE:
if (!start_lid_hinge_close(INVALID_ID, policy)) {
error = errors::ErrorCode::LID_MOTOR_BUSY;
}
Expand Down Expand Up @@ -1201,20 +1166,20 @@ class MotorTask {
auto next_state =
shared_switches
? LidState::Status::OPENING_RETRACT_SEAL_BACKOFF
: LidState::Status::OPENING_LID_HINGE;
: LidState::Status::OPENING_OPEN_HINGE;
error = handle_lid_state_enter(next_state, policy);
break;
}
case LidState::Status::OPENING_RETRACT_SEAL_BACKOFF: {
_seal_position = motor_util::SealStepper::Status::RETRACTED;
// Start lid motor movement
error = handle_lid_state_enter(
LidState::Status::OPENING_LID_HINGE, policy);
break;
}
case LidState::Status::OPENING_LID_HINGE: {
error = handle_lid_state_enter(LidState::Status::IDLE, policy);
LidState::Status::OPENING_OPEN_HINGE, 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 =
Expand All @@ -1224,31 +1189,22 @@ class MotorTask {
auto next_state =
shared_switches
? LidState::Status::CLOSING_RETRACT_SEAL_BACKOFF
: LidState::Status::CLOSING_LID_HINGE;
: LidState::Status::CLOSING_CLOSE_HINGE;
error = handle_lid_state_enter(next_state, policy);
break;
}
case LidState::Status::CLOSING_RETRACT_SEAL_BACKOFF: {
_seal_position = motor_util::SealStepper::Status::RETRACTED;
// Start lid motor movement
error = handle_lid_state_enter(
LidState::Status::CLOSING_LID_HINGE, policy);
break;
}
case LidState::Status::CLOSING_LID_HINGE: {
error = handle_lid_state_enter(
LidState::Status::CLOSING_EXTEND_SEAL, policy);
LidState::Status::CLOSING_CLOSE_HINGE, 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<void>(
_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;
Expand Down Expand Up @@ -1277,26 +1233,6 @@ class MotorTask {
return error;
}

template <MotorExecutionPolicy Policy>
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<void>(_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.
Expand All @@ -1315,103 +1251,48 @@ 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<void>(
_task_registry->comms->get_message_queue().try_send(
messages::HostCommsMessage(response)));
}
break;
case LidStepperState::Status::OPEN_TO_SWITCH:
if (!policy.lid_read_open_switch()) {
handle_lid_movement_error(policy);
auto response = messages::ErrorMessage{
.code = errors::ErrorCode::UNEXPECTED_LID_STATE};
static_cast<void>(
_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;
}
// 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;
break;
case LidStepperState::Status::OPEN_OVERDRIVE:
// 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<void>(
_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);
}
// 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:
if (!policy.lid_read_closed_switch()) {
handle_lid_movement_error(policy);
auto response = messages::ErrorMessage{
.code = errors::ErrorCode::UNEXPECTED_LID_STATE};
static_cast<void>(
_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;
}
// 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()) {
handle_lid_movement_error(policy);
auto response = messages::ErrorMessage{
.code = errors::ErrorCode::UNEXPECTED_LID_STATE};
static_cast<void>(
_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
error = errors::ErrorCode::UNEXPECTED_LID_STATE;
}
break;
case LidStepperState::Status::LIFT_NUDGE:
Expand Down
Loading
Loading