report encoder pulses when something goes wrong (#722)

Opentrons · Sep 19, 2023 · 0f1df6a · 0f1df6a
1 parent ae4a7ff
commit 0f1df6a
Show file tree

Hide file tree

Showing 2 changed files with 37 additions and 5 deletions.
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
@@ -138,17 +138,18 @@ 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
                 // movements that occur from motion and vibration
                 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
@@ -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<can::messages::ErrorMessage>(
                         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<motor_messages::UpdatePositionResponse>(
+                        test_objs.reporter.messages.back());
+                REQUIRE(pos_response.encoder_pulses == 40000);
             }
         }
     }
@@ -354,14 +359,19 @@ 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<can::messages::ErrorMessage>(
                         test_objs.reporter.messages.front());
                 REQUIRE(err.error_code ==
                         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<motor_messages::UpdatePositionResponse>(
+                        test_objs.reporter.messages.back());
+                REQUIRE(pos_response.encoder_pulses == 40000);
             }
         }
     }
@@ -432,14 +442,19 @@ 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<can::messages::ErrorMessage>(
                         test_objs.reporter.messages.front());
                 REQUIRE(err.error_code ==
                         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<motor_messages::UpdatePositionResponse>(
+                        test_objs.reporter.messages.back());
+                REQUIRE(pos_response.encoder_pulses == 200000);
             }
         }
     }