diff --git a/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp b/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp index 113f56214..9be7b625b 100644 --- a/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp +++ b/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp @@ -51,9 +51,6 @@ class MotionController { .stay_engaged = can_msg.stay_engaged, .stop_condition = MoveStopCondition::none, .usage_key = hardware.get_usage_eeprom_config().get_distance_key()}; - if (!enabled) { - enable_motor(); - } queue.try_write(msg); } @@ -66,9 +63,6 @@ class MotionController { .seq_id = can_msg.seq_id, .stop_condition = MoveStopCondition::limit_switch, .usage_key = hardware.get_usage_eeprom_config().get_distance_key()}; - if (!enabled) { - enable_motor(); - } queue.try_write(msg); } @@ -84,16 +78,15 @@ class MotionController { get_mechanical_config().get_encoder_um_per_pulse()), .stop_condition = MoveStopCondition::encoder_position, .usage_key = hardware.get_usage_eeprom_config().get_distance_key()}; - if (!enabled) { - enable_motor(); - } queue.try_write(msg); } void enable_motor() { - hardware.activate_motor(); - hardware.start_timer_interrupt(); - enabled = true; + if (!enabled) { + hardware.activate_motor(); + hardware.start_timer_interrupt(); + enabled = true; + } } void disable_motor() { diff --git a/include/motor-control/core/stepper_motor/motion_controller.hpp b/include/motor-control/core/stepper_motor/motion_controller.hpp index fe356fa6d..0f33f7e9b 100644 --- a/include/motor-control/core/stepper_motor/motion_controller.hpp +++ b/include/motor-control/core/stepper_motor/motion_controller.hpp @@ -80,9 +80,6 @@ class MotionController { 0, hardware.get_usage_eeprom_config().get_distance_key(), can_msg.sensor_id}; - if (!enabled) { - enable_motor(); - } queue.try_write(msg); } @@ -101,9 +98,6 @@ class MotionController { .stop_condition = can_msg.request_stop_condition, .usage_key = hardware.get_usage_eeprom_config().get_distance_key(), .sensor_id = can::ids::SensorId::UNUSED}; - if (!enabled) { - enable_motor(); - } queue.try_write(msg); } @@ -121,9 +115,6 @@ class MotionController { static_cast(MoveStopCondition::limit_switch), .usage_key = hardware.get_usage_eeprom_config().get_distance_key(), .sensor_id = can::ids::SensorId::UNUSED}; - if (!enabled) { - enable_motor(); - } queue.try_write(msg); } #else @@ -142,9 +133,6 @@ 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) { - enable_motor(); - } queue.try_write(msg); } @@ -161,9 +149,6 @@ class MotionController { .stop_condition = static_cast(MoveStopCondition::limit_switch), .usage_key = hardware.get_usage_eeprom_config().get_distance_key()}; - if (!enabled) { - enable_motor(); - } queue.try_write(msg); } @@ -211,9 +196,11 @@ class MotionController { auto read_tmc_diag0() -> bool { return hardware.read_tmc_diag0(); } void enable_motor() { - hardware.start_timer_interrupt(); - hardware.activate_motor(); - enabled = true; + if (!enabled) { + hardware.start_timer_interrupt(); + hardware.activate_motor(); + enabled = true; + } } void disable_motor() { @@ -330,10 +317,6 @@ class PipetteMotionController { can_msg.action, gear_motor_id, can::ids::SensorId::UNUSED}; - - if (!enabled) { - enable_motor(); - } queue.try_write(msg); } @@ -369,9 +352,11 @@ class PipetteMotionController { auto read_tmc_diag0() -> bool { return hardware.read_tmc_diag0(); } void enable_motor() { - hardware.start_timer_interrupt(); - hardware.activate_motor(); - enabled = true; + if (!enabled) { + hardware.start_timer_interrupt(); + hardware.activate_motor(); + enabled = true; + } } void disable_motor() { diff --git a/include/motor-control/core/tasks/brushed_motion_controller_task.hpp b/include/motor-control/core/tasks/brushed_motion_controller_task.hpp index cc817135d..f3d37f247 100644 --- a/include/motor-control/core/tasks/brushed_motion_controller_task.hpp +++ b/include/motor-control/core/tasks/brushed_motion_controller_task.hpp @@ -74,14 +74,17 @@ class MotionControllerMessageHandler { } void handle(const can::messages::AddBrushedLinearMoveRequest& m) { + controller.enable_motor(); controller.move(m); } void handle(const can::messages::GripperGripRequest& m) { + controller.enable_motor(); controller.move(m); } void handle(const can::messages::GripperHomeRequest& m) { + controller.enable_motor(); controller.move(m); } diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp index 239377ef5..a7dc70384 100644 --- a/include/motor-control/core/tasks/motion_controller_task.hpp +++ b/include/motor-control/core/tasks/motion_controller_task.hpp @@ -131,6 +131,7 @@ class MotionControllerMessageHandler { .error_code = can::ids::ErrorCode::motor_driver_error_detected}); } else { + controller.enable_motor(); controller.move(m); } } @@ -141,6 +142,7 @@ 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.enable_motor(); controller.move(m); } #endif @@ -158,6 +160,7 @@ class MotionControllerMessageHandler { .error_code = can::ids::ErrorCode::motor_driver_error_detected}); } else { + controller.enable_motor(); controller.move(m); } } diff --git a/include/pipettes/core/tasks/motion_controller_task.hpp b/include/pipettes/core/tasks/motion_controller_task.hpp index 0a8de42ec..246fc1f3b 100644 --- a/include/pipettes/core/tasks/motion_controller_task.hpp +++ b/include/pipettes/core/tasks/motion_controller_task.hpp @@ -138,6 +138,7 @@ class MotionControllerMessageHandler { .error_code = can::ids::ErrorCode::motor_driver_error_detected}); } else { + controller.enable_motor(); controller.move(m); } }