From 62e582547238ac15269c00da9dce1970141fe43d Mon Sep 17 00:00:00 2001 From: pmoegenburg Date: Mon, 12 Feb 2024 15:46:00 -0500 Subject: [PATCH] refactored debounce mechanism, changed existing-error-present message, cleaned up motion controller code --- include/can/core/ids.hpp | 1 - include/can/core/messages.hpp | 5 +- include/motor-control/core/tasks/messages.hpp | 96 +------------- .../core/tasks/motion_controller_task.hpp | 118 ++---------------- .../core/tasks/motion_controller_task.hpp | 8 +- pipettes/firmware/main.cpp | 4 +- 6 files changed, 18 insertions(+), 214 deletions(-) diff --git a/include/can/core/ids.hpp b/include/can/core/ids.hpp index b915e2853..621e9f3ee 100644 --- a/include/can/core/ids.hpp +++ b/include/can/core/ids.hpp @@ -63,7 +63,6 @@ enum class MessageId { read_motor_current_response = 0x35, read_motor_driver_error_status_request = 0x36, read_motor_driver_error_status_response = 0x37, - motor_driver_in_error_state = 0x38, set_brushed_motor_vref_request = 0x40, set_brushed_motor_pwm_request = 0x41, gripper_grip_request = 0x42, diff --git a/include/can/core/messages.hpp b/include/can/core/messages.hpp index 8ac097aeb..0f1346c8f 100644 --- a/include/can/core/messages.hpp +++ b/include/can/core/messages.hpp @@ -198,8 +198,6 @@ using MotorPositionRequest = Empty; using UpdateMotorPositionEstimationRequest = Empty; -using MotorDriverInErrorState = Empty; - struct WriteToEEPromRequest : BaseMessage { uint32_t message_index; eeprom::types::address address; @@ -1614,7 +1612,6 @@ using ResponseMessageType = std::variant< PeripheralStatusResponse, BrushedMotorConfResponse, UpdateMotorPositionEstimationResponse, BaselineSensorResponse, PushTipPresenceNotification, GetMotorUsageResponse, GripperJawStateResponse, - GripperJawHoldoffResponse, ReadMotorDriverErrorStatusResponse, - MotorDriverInErrorState>; + GripperJawHoldoffResponse, ReadMotorDriverErrorStatusResponse>; } // namespace can::messages diff --git a/include/motor-control/core/tasks/messages.hpp b/include/motor-control/core/tasks/messages.hpp index bc55c0d60..1614c6037 100644 --- a/include/motor-control/core/tasks/messages.hpp +++ b/include/motor-control/core/tasks/messages.hpp @@ -6,103 +6,9 @@ namespace motor_control_task_messages { -/*enum class MessageId { - route_motor_driver_interrupt = 0x0, - motor_driver_error_encountered = 0x1, - reset_motor_driver_error_handling = 0x2, -}; - -template -struct BaseMessage { - static const auto id = MId; - auto operator==(const BaseMessage& other) const -> bool = default; -}; - -template -struct Empty : BaseMessage { - uint32_t message_index; - template - static auto parse(Input body, Limit limit) -> Empty { - uint32_t msg_ind = 0; - body = bit_utils::bytes_to_int(body, limit, msg_ind); - return Empty{.message_index = msg_ind}; - } - - template - auto serialize(Output body, Limit limit) const -> uint8_t { - auto iter = bit_utils::int_to_bytes(message_index, body, limit); - return iter - body; - } - - auto operator==(const Empty&) const -> bool = default; -}; - -using RouteMotorDriverInterrupt = -Empty; - -using MotorDriverErrorEncountered = -Empty; - -using ResetMotorDriverErrorHandling = -Empty; - -struct RouteMotorDriverInterrupt { - uint32_t message_index; - template - static auto parse(Input body, Limit limit) -> RouteMotorDriverInterrupt { - uint32_t msg_ind = 0; - body = bit_utils::bytes_to_int(body, limit, msg_ind); - return RouteMotorDriverInterrupt{.message_index = msg_ind}; - } - - template - auto serialize(Output body, Limit limit) const -> uint8_t { - auto iter = bit_utils::int_to_bytes(message_index, body, limit); - return iter - body; - } - - auto operator==(const RouteMotorDriverInterrupt&) const -> bool = default; -}; - -struct MotorDriverErrorEncountered { - uint32_t message_index; - template - static auto parse(Input body, Limit limit) -> MotorDriverErrorEncountered { - uint32_t msg_ind = 0; - body = bit_utils::bytes_to_int(body, limit, msg_ind); - return MotorDriverErrorEncountered{.message_index = msg_ind}; - } - - template - auto serialize(Output body, Limit limit) const -> uint8_t { - auto iter = bit_utils::int_to_bytes(message_index, body, limit); - return iter - body; - } - - auto operator==(const MotorDriverErrorEncountered&) const -> bool = default; -}; - -struct ResetMotorDriverErrorHandling { - uint32_t message_index; - template - static auto parse(Input body, Limit limit) -> ResetMotorDriverErrorHandling -{ uint32_t msg_ind = 0; body = bit_utils::bytes_to_int(body, limit, msg_ind); - return ResetMotorDriverErrorHandling{.message_index = msg_ind}; - } - - template - auto serialize(Output body, Limit limit) const -> uint8_t { - auto iter = bit_utils::int_to_bytes(message_index, body, limit); - return iter - body; - } - - auto operator==(const ResetMotorDriverErrorHandling&) const -> bool = -default; -};*/ -// needs a member 'id'. This seems to just be becoming a CAN msg! - struct RouteMotorDriverInterrupt { uint32_t message_index; + uint8_t debounce_count; }; using MotionControlTaskMessage = std::variant< diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp index 9cc020263..d77f07c10 100644 --- a/include/motor-control/core/tasks/motion_controller_task.hpp +++ b/include/motor-control/core/tasks/motion_controller_task.hpp @@ -70,9 +70,7 @@ class MotionControllerMessageHandler { void handle(const can::messages::EnableMotorRequest& m) { LOG("Received enable motor request"); if (controller.read_tmc_diag0()) { - can_client.send_can_message(can::ids::NodeId::host, - can::messages::MotorDriverInErrorState{ - .message_index = m.message_index}); + 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}); } else { controller.enable_motor(); can_client.send_can_message(can::ids::NodeId::host, @@ -116,9 +114,7 @@ class MotionControllerMessageHandler { m.velocity, m.acceleration, m.group_id, m.seq_id, m.duration, m.request_stop_condition); if (controller.read_tmc_diag0()) { - can_client.send_can_message(can::ids::NodeId::host, - can::messages::MotorDriverInErrorState{ - .message_index = m.message_index}); + 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}); } else { controller.move(m); } @@ -129,9 +125,7 @@ class MotionControllerMessageHandler { "groupid=%d, seqid=%d\n", m.velocity, m.group_id, m.seq_id); if (controller.read_tmc_diag0()) { - can_client.send_can_message(can::ids::NodeId::host, - can::messages::MotorDriverInErrorState{ - .message_index = m.message_index}); + 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}); } else { controller.move(m); } @@ -171,64 +165,13 @@ class MotionControllerMessageHandler { can_client.send_can_message(can::ids::NodeId::host, response); } } + void handle(const can::messages::GetMotorUsageRequest& m) { controller.send_usage_data(m.message_index, usage_client); } - // no debouncing needed in final product - // make messages non-CAN, should be trivial, everything takes std::monostate - /* - void handle(const motor_control_task_messages::RouteMotorDriverInterrupt& m) - { vTaskDelay(pdMS_TO_TICKS(100)); debounce_count++; if (debounce_count > 9) - { if (controller.read_tmc_diag0()) { - motion_client.send_motion_controller_queue(motor_control_task_messages::MotorDriverErrorEncountered{.message_index - = m.message_index}); } else { - motion_client.send_motion_controller_queue(motor_control_task_messages::ResetMotorDriverErrorHandling{.message_index - = m.message_index}); - } - diag0_debounced = false; - debounce_count = 0; - } else { - motion_client.send_motion_controller_queue(motor_control_task_messages::RouteMotorDriverInterrupt{.message_index - = m.message_index}); - } - } - - void handle(const motor_control_task_messages::MotorDriverErrorEncountered& - m) { 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 - motor_control_task_messages::ResetMotorDriverErrorHandling& m) { - static_cast(m); - controller.clear_cancel_request(); - can_client.send_can_message( - can::ids::NodeId::host, - can::messages::ErrorMessage{ - .message_index = m.message_index, - .severity = can::ids::ErrorSeverity::none, - .error_code = - can::ids::ErrorCode::motor_driver_error_detected}); // - delete - } - */ - - void handle( - const motor_control_task_messages::RouteMotorDriverInterrupt& m) { - if (debounce_count > 9) { + void handle(const motor_control_task_messages::RouteMotorDriverInterrupt& m) { + if (m.debounce_count > 9) { if (controller.read_tmc_diag0()) { controller.stop( can::ids::ErrorSeverity::unrecoverable, @@ -249,23 +192,22 @@ class MotionControllerMessageHandler { controller.clear_cancel_request(); } diag0_debounced = false; - debounce_count = 0; } else { - debounce_count++; vTaskDelay(pdMS_TO_TICKS(100)); - motion_client.send_motion_controller_queue( - motor_control_task_messages::RouteMotorDriverInterrupt{ - .message_index = m.message_index}); + motion_client.send_motion_controller_queue(increment_message_debounce_count(m)); } } + auto increment_message_debounce_count(const motor_control_task_messages::RouteMotorDriverInterrupt& m) -> motor_control_task_messages::RouteMotorDriverInterrupt { + return motor_control_task_messages::RouteMotorDriverInterrupt{.message_index = m.message_index, .debounce_count = static_cast(m.debounce_count + 1)}; + } + MotorControllerType& controller; CanClient& can_client; UsageClient& usage_client; DriverClient& driver_client; MotionClient& motion_client; std::atomic& diag0_debounced; - std::atomic debounce_count = 0; }; /** @@ -313,41 +255,11 @@ class MotionControllerTask { [[nodiscard]] auto get_queue() const -> QueueType& { return queue; } - /* - void run_diag0_interrupt() { - if (!diag0_debounced) { - static_cast(queue.try_write_isr(motor_control_task_messages::RouteMotorDriverInterrupt{.message_index - = 0})); diag0_debounced = true; - } - } - - void run_diag0_interrupt() { - if (controller->read_tmc_diag0()) { - 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 = 0, - .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 = - 0}); - } - } else { - controller->clear_cancel_request(); - } - } - */ - void run_diag0_interrupt() { if (!diag0_debounced) { static_cast(queue.try_write_isr( motor_control_task_messages::RouteMotorDriverInterrupt{ - .message_index = 0})); + .message_index = 0, .debounce_count = 0})); diag0_debounced = true; } } @@ -355,12 +267,6 @@ class MotionControllerTask { private: QueueType& queue; std::atomic diag0_debounced = false; - // static motion_controller::MotionController* - // controller = nullptr; static can::message_writer_task::TaskClient* - // can_client = nullptr; static tmc::tasks::TaskClient* driver_client = - // nullptr; static member object for motion controller (a nullable pointer) - // Can that much calling of motion controller be done in interrupt?? - // do what impacts the least }; } // namespace motion_controller_task diff --git a/include/pipettes/core/tasks/motion_controller_task.hpp b/include/pipettes/core/tasks/motion_controller_task.hpp index ceade960d..8685bf66f 100644 --- a/include/pipettes/core/tasks/motion_controller_task.hpp +++ b/include/pipettes/core/tasks/motion_controller_task.hpp @@ -62,9 +62,7 @@ class MotionControllerMessageHandler { // TODO only toggle the enable pin once since all motors share // a single enable pin line. if (controller.read_tmc_diag0()) { - can_client.send_can_message(can::ids::NodeId::host, - can::messages::MotorDriverInErrorState{ - .message_index = m.message_index}); + 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}); } else { controller.enable_motor(); can_client.send_can_message(can::ids::NodeId::host, @@ -109,9 +107,7 @@ class MotionControllerMessageHandler { "acceleration=%d, groupid=%d, seqid=%d\n", m.velocity, m.acceleration, m.group_id, m.seq_id); if (controller.read_tmc_diag0()) { - can_client.send_can_message(can::ids::NodeId::host, - can::messages::MotorDriverInErrorState{ - .message_index = m.message_index}); + 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}); } else { controller.move(m); } diff --git a/pipettes/firmware/main.cpp b/pipettes/firmware/main.cpp index 2d30ed5ce..5073dbd9d 100644 --- a/pipettes/firmware/main.cpp +++ b/pipettes/firmware/main.cpp @@ -147,8 +147,8 @@ extern "C" void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { sensors::tip_presence::TipStatusChangeDetected{})); } } else if (GPIO_Pin == linear_motor_hardware.get_pins().diag0.pin) { - if (call_diag0_handler != NULL) { - if (*call_diag0_handler != NULL) { + if (call_diag0_handler != nullptr) { + if (*call_diag0_handler != nullptr) { (*call_diag0_handler)(); } }