From 14edd4c8e3c9eac953e1c4feba908265a9ac71da Mon Sep 17 00:00:00 2001 From: Alise Au <20424172+ahiuchingau@users.noreply.github.com> Date: Tue, 2 Jan 2024 12:53:07 -0500 Subject: [PATCH] feat(gripper): add can message to handle updating gripper jaw idle value (#735) * add can message to handle updating gripper jaw idle value * format gripper * format * format can * fix tests * use fixed point~ * lint & format --- gripper/firmware/interfaces_grip_motor.cpp | 5 ++- gripper/simulator/interfaces.cpp | 3 +- include/bootloader/core/ids.h | 3 ++ include/can/core/ids.hpp | 3 ++ include/can/core/message_handlers/motion.hpp | 3 +- include/can/core/messages.hpp | 42 ++++++++++++++++++- include/gripper/core/can_task.hpp | 4 +- .../brushed_motion_controller.hpp | 11 +++++ .../brushed_motor_interrupt_handler.hpp | 13 +++--- .../brushed_motor/error_tolerance_config.hpp | 22 +++++++++- .../tasks/brushed_motion_controller_task.hpp | 13 ++++++ include/motor-control/core/tasks/messages.hpp | 4 +- .../tests/mock_brushed_motor_components.hpp | 12 ++++++ ...brushed_motor_error_tolerance_handling.cpp | 24 ++++++++++- .../test_brushed_motor_interrupt_handler.cpp | 33 ++++++++++----- 15 files changed, 166 insertions(+), 29 deletions(-) diff --git a/gripper/firmware/interfaces_grip_motor.cpp b/gripper/firmware/interfaces_grip_motor.cpp index 1b62950b0..5ecc424f4 100644 --- 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 { @@ -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 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 index 3f7374a71..f3dfd2a14 100644 --- a/gripper/simulator/interfaces.cpp +++ b/gripper/simulator/interfaces.cpp @@ -119,7 +119,8 @@ static auto gear_conf = lms::LinearMotionSystemConfig{ .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 index 71ee3a3dc..9afd0191c 100644 --- 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 index 3633df880..792eada8a 100644 --- 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 index d8575df3c..203407ab7 100644 --- a/include/can/core/message_handlers/motion.hpp +++ b/include/can/core/message_handlers/motion.hpp @@ -49,7 +49,8 @@ class BrushedMotionHandler { std::variant; + 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 index 2932cd005..cc4b92e0a 100644 --- a/include/can/core/messages.hpp +++ b/include/can/core/messages.hpp @@ -1521,6 +1521,44 @@ struct GripperJawStateResponse -> bool = default; }; +struct SetGripperJawHoldoffRequest + : BaseMessage { + uint32_t message_index; + uint32_t holdoff_ms; + + template + 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; + +struct GripperJawHoldoffResponse + : BaseMessage { + uint32_t message_index; + uint32_t holdoff_ms; + + template + 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(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 index 0b560acfe..e04affca3 100644 --- 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 index a7a40cacd..91610651c 100644 --- 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 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 index 5fabf2b0a..1c417adfe 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 @@ -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