diff --git a/include/motor-control/core/brushed_motor/brushed_motor_interrupt_handler.hpp b/include/motor-control/core/brushed_motor/brushed_motor_interrupt_handler.hpp index f482be28c..8cfce6c04 100644 --- a/include/motor-control/core/brushed_motor/brushed_motor_interrupt_handler.hpp +++ b/include/motor-control/core/brushed_motor/brushed_motor_interrupt_handler.hpp @@ -138,10 +138,10 @@ class BrushedMotorInterruptHandler { update_and_start_move(); } else if (!error_handled) { auto motor_state = hardware.get_motor_state(); + auto pulses = hardware.get_encoder_pulses(); // has not reported an error yet if (motor_state == BrushedMotorState::POSITION_CONTROLLING) { - int32_t move_delta = - hardware.get_encoder_pulses() - hold_encoder_position; + int32_t move_delta = pulses - hold_encoder_position; controlled_move_to(move_delta); // we use a value higher than the acceptable position here to // allow the pid loop the opportunity to maintain small @@ -149,6 +149,7 @@ class BrushedMotorInterruptHandler { if (move_delta > error_conf.unwanted_movement_threshold) { cancel_and_clear_moves( can::ids::ErrorCode::collision_detected); + report_position(pulses); error_handled = true; } } else if (motor_state != BrushedMotorState::UNHOMED) { @@ -162,6 +163,7 @@ class BrushedMotorInterruptHandler { ? can::ids::ErrorCode::labware_dropped : can::ids::ErrorCode::collision_detected; cancel_and_clear_moves(err); + report_position(pulses); error_handled = true; } } @@ -301,6 +303,21 @@ class BrushedMotorInterruptHandler { } } + void report_position(int32_t pulses) { + // this message is used only to report encoder position on the can bus + // when the move fails, and will not be used nor handled by the host + uint32_t message_index = 0; + if (_has_active_move) { + message_index = buffered_move.message_index; + } + status_queue_client.send_brushed_move_status_reporter_queue( + motor_messages::UpdatePositionResponse{ + .message_index = message_index, + .stepper_position_counts = 0, + .encoder_pulses = pulses, + .position_flags = 0}); + } + void homing_stopped() { // since the buffered move start position isn't reliable here, // and then end position will always be 0, we set the buffered move diff --git a/motor-control/tests/test_brushed_motor_interrupt_handler.cpp b/motor-control/tests/test_brushed_motor_interrupt_handler.cpp index 83e73442d..793af429b 100644 --- a/motor-control/tests/test_brushed_motor_interrupt_handler.cpp +++ b/motor-control/tests/test_brushed_motor_interrupt_handler.cpp @@ -309,13 +309,18 @@ SCENARIO("labware dropped during grip move") { test_objs.handler.run_interrupt(); } // An error and increase error count is sent - REQUIRE(test_objs.reporter.messages.size() == 2); + REQUIRE(test_objs.reporter.messages.size() == 3); can::messages::ErrorMessage err = std::get( test_objs.reporter.messages.front()); REQUIRE(err.error_code == can::ids::ErrorCode::labware_dropped); REQUIRE(test_objs.handler.error_handled); REQUIRE(test_objs.hw.get_stay_enabled()); + // position reported + motor_messages::UpdatePositionResponse pos_response = + std::get( + test_objs.reporter.messages.back()); + REQUIRE(pos_response.encoder_pulses == 40000); } } } @@ -354,7 +359,7 @@ SCENARIO("collision while homed") { test_objs.handler.run_interrupt(); } // An error and increase error count is sent - REQUIRE(test_objs.reporter.messages.size() == 2); + REQUIRE(test_objs.reporter.messages.size() == 3); can::messages::ErrorMessage err = std::get( test_objs.reporter.messages.front()); @@ -362,6 +367,11 @@ SCENARIO("collision while homed") { can::ids::ErrorCode::collision_detected); REQUIRE(test_objs.handler.error_handled); REQUIRE(!test_objs.hw.get_stay_enabled()); + // position reported + motor_messages::UpdatePositionResponse pos_response = + std::get( + test_objs.reporter.messages.back()); + REQUIRE(pos_response.encoder_pulses == 40000); } } } @@ -432,7 +442,7 @@ SCENARIO("A collision during position controlled move") { test_objs.handler.run_interrupt(); } // An error and increase error count is sent - REQUIRE(test_objs.reporter.messages.size() == 2); + REQUIRE(test_objs.reporter.messages.size() == 3); can::messages::ErrorMessage err = std::get( test_objs.reporter.messages.front()); @@ -440,6 +450,11 @@ SCENARIO("A collision during position controlled move") { can::ids::ErrorCode::collision_detected); REQUIRE(test_objs.handler.error_handled); REQUIRE(!test_objs.hw.get_stay_enabled()); + // position reported + motor_messages::UpdatePositionResponse pos_response = + std::get( + test_objs.reporter.messages.back()); + REQUIRE(pos_response.encoder_pulses == 200000); } } }