feat(gripper): add can message to handle updating gripper jaw idle va…

…lue (#735) * add can message to handle updating gripper jaw idle value * format gripper * format * format can * fix tests * use fixed point~ * lint & format
Opentrons · Jan 2, 2024 · 14edd4c · 14edd4c
1 parent de7fbaf
commit 14edd4c
Show file tree

Hide file tree

Showing 15 changed files with 166 additions and 29 deletions.
diff --git a/gripper/firmware/interfaces_grip_motor.cpp b/gripper/firmware/interfaces_grip_motor.cpp
@@ -16,6 +16,7 @@
 
 constexpr uint32_t PWM_MAX = 60;
 constexpr uint32_t PWM_MIN = 7;
+auto constexpr pwm_freq = double(GRIPPER_JAW_PWM_FREQ_HZ);
 
 struct motor_hardware::UsageEEpromConfig brushed_usage_config {
     std::array<UsageRequestSet, 3> {
@@ -74,7 +75,7 @@ struct motor_hardware::BrushedHardwareConfig brushed_motor_conf {
             .port = ESTOP_IN_PORT,
             .pin = ESTOP_IN_PIN,
             .active_setting = GPIO_PIN_RESET},
-    .encoder_interrupt_freq = double(GRIPPER_JAW_PWM_FREQ_HZ),
+    .encoder_interrupt_freq = pwm_freq,
 
     /* the expected behavior with these pid values is that the motor runs at
      * full power until it's about 2mm away and then it slows down on that
@@ -137,7 +138,7 @@ static lms::LinearMotionSystemConfig<lms::GearBoxConfig> gear_config{
     .encoder_pulses_per_rev = 512};
 
 static error_tolerance_config::BrushedMotorErrorTolerance error_conf(
-    gear_config);
+    gear_config, pwm_freq);
 
 static brushed_motor::BrushedMotor grip_motor(gear_config,
                                               brushed_motor_hardware_iface,

diff --git a/gripper/simulator/interfaces.cpp b/gripper/simulator/interfaces.cpp
@@ -119,7 +119,8 @@ static auto gear_conf = lms::LinearMotionSystemConfig<lms::GearBoxConfig>{
     .microstep = 0,
     .encoder_pulses_per_rev = 512};
 
-static error_tolerance_config::BrushedMotorErrorTolerance error_conf(gear_conf);
+static error_tolerance_config::BrushedMotorErrorTolerance error_conf(gear_conf,
+                                                                     32000);
 
 static auto grip_motor = brushed_motor::BrushedMotor(
     gear_conf, brushed_motor_hardware_iface, brushed_motor_driver_iface,

diff --git a/include/bootloader/core/ids.h b/include/bootloader/core/ids.h
@@ -69,6 +69,9 @@ typedef enum {
     can_messageid_set_gripper_error_tolerance = 0x47,
     can_messageid_gripper_jaw_state_request = 0x48,
     can_messageid_gripper_jaw_state_response = 0x49,
+    can_messageid_set_gripper_jaw_holdoff_request = 0x401,
+    can_messageid_gripper_jaw_holdoff_request = 0x402,
+    can_messageid_gripper_jaw_holdoff_response = 0x403,
     can_messageid_acknowledgement = 0x50,
     can_messageid_read_presence_sensing_voltage_request = 0x600,
     can_messageid_read_presence_sensing_voltage_response = 0x601,

diff --git a/include/can/core/ids.hpp b/include/can/core/ids.hpp
@@ -71,6 +71,9 @@ enum class MessageId {
     set_gripper_error_tolerance = 0x47,
     gripper_jaw_state_request = 0x48,
     gripper_jaw_state_response = 0x49,
+    set_gripper_jaw_holdoff_request = 0x401,
+    gripper_jaw_holdoff_request = 0x402,
+    gripper_jaw_holdoff_response = 0x403,
     acknowledgement = 0x50,
     read_presence_sensing_voltage_request = 0x600,
     read_presence_sensing_voltage_response = 0x601,

diff --git a/include/can/core/message_handlers/motion.hpp b/include/can/core/message_handlers/motion.hpp
@@ -49,7 +49,8 @@ class BrushedMotionHandler {
         std::variant<std::monostate, DisableMotorRequest, EnableMotorRequest,
                      ReadLimitSwitchRequest, MotorPositionRequest,
                      SetGripperErrorToleranceRequest, GetMotorUsageRequest,
-                     GripperJawStateRequest>;
+                     GripperJawStateRequest, SetGripperJawHoldoffRequest,
+                     GripperJawHoldoffRequest>;
 
     BrushedMotionHandler(BrushedMotionTaskClient &motion_client)
         : motion_client{motion_client} {}

diff --git a/include/can/core/messages.hpp b/include/can/core/messages.hpp
@@ -1521,6 +1521,44 @@ struct GripperJawStateResponse
         -> bool = default;
 };
 
+struct SetGripperJawHoldoffRequest
+    : BaseMessage<MessageId::set_gripper_jaw_holdoff_request> {
+    uint32_t message_index;
+    uint32_t holdoff_ms;
+
+    template <bit_utils::ByteIterator Input, typename Limit>
+    static auto parse(Input body, Limit limit) -> SetGripperJawHoldoffRequest {
+        uint32_t holdoff_ms = 0;
+        uint32_t msg_ind = 0;
+
+        body = bit_utils::bytes_to_int(body, limit, msg_ind);
+        body = bit_utils::bytes_to_int(body, limit, holdoff_ms);
+        return SetGripperJawHoldoffRequest{.message_index = msg_ind,
+                                           .holdoff_ms = holdoff_ms};
+    }
+    auto operator==(const SetGripperJawHoldoffRequest& other) const
+        -> bool = default;
+};
+
+using GripperJawHoldoffRequest = Empty<MessageId::gripper_jaw_holdoff_request>;
+
+struct GripperJawHoldoffResponse
+    : BaseMessage<MessageId::gripper_jaw_holdoff_response> {
+    uint32_t message_index;
+    uint32_t holdoff_ms;
+
+    template <bit_utils::ByteIterator Output, typename Limit>
+    auto serialize(Output body, Limit limit) const -> uint8_t {
+        auto iter = bit_utils::int_to_bytes(message_index, body, limit);
+        iter = bit_utils::int_to_bytes(static_cast<uint32_t>(holdoff_ms), iter,
+                                       limit);
+        return iter - body;
+    }
+
+    auto operator==(const GripperJawHoldoffResponse& other) const
+        -> bool = default;
+};
+
 /**
  * A variant of all message types we might send..
  */
@@ -1536,7 +1574,7 @@ using ResponseMessageType = std::variant<
     BindSensorOutputResponse, GripperInfoResponse, TipActionResponse,
     PeripheralStatusResponse, BrushedMotorConfResponse,
     UpdateMotorPositionEstimationResponse, BaselineSensorResponse,
-    PushTipPresenceNotification, GetMotorUsageResponse,
-    GripperJawStateResponse>;
+    PushTipPresenceNotification, GetMotorUsageResponse, GripperJawStateResponse,
+    GripperJawHoldoffResponse>;
 
 }  // namespace can::messages
diff --git a/include/gripper/core/can_task.hpp b/include/gripper/core/can_task.hpp
@@ -48,7 +48,9 @@ using BrushedMotionDispatchTarget = can::dispatch::DispatchParseTarget<
     can::messages::DisableMotorRequest, can::messages::EnableMotorRequest,
     can::messages::ReadLimitSwitchRequest, can::messages::MotorPositionRequest,
     can::messages::SetGripperErrorToleranceRequest,
-    can::messages::GetMotorUsageRequest, can::messages::GripperJawStateRequest>;
+    can::messages::GetMotorUsageRequest, can::messages::GripperJawStateRequest,
+    can::messages::SetGripperJawHoldoffRequest,
+    can::messages::GripperJawHoldoffRequest>;
 using BrushedMoveGroupDispatchTarget = can::dispatch::DispatchParseTarget<
     can::message_handlers::move_group::BrushedMoveGroupHandler<
         g_tasks::QueueClient>,

diff --git a/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp b/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp
@@ -131,6 +131,17 @@ class MotionController {
                                  S15Q16_RADIX));
     }
 
+    void set_idle_holdoff(
+        const can::messages::SetGripperJawHoldoffRequest& can_msg) {
+        error_config.update_idle_holdoff_ticks(
+            fixed_point_to_float(can_msg.holdoff_ms, S15Q16_RADIX));
+    }
+
+    auto get_idle_holdoff_ms() -> uint32_t {
+        return convert_to_fixed_point(error_config.get_holdoff_ms(),
+                                      S15Q16_RADIX);
+    }
+
     template <usage_storage_task::TaskClient UsageClient>
     void send_usage_data(uint32_t message_index, UsageClient& usage_client) {
         usage_messages::GetUsageRequest req = {

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
@@ -25,11 +25,6 @@ using namespace motor_messages;
  *
  */
 
-static constexpr uint32_t HOLDOFF_TICKS =
-    32;  // hold off for 1 ms (with a 32k Hz timer)
-// using the logic analyzer it takes about 0.2-0.3 ms for the output
-// to stablize after changing directions of the PWM
-
 template <template <class> class QueueImpl,
           move_status_reporter_task::BrushedTaskClient StatusClient>
 requires MessageQueue<QueueImpl<BrushedMove>, BrushedMove>
@@ -207,7 +202,7 @@ class BrushedMotorInterruptHandler {
             }
             cancel_and_clear_moves(can::ids::ErrorCode::stop_requested,
                                    can::ids::ErrorSeverity::warning);
-        } else if (tick < HOLDOFF_TICKS) {
+        } else if (tick < error_conf.idle_holdoff_ticks) {
             tick++;
         } else if (_has_active_move) {
             execute_active_move();
@@ -221,7 +216,7 @@ class BrushedMotorInterruptHandler {
          * the encoder is idle */
         return !(buffered_move.stop_condition ==
                      MoveStopCondition::limit_switch ||
-                 tick < HOLDOFF_TICKS);
+                 tick < error_conf.idle_holdoff_ticks);
     }
 
     void set_enc_idle_state(bool val) {
@@ -417,6 +412,10 @@ class BrushedMotorInterruptHandler {
 
     auto has_active_move() -> bool { return _has_active_move.load(); }
 
+    [[nodiscard]] auto get_idle_holdoff_ticks() const -> uint32_t {
+        return error_conf.idle_holdoff_ticks;
+    }
+
     std::atomic<bool> is_idle = true;
     uint32_t tick = 0;
     uint32_t timeout_ticks = 0;

diff --git a/include/motor-control/core/brushed_motor/error_tolerance_config.hpp b/include/motor-control/core/brushed_motor/error_tolerance_config.hpp
@@ -8,17 +8,24 @@ namespace error_tolerance_config {
 static constexpr double ACCEPTABLE_DISTANCE_TOLERANCE_MM = 2;
 static constexpr double UNWANTED_MOVEMENT_DISTANCE_MM = 2;
 
+// hold off for 1 ms (with a 32k Hz timer)
+// using the logic analyzer it takes about 0.2-0.3 ms for the output
+// to stablize after changing directions of the PWM
+static constexpr double IDLE_HOLDOFF_MS = 1;
+
 class BrushedMotorErrorTolerance {
   public:
     BrushedMotorErrorTolerance(
-        lms::LinearMotionSystemConfig<lms::GearBoxConfig>& gearbox_config)
-        : gear_conf(gearbox_config) {
+        lms::LinearMotionSystemConfig<lms::GearBoxConfig>& gearbox_config,
+        double pwm_timer_freq)
+        : gear_conf(gearbox_config), timer_freq(pwm_timer_freq) {
         acceptable_position_error =
             int32_t(gear_conf.get_encoder_pulses_per_mm() *
                     ACCEPTABLE_DISTANCE_TOLERANCE_MM);
         unwanted_movement_threshold =
             int32_t(gear_conf.get_encoder_pulses_per_mm() *
                     UNWANTED_MOVEMENT_DISTANCE_MM);
+        idle_holdoff_ticks = uint32_t(timer_freq / 1000 * IDLE_HOLDOFF_MS);
     }
 
     void update_tolerance(double pos_error, double unwanted_movement) {
@@ -27,9 +34,20 @@ class BrushedMotorErrorTolerance {
         unwanted_movement_threshold =
             int32_t(gear_conf.get_encoder_pulses_per_mm() * unwanted_movement);
     }
+
+    void update_idle_holdoff_ticks(double holdoff_ms) {
+        idle_holdoff_ticks = uint32_t(timer_freq / 1000 * holdoff_ms);
+    }
+
+    [[nodiscard]] auto get_holdoff_ms() const -> double {
+        return double(idle_holdoff_ticks * 1000 / timer_freq);
+    }
+
     lms::LinearMotionSystemConfig<lms::GearBoxConfig>& gear_conf;
+    double timer_freq;
     int32_t acceptable_position_error = 0;
     int32_t unwanted_movement_threshold = 0;
+    uint32_t idle_holdoff_ticks = 0;
 };
 
 }  // namespace error_tolerance_config
diff --git a/include/motor-control/core/tasks/brushed_motion_controller_task.hpp b/include/motor-control/core/tasks/brushed_motion_controller_task.hpp
@@ -124,6 +124,19 @@ class MotionControllerMessageHandler {
         can_client.send_can_message(can::ids::NodeId::host, msg);
     }
 
+    void handle(const can::messages::SetGripperJawHoldoffRequest& m) {
+        controller.set_idle_holdoff(m);
+        can_client.send_can_message(can::ids::NodeId::host,
+                                    can::messages::ack_from_request(m));
+    }
+
+    void handle(const can::messages::GripperJawHoldoffRequest& m) {
+        auto holdoff = controller.get_idle_holdoff_ms();
+        can::messages::GripperJawHoldoffResponse msg{
+            .message_index = m.message_index, .holdoff_ms = holdoff};
+        can_client.send_can_message(can::ids::NodeId::host, msg);
+    }
+
     brushed_motion_controller::MotionController<MEConfig>& controller;
     CanClient& can_client;
     UsageClient& usage_client;

diff --git a/include/motor-control/core/tasks/messages.hpp b/include/motor-control/core/tasks/messages.hpp
@@ -45,7 +45,9 @@ using BrushedMotionControllerTaskMessage = std::variant<
     can::messages::AddBrushedLinearMoveRequest, can::messages::StopRequest,
     can::messages::ReadLimitSwitchRequest, can::messages::MotorPositionRequest,
     can::messages::SetGripperErrorToleranceRequest,
-    can::messages::GetMotorUsageRequest, can::messages::GripperJawStateRequest>;
+    can::messages::GetMotorUsageRequest, can::messages::GripperJawStateRequest,
+    can::messages::SetGripperJawHoldoffRequest,
+    can::messages::GripperJawHoldoffRequest>;
 
 using BrushedMoveGroupTaskMessage = std::variant<
     std::monostate, can::messages::ClearAllMoveGroupsRequest,

diff --git a/include/motor-control/tests/mock_brushed_motor_components.hpp b/include/motor-control/tests/mock_brushed_motor_components.hpp
@@ -170,6 +170,18 @@ struct MockBrushedMotionController {
             fixed_point_to_float(can_msg.max_unwanted_movement_mm,
                                  S15Q16_RADIX));
     }
+
+    void set_idle_holdoff(
+        const can::messages::SetGripperJawHoldoffRequest& can_msg) {
+        error_config.update_idle_holdoff_ticks(
+            fixed_point_to_float(can_msg.holdoff_ms, S15Q16_RADIX));
+    }
+
+    [[nodiscard]] auto get_idle_holdoff_ms() const -> uint32_t {
+        return convert_to_fixed_point(error_config.get_holdoff_ms(),
+                                      S15Q16_RADIX);
+    }
+
     BrushedMotorErrorTolerance& error_config;
 };
 

diff --git a/motor-control/tests/test_brushed_motor_error_tolerance_handling.cpp b/motor-control/tests/test_brushed_motor_error_tolerance_handling.cpp
@@ -16,7 +16,7 @@ auto _gear_config = lms::LinearMotionSystemConfig<lms::GearBoxConfig>{
     .microstep = 0,
     .encoder_pulses_per_rev = 512};
 auto _error_config =
-    error_tolerance_config::BrushedMotorErrorTolerance{_gear_config};
+    error_tolerance_config::BrushedMotorErrorTolerance{_gear_config, 32000};
 
 SCENARIO("testing error tolerance handling") {
     GIVEN("a motion controller and motor interrupt handler") {
@@ -45,4 +45,26 @@ SCENARIO("testing error tolerance handling") {
             }
         }
     }
+}
+
+SCENARIO("testing brushed motor holdoff value") {
+    GIVEN("a motion controller and motor interrupt handler") {
+        test_mocks::MockBrushedMotionController controller(_error_config);
+        CHECK(_error_config.get_holdoff_ms() == 1.0);
+        WHEN("a set idle holdoff request is handled") {
+            auto msg = can::messages::SetGripperJawHoldoffRequest{
+                .message_index = 0,
+                .holdoff_ms = 1310720,  // 20 ms
+            };
+            controller.set_idle_holdoff(msg);
+            THEN("idle holdoff value should be updated in error config") {
+                CHECK(_error_config.get_holdoff_ms() == 20.0);
+                CHECK(_error_config.idle_holdoff_ticks == 640);
+
+                THEN("controller should get the correct holdoff ms value") {
+                    CHECK(controller.get_idle_holdoff_ms() == 1310720);
+                }
+            }
+        }
+    }
 }