diff --git a/gripper/firmware/interfaces_grip_motor.cpp b/gripper/firmware/interfaces_grip_motor.cpp index 1b62950b0..ae86b4ef6 100644 --- a/gripper/firmware/interfaces_grip_motor.cpp +++ b/gripper/firmware/interfaces_grip_motor.cpp @@ -137,7 +137,7 @@ static lms::LinearMotionSystemConfig gear_config{ .encoder_pulses_per_rev = 512}; static error_tolerance_config::BrushedMotorErrorTolerance error_conf( - gear_config); + gear_config, double(GRIPPER_JAW_PWM_FREQ_HZ)); static brushed_motor::BrushedMotor grip_motor(gear_config, brushed_motor_hardware_iface, diff --git a/include/can/core/messages.hpp b/include/can/core/messages.hpp index a08458c3a..cc4b92e0a 100644 --- a/include/can/core/messages.hpp +++ b/include/can/core/messages.hpp @@ -1524,17 +1524,17 @@ struct GripperJawStateResponse struct SetGripperJawHoldoffRequest : BaseMessage { uint32_t message_index; - uint32_t holdoff_ticks; + uint32_t holdoff_ms; template static auto parse(Input body, Limit limit) -> SetGripperJawHoldoffRequest { - uint32_t holdoff_ticks = 0; + 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_ticks); + body = bit_utils::bytes_to_int(body, limit, holdoff_ms); return SetGripperJawHoldoffRequest{.message_index = msg_ind, - .holdoff_ticks = holdoff_ticks}; + .holdoff_ms = holdoff_ms}; } auto operator==(const SetGripperJawHoldoffRequest& other) const -> bool = default; @@ -1545,13 +1545,13 @@ using GripperJawHoldoffRequest = Empty; struct GripperJawHoldoffResponse : BaseMessage { uint32_t message_index; - uint32_t holdoff_ticks; + 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_ticks), - iter, limit); + iter = bit_utils::int_to_bytes(static_cast(holdoff_ms), iter, + limit); return iter - body; } 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 471fad2a0..e0ed34c5c 100644 --- a/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp +++ b/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp @@ -133,11 +133,13 @@ class MotionController { void set_idle_holdoff( const can::messages::SetGripperJawHoldoffRequest& can_msg) { - error_config.update_idle_holdoff_ticks(can_msg.holdoff_ticks); + error_config.update_idle_holdoff_ticks( + fixed_point_to_float(can_msg.holdoff_ms, S15Q16_RADIX)); } auto get_idle_holdoff() -> uint32_t { - return error_config.idle_holdoff_ticks; + return convert_to_fixed_point(error_config.idle_holdoff_ticks, + S15Q16_RADIX); } template diff --git a/include/motor-control/core/brushed_motor/error_tolerance_config.hpp b/include/motor-control/core/brushed_motor/error_tolerance_config.hpp index 5a6fbac1c..a35614df9 100644 --- a/include/motor-control/core/brushed_motor/error_tolerance_config.hpp +++ b/include/motor-control/core/brushed_motor/error_tolerance_config.hpp @@ -11,20 +11,21 @@ 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 uint32_t IDLE_HOLDOFF_TICKS = 32; +static constexpr double IDLE_HOLDOFF_MS = 1; class BrushedMotorErrorTolerance { public: BrushedMotorErrorTolerance( - lms::LinearMotionSystemConfig& gearbox_config) - : gear_conf(gearbox_config) { + lms::LinearMotionSystemConfig& gearbox_config, + double pwm_timer_freq) + : gear_conf(gearbox_config), pwm_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(IDLE_HOLDOFF_TICKS); + idle_holdoff_ticks = uint32_t(pwm_freq / 1000 * IDLE_HOLDOFF_MS); } void update_tolerance(double pos_error, double unwanted_movement) { @@ -34,10 +35,11 @@ class BrushedMotorErrorTolerance { int32_t(gear_conf.get_encoder_pulses_per_mm() * unwanted_movement); } - void update_idle_holdoff_ticks(uint32_t holdoff_ticks) { - idle_holdoff_ticks = holdoff_ticks; + void update_idle_holdoff_ticks(double holdoff_ms) { + idle_holdoff_ticks = uint32_t(pwm_freq / 1000 * holdoff_ms); } lms::LinearMotionSystemConfig& gear_conf; + double pwm_freq; int32_t acceptable_position_error = 0; int32_t unwanted_movement_threshold = 0; uint32_t idle_holdoff_ticks = 0; 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 99c2511cb..850cec34b 100644 --- a/include/motor-control/core/tasks/brushed_motion_controller_task.hpp +++ b/include/motor-control/core/tasks/brushed_motion_controller_task.hpp @@ -133,7 +133,7 @@ class MotionControllerMessageHandler { void handle(const can::messages::GripperJawHoldoffRequest& m) { auto ticks = controller.get_idle_holdoff(); can::messages::GripperJawHoldoffResponse msg{ - .message_index = m.message_index, .holdoff_ticks = ticks}; + .message_index = m.message_index, .holdoff_ms = ticks}; can_client.send_can_message(can::ids::NodeId::host, msg); }