From 2776462f45eb712788abe7699385807389528b10 Mon Sep 17 00:00:00 2001 From: pmoegenburg Date: Wed, 8 Nov 2023 15:43:54 -0500 Subject: [PATCH] debugging commit --- .../core/stepper_motor/motion_controller.hpp | 6 +- .../core/tasks/motion_controller_task.hpp | 64 ++++++++++--------- .../core/tasks/motion_controller_task.hpp | 18 ++++-- 3 files changed, 48 insertions(+), 40 deletions(-) diff --git a/include/motor-control/core/stepper_motor/motion_controller.hpp b/include/motor-control/core/stepper_motor/motion_controller.hpp index 519cc957b..9cdb71a73 100644 --- a/include/motor-control/core/stepper_motor/motion_controller.hpp +++ b/include/motor-control/core/stepper_motor/motion_controller.hpp @@ -73,7 +73,7 @@ class MotionController { .seq_id = can_msg.seq_id, .stop_condition = can_msg.request_stop_condition, .usage_key = hardware.get_usage_eeprom_config().get_distance_key()}; - if (!enabled && !read_tmc_diag0()) { + if (!enabled) { enable_motor(); } queue.try_write(msg); @@ -92,7 +92,7 @@ class MotionController { .stop_condition = static_cast(MoveStopCondition::limit_switch), .usage_key = hardware.get_usage_eeprom_config().get_distance_key()}; - if (!enabled && !read_tmc_diag0()) { + if (!enabled) { enable_motor(); } queue.try_write(msg); @@ -257,7 +257,7 @@ class PipetteMotionController { can_msg.action, gear_motor_id}; - if (!enabled && !read_tmc_diag0()) { + if (!enabled) { enable_motor(); } queue.try_write(msg); diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp index 6fe2a7126..0304dfdf2 100644 --- a/include/motor-control/core/tasks/motion_controller_task.hpp +++ b/include/motor-control/core/tasks/motion_controller_task.hpp @@ -56,14 +56,14 @@ class MotionControllerMessageHandler { void handle(const can::messages::EnableMotorRequest& m) { LOG("Received enable motor request"); - if (!controller.read_tmc_diag0()) { - controller.enable_motor(); - can_client.send_can_message(can::ids::NodeId::host, - can::messages::ack_from_request(m)); - } else { + if (controller.read_tmc_diag0()) { can_client.send_can_message( can::ids::NodeId::host, can::messages::MotorDriverInErrorState{.message_index = 0}); + } else { + controller.enable_motor(); + can_client.send_can_message(can::ids::NodeId::host, + can::messages::ack_from_request(m)); } } @@ -102,14 +102,26 @@ class MotionControllerMessageHandler { "groupid=%d, seqid=%d, duration=%d, stopcondition=%d", m.velocity, m.acceleration, m.group_id, m.seq_id, m.duration, m.request_stop_condition); - controller.move(m); + if (controller.read_tmc_diag0()) { + can_client.send_can_message( + can::ids::NodeId::host, + can::messages::MotorDriverInErrorState{.message_index = 0}); + } else { + controller.move(m); + } } void handle(const can::messages::HomeRequest& m) { LOG("Motion Controller Received home request: velocity=%d, " "groupid=%d, seqid=%d\n", m.velocity, m.group_id, m.seq_id); - controller.move(m); + if (controller.read_tmc_diag0()) { + can_client.send_can_message( + can::ids::NodeId::host, + can::messages::MotorDriverInErrorState{.message_index = 0}); + } else { + controller.move(m); + } } void handle(const can::messages::ReadLimitSwitchRequest& m) { @@ -162,41 +174,31 @@ class MotionControllerMessageHandler { } void handle(const can::messages::MotorDriverErrorEncountered& m) { - // if (!driver_error_handled()) { - controller.stop(can::ids::ErrorSeverity::unrecoverable, - can::ids::ErrorCode::motor_driver_error_detected); - if (!controller.is_timer_interrupt_running()) { - can_client.send_can_message( - can::ids::NodeId::host, - can::messages::ErrorMessage{ - .message_index = m.message_index, - .severity = can::ids::ErrorSeverity::unrecoverable, - .error_code = - can::ids::ErrorCode::motor_driver_error_detected}); - driver_client.send_motor_driver_queue( - can::messages::ReadMotorDriverErrorStatus{ - .message_index = m.message_index}); - } - // } + controller.stop(can::ids::ErrorSeverity::unrecoverable, + can::ids::ErrorCode::motor_driver_error_detected); + if (!controller.is_timer_interrupt_running()) { + can_client.send_can_message( + can::ids::NodeId::host, + can::messages::ErrorMessage{ + .message_index = m.message_index, + .severity = can::ids::ErrorSeverity::unrecoverable, + .error_code = + can::ids::ErrorCode::motor_driver_error_detected}); + driver_client.send_motor_driver_queue( + can::messages::ReadMotorDriverErrorStatus{.message_index = + m.message_index}); + } } void handle(const can::messages::ResetMotorDriverErrorHandling& m) { static_cast(m); - driver_error_handled_flag.exchange(false); controller.clear_cancel_request(); } - auto driver_error_handled() -> bool { - return driver_error_handled_flag.exchange(true); - } - MotorControllerType& controller; CanClient& can_client; UsageClient& usage_client; DriverClient& driver_client; - std::atomic driver_error_handled_flag = - false; // can get rid of this if indefinite interrupt trigger issue - // gets resolved }; /** diff --git a/include/pipettes/core/tasks/motion_controller_task.hpp b/include/pipettes/core/tasks/motion_controller_task.hpp index 14120160c..aa6378143 100644 --- a/include/pipettes/core/tasks/motion_controller_task.hpp +++ b/include/pipettes/core/tasks/motion_controller_task.hpp @@ -61,14 +61,14 @@ class MotionControllerMessageHandler { LOG("Received enable motor request"); // TODO only toggle the enable pin once since all motors share // a single enable pin line. - if (!controller.read_tmc_diag0()) { - controller.enable_motor(); - can_client.send_can_message(can::ids::NodeId::host, - can::messages::ack_from_request(m)); - } else { + if (controller.read_tmc_diag0()) { can_client.send_can_message( can::ids::NodeId::host, can::messages::MotorDriverInErrorState{.message_index = 0}); + } else { + controller.enable_motor(); + can_client.send_can_message(can::ids::NodeId::host, + can::messages::ack_from_request(m)); } } @@ -108,7 +108,13 @@ class MotionControllerMessageHandler { LOG("Motion Controller Received a tip action request: velocity=%d, " "acceleration=%d, groupid=%d, seqid=%d\n", m.velocity, m.acceleration, m.group_id, m.seq_id); - controller.move(m); + if (controller.read_tmc_diag0()) { + can_client.send_can_message( + can::ids::NodeId::host, + can::messages::MotorDriverInErrorState{.message_index = 0}); + } else { + controller.move(m); + } } void handle(const can::messages::ReadLimitSwitchRequest& m) {