From a9d17dc209ee5ca40479ce3f5b7343bd4dd48daf Mon Sep 17 00:00:00 2001 From: gqsdjhh <1369103595@qq.com> Date: Mon, 13 Jul 2026 13:44:38 +0800 Subject: [PATCH] feat(sentry): add chassis climb control --- rmcs_ws/src/rmcs_bringup/config/sentry.yaml | 50 +- rmcs_ws/src/rmcs_core/plugins.xml | 1 + .../chassis/chassis_climber_controller.cpp | 722 ++++++++++++++++++ .../controller/chassis/chassis_controller.cpp | 125 ++- .../chassis/chassis_power_controller.cpp | 5 + rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp | 79 +- 6 files changed, 933 insertions(+), 49 deletions(-) create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml index a9a092d4..03595894 100644 --- a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml @@ -17,19 +17,20 @@ rmcs_executor: - rmcs_core::controller::pid::PidController -> left_friction_velocity_pid_controller - rmcs_core::controller::pid::PidController -> right_friction_velocity_pid_controller - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller + - rmcs_core::controller::chassis::ChassisClimberController -> climber_controller - rmcs_core::controller::chassis::ChassisController -> chassis_controller - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - rmcs_core::controller::chassis::SteeringWheelController -> steering_wheel_controller # - rmcs::navigation::Navigation -> rmcs_navigation - # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster - - rmcs::AutoAimCapturerComponent -> auto_aim_capturer + # - rmcs::AutoAimCapturerComponent -> auto_aim_capturer # - rmcs::AutoAimPlayerComponent -> auto_aim_player - - rmcs::AutoAimRecorderComponent -> auto_aim_recorder - - rmcs::AutoAimComponent -> auto_aim_component + # - rmcs::AutoAimRecorderComponent -> auto_aim_recorder + # - rmcs::AutoAimComponent -> auto_aim_component auto_aim_capturer: ros__parameters: @@ -55,18 +56,18 @@ value_broadcaster: # The positive direction is the one that battery exists sentry_hardware: ros__parameters: - board_serial_bottom_board: "af-79cf" + board_serial_bottom_board: "af-b4e5" board_serial_gimbal_board: "af-8b8b" - pitch_motor_zero_point: 12158 + pitch_motor_zero_point: 17241 - bottom_yaw_motor_zero_point: 9445 - top_yaw_motor_zero_point: 33409 + bottom_yaw_motor_zero_point: 65414 + top_yaw_motor_zero_point: 32736 - left_front_zero_point: 6775 - left_back_zero_point: 5187 - right_back_zero_point: 239 - right_front_zero_point: 3750 + left_front_zero_point: 5776 + left_back_zero_point: 3784 + right_back_zero_point: 3048 + right_front_zero_point: 5135 rmcs_navigation: ros__parameters: @@ -122,6 +123,31 @@ chassis_controller: following_velocity_ki: 0.001 following_velocity_kd: 0.0 +climber_controller: + ros__parameters: + front_climber_velocity: 20.0 + back_climber_velocity: 30.0 + auto_climb_support_retract_velocity_fast: 60.0 + auto_climb_support_retract_velocity_slow: 20.0 + auto_climb_approach_chassis_velocity: 1.0 + auto_climb_support_deploy_chassis_velocity: 0.3 + auto_climb_support_retract_chassis_velocity: 0.3 + auto_climb_dash_chassis_velocity: 3.0 + first_stair_dash_leveled_pitch_threshold: 0.05 + second_stair_dash_leveled_pitch_threshold: -0.09 + sync_coefficient: 0.2 + first_stair_approach_pitch: 0.585 + second_stair_approach_pitch: 0.365 + front_kp: 1.0 + front_ki: 0.0 + front_kd: 0.5 + front_power_estimate_bias: 0.0 + front_power_estimate_k_tau2: 1.0 + front_power_estimate_k_mech: 1.0 + back_kp: 0.5 + back_ki: 0.0 + back_kd: 0.0 + friction_wheel_controller: ros__parameters: friction_wheels: diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 22a3de47..4b0932fc 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -15,6 +15,7 @@ + diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp new file mode 100644 index 00000000..cfd1a669 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp @@ -0,0 +1,722 @@ +#include "controller/pid/matrix_pid_calculator.hpp" +#include "rmcs_msgs/keyboard.hpp" +#include "rmcs_msgs/switch.hpp" +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::controller::chassis { + +namespace { +double estimate_front_power( + double left_torque, double right_torque, double left_velocity, double right_velocity, + double bias, double k_tau2, double k_mech) { + + if (!std::isfinite(left_torque) || !std::isfinite(right_torque)) + return 0.0; + + return bias + k_tau2 * (std::pow(left_torque, 2) + std::pow(right_torque, 2)) + + k_mech + * (std::abs(left_torque * left_velocity) + std::abs(right_torque * right_velocity)); +} +} // namespace + +enum class AutoClimbState { IDLE, ALIGN, APPROACH, SUPPORT_DEPLOY, DASH, SUPPORT_RETRACT }; + +class ChassisClimberController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + ChassisClimberController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , logger_(get_logger()) + , front_velocity_pid_calculator_( + get_parameter("front_kp").as_double(), get_parameter("front_ki").as_double(), + get_parameter("front_kd").as_double()) + , back_velocity_pid_calculator_( + get_parameter("back_kp").as_double(), get_parameter("back_ki").as_double(), + get_parameter("back_kd").as_double()) { + + track_velocity_max_ = get_parameter("front_climber_velocity").as_double(); + climber_back_control_velocity_abs_ = get_parameter("back_climber_velocity").as_double(); + auto_climb_support_retract_velocity_fast_abs_ = + get_parameter("auto_climb_support_retract_velocity_fast").as_double(); + auto_climb_support_retract_velocity_slow_abs_ = + get_parameter("auto_climb_support_retract_velocity_slow").as_double(); + auto_climb_approach_chassis_velocity_ = + get_parameter("auto_climb_approach_chassis_velocity").as_double(); + auto_climb_support_deploy_chassis_velocity_ = + get_parameter("auto_climb_support_deploy_chassis_velocity").as_double(); + auto_climb_support_retract_chassis_velocity_ = + get_parameter("auto_climb_support_retract_chassis_velocity").as_double(); + auto_climb_dash_chassis_velocity_ = + get_parameter("auto_climb_dash_chassis_velocity").as_double(); + first_stair_dash_leveled_pitch_threshold_ = + get_parameter("first_stair_dash_leveled_pitch_threshold").as_double(); + second_stair_dash_leveled_pitch_threshold_ = + get_parameter("second_stair_dash_leveled_pitch_threshold").as_double(); + sync_coefficient_ = get_parameter("sync_coefficient").as_double(); + first_stair_approach_pitch_ = get_parameter("first_stair_approach_pitch").as_double(); + second_stair_approach_pitch_ = get_parameter("second_stair_approach_pitch").as_double(); + front_power_estimate_bias_ = get_parameter("front_power_estimate_bias").as_double(); + front_power_estimate_k_tau2_ = get_parameter("front_power_estimate_k_tau2").as_double(); + front_power_estimate_k_mech_ = get_parameter("front_power_estimate_k_mech").as_double(); + + register_output( + "/chassis/climber/left_front_motor/requested_control_torque", + climber_front_left_requested_control_torque_, nan_); + register_output( + "/chassis/climber/right_front_motor/requested_control_torque", + climber_front_right_requested_control_torque_, nan_); + register_output( + "/chassis/climber/left_back_motor/control_torque", climber_back_left_control_torque_, + nan_); + register_output( + "/chassis/climber/right_back_motor/control_torque", climber_back_right_control_torque_, + nan_); + register_output("/chassis/climbing_forward_velocity", climbing_forward_velocity_, nan_); + register_output("/chassis/climber/auto_climb_active", auto_climb_active_, false); + register_output( + "/chassis/climber/front/power_budget_active", front_power_budget_active_, false); + register_output( + "/chassis/climber/front/power_demand_estimate", front_power_demand_estimate_, 0.0); + + register_input("/chassis/climber/left_front_motor/velocity", climber_front_left_velocity_); + register_input( + "/chassis/climber/right_front_motor/velocity", climber_front_right_velocity_); + register_input("/chassis/climber/left_back_motor/velocity", climber_back_left_velocity_); + register_input("/chassis/climber/right_back_motor/velocity", climber_back_right_velocity_); + + register_input("/chassis/climber/left_back_motor/torque", climber_back_left_torque_); + register_input("/chassis/climber/right_back_motor/torque", climber_back_right_torque_); + + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + register_input("/remote/keyboard", keyboard_); + register_input("/remote/rotary_knob_switch", rotary_knob_switch_); + register_input("/chassis/pitch_imu", chassis_pitch_imu_); + + register_input("/gimbal/yaw/angle", gimbal_yaw_angle_); + register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_); + register_input("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); + + front_power_limiter_ = create_partner_component( + get_component_name() + "_front_power_limiter", front_power_estimate_bias_, + front_power_estimate_k_tau2_, front_power_estimate_k_mech_); + } + + void update() override { + using namespace rmcs_msgs; + auto switch_right = *switch_right_; + auto switch_left = *switch_left_; + auto keyboard = *keyboard_; + auto rotary_knob_switch = *rotary_knob_switch_; + + bool rotary_knob_to_down = + (last_rotary_knob_switch_ != Switch::DOWN && rotary_knob_switch == Switch::DOWN); + bool rotary_knob_from_down = + (last_rotary_knob_switch_ == Switch::DOWN && rotary_knob_switch != Switch::DOWN); + + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + reset_all_controls(); + } else { + handle_auto_climb_requests( + (!last_keyboard_.g && keyboard.g) || rotary_knob_to_down, rotary_knob_from_down, + rotary_knob_switch); + + if (auto_climb_state_ != AutoClimbState::IDLE) { + stop_manual_support(); + apply_climb_control(update_auto_climb_control()); + } else { + apply_climb_control(update_manual_support_control(keyboard)); + } + } + + last_keyboard_ = keyboard; + last_rotary_knob_switch_ = rotary_knob_switch; + } + +private: + struct AutoClimbControl { + double front_track_velocity = nan_; + double back_climber_velocity = nan_; + double override_chassis_vx = nan_; + }; + + class ChassisClimberFrontPowerLimiter : public rmcs_executor::Component { + public: + ChassisClimberFrontPowerLimiter(double bias, double k_tau2, double k_mech) + : front_power_estimate_bias_(bias) + , front_power_estimate_k_tau2_(k_tau2) + , front_power_estimate_k_mech_(k_mech) { + register_input( + "/chassis/climber/left_front_motor/requested_control_torque", + left_requested_control_torque_); + register_input( + "/chassis/climber/right_front_motor/requested_control_torque", + right_requested_control_torque_); + register_input("/chassis/climber/left_front_motor/velocity", left_velocity_); + register_input("/chassis/climber/right_front_motor/velocity", right_velocity_); + register_input("/chassis/climber/left_front_motor/max_torque", left_max_torque_); + register_input("/chassis/climber/right_front_motor/max_torque", right_max_torque_); + register_input("/chassis/climber/front/control_power_limit", control_power_limit_); + + register_output( + "/chassis/climber/left_front_motor/control_torque", left_control_torque_, nan_); + register_output( + "/chassis/climber/right_front_motor/control_torque", right_control_torque_, nan_); + register_output( + "/chassis/climber/front/actual_power_estimate", actual_power_estimate_, 0.0); + } + + void update() override { + const double left_requested = *left_requested_control_torque_; + const double right_requested = *right_requested_control_torque_; + + if (!std::isfinite(left_requested) || !std::isfinite(right_requested)) { + *left_control_torque_ = nan_; + *right_control_torque_ = nan_; + *actual_power_estimate_ = 0.0; + return; + } + + if (*control_power_limit_ <= 0.0) { + *left_control_torque_ = 0.0; + *right_control_torque_ = 0.0; + *actual_power_estimate_ = estimate_front_power( + *left_control_torque_, *right_control_torque_, *left_velocity_, + *right_velocity_, front_power_estimate_bias_, front_power_estimate_k_tau2_, + front_power_estimate_k_mech_); + return; + } + + const double left_torque = + std::clamp(left_requested, -*left_max_torque_, *left_max_torque_); + const double right_torque = + std::clamp(right_requested, -*right_max_torque_, *right_max_torque_); + + const double estimated_power = estimate_front_power( + left_torque, right_torque, *left_velocity_, *right_velocity_, + front_power_estimate_bias_, front_power_estimate_k_tau2_, + front_power_estimate_k_mech_); + + if (estimated_power <= *control_power_limit_ || estimated_power <= 0.0) { + *left_control_torque_ = left_torque; + *right_control_torque_ = right_torque; + *actual_power_estimate_ = estimated_power; + return; + } + + const double scale = std::clamp(*control_power_limit_ / estimated_power, 0.0, 1.0); + *left_control_torque_ = left_torque * scale; + *right_control_torque_ = right_torque * scale; + *actual_power_estimate_ = estimate_front_power( + *left_control_torque_, *right_control_torque_, *left_velocity_, *right_velocity_, + front_power_estimate_bias_, front_power_estimate_k_tau2_, + front_power_estimate_k_mech_); + } + + private: + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + + double front_power_estimate_bias_; + double front_power_estimate_k_tau2_; + double front_power_estimate_k_mech_; + + InputInterface left_requested_control_torque_; + InputInterface right_requested_control_torque_; + InputInterface left_velocity_; + InputInterface right_velocity_; + InputInterface left_max_torque_; + InputInterface right_max_torque_; + InputInterface control_power_limit_; + + OutputInterface left_control_torque_; + OutputInterface right_control_torque_; + OutputInterface actual_power_estimate_; + }; + + void handle_auto_climb_requests( + bool start_requested, bool abort_by_rotary, rmcs_msgs::Switch rotary_knob_switch) { + + if (start_requested) { + if (auto_climb_state_ == AutoClimbState::IDLE) { + start_auto_climb( + rotary_knob_switch == rmcs_msgs::Switch::UP ? "Rotary Knob" : "Keyboard G"); + } else { + abort_auto_climb("toggled again"); + } + } else if (abort_by_rotary && auto_climb_state_ != AutoClimbState::IDLE) { + abort_auto_climb("rotary knob left UP"); + } + } + + void start_auto_climb(const char* source) { + stop_manual_support(); + back_climber_zero_velocity_hold_ = false; + auto_climb_stair_index_ = 0; + auto_climb_align_stable_count_ = 0; + auto_climb_support_block_count_ = 0; + enter_auto_climb_state(AutoClimbState::ALIGN); + + RCLCPP_INFO(logger_, "Auto climb started by %s. Entering ALIGN.", source); + } + + void abort_auto_climb(const char* reason) { + stop_auto_climb(); + start_back_climber_retract("Auto climb exit"); + RCLCPP_INFO(logger_, "Auto climb aborted (%s).", reason); + } + + AutoClimbControl update_auto_climb_control() { + if (auto_climb_state_ == AutoClimbState::IDLE) + return {}; + + auto_climb_timer_++; + + switch (auto_climb_state_) { + case AutoClimbState::IDLE: return {}; + case AutoClimbState::ALIGN: return update_auto_climb_align(); + case AutoClimbState::APPROACH: return update_auto_climb_approach(); + case AutoClimbState::SUPPORT_DEPLOY: return update_auto_climb_support_deploy(); + case AutoClimbState::DASH: return update_auto_climb_dash(); + case AutoClimbState::SUPPORT_RETRACT: return update_auto_climb_support_retract(); + } + + return {}; + } + + AutoClimbControl update_manual_support_control(const rmcs_msgs::Keyboard& keyboard) { + AutoClimbControl control; + + if (keyboard.b) { + back_climber_zero_velocity_hold_ = false; + control.back_climber_velocity = climber_back_control_velocity_abs_; + return control; + } + + if (last_keyboard_.b) { + start_back_climber_retract("Manual support"); + } + + if (!manual_support_retracting_) { + if (back_climber_zero_velocity_hold_) + control.back_climber_velocity = 0.0; + return control; + } + + if (back_climber_recover_count > 1200) { + control.back_climber_velocity = -auto_climb_support_retract_velocity_slow_abs_; + } else { + control.back_climber_velocity = -auto_climb_support_retract_velocity_fast_abs_; + } + + if (is_back_climber_blocked()) + manual_support_retract_block_count_++; + else + manual_support_retract_block_count_ = 0; + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "MANUAL_SUPPORT_RETRACT: blocked_ticks=%d", + manual_support_retract_block_count_); + + if (manual_support_retract_block_count_ >= kManualSupportRetractConfirmTicks) { + stop_manual_support(); + back_climber_zero_velocity_hold_ = true; + control.back_climber_velocity = 0.0; + RCLCPP_INFO(logger_, "Manual support retract completed."); + } + + return control; + } + + AutoClimbControl update_auto_climb_align() { + AutoClimbControl control{ + .front_track_velocity = 0.0, + .back_climber_velocity = -5.0, + .override_chassis_vx = 0.0, + }; + + double gimbal_yaw_angle_error = *gimbal_yaw_angle_error_; + if (gimbal_yaw_angle_error < 0) + gimbal_yaw_angle_error += 2 * std::numbers::pi; + + double err = gimbal_yaw_angle_error + *gimbal_yaw_angle_; + while (err >= std::numbers::pi) + err -= 2 * std::numbers::pi; + while (err < -std::numbers::pi) + err += 2 * std::numbers::pi; + + double yaw_velocity = *gimbal_yaw_velocity_imu_; + bool is_aligned = std::abs(err) < kAutoClimbAlignThreshold; + bool is_stable = std::abs(yaw_velocity) < kAutoClimbAlignVelocityThreshold; + + if (is_aligned && is_stable) + auto_climb_align_stable_count_++; + else + auto_climb_align_stable_count_ = 0; + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "ALIGN: err=%.3f, yaw_velocity=%.3f, stable_ticks=%d", err, + yaw_velocity, auto_climb_align_stable_count_); + + if (auto_climb_align_stable_count_ >= kAutoClimbAlignConfirmTicks) { + enter_auto_climb_state(AutoClimbState::APPROACH); + RCLCPP_INFO(logger_, "Chassis aligned. Entering APPROACH."); + } + + return control; + } + + AutoClimbControl update_auto_climb_approach() { + AutoClimbControl control{ + .front_track_velocity = track_velocity_max_, + .back_climber_velocity = -5.0, + .override_chassis_vx = auto_climb_approach_chassis_velocity_, + }; + + double pitch = *chassis_pitch_imu_; + double target_pitch = auto_climb_stair_index_ == 0 ? first_stair_approach_pitch_ + : second_stair_approach_pitch_; + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "APPROACH (step %d): pitch=%.3f, target>%.3f", + auto_climb_stair_index_ + 1, pitch, target_pitch); + + if (pitch > target_pitch) { + enter_auto_climb_state(AutoClimbState::SUPPORT_DEPLOY); + RCLCPP_INFO( + logger_, "Auto climb entering SUPPORT_DEPLOY (step %d).", + auto_climb_stair_index_ + 1); + } + + return control; + } + + AutoClimbControl update_auto_climb_support_deploy() { + AutoClimbControl control{ + .front_track_velocity = 0.0, + .back_climber_velocity = climber_back_control_velocity_abs_, + .override_chassis_vx = auto_climb_support_deploy_chassis_velocity_, + }; + + if (is_back_climber_blocked()) + auto_climb_support_block_count_++; + else + auto_climb_support_block_count_ = 0; + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "SUPPORT_DEPLOY (step %d): blocked_ticks=%d", + auto_climb_stair_index_ + 1, auto_climb_support_block_count_); + + if (auto_climb_support_block_count_ >= kAutoClimbSupportConfirmTicks) { + enter_auto_climb_state(AutoClimbState::DASH); + RCLCPP_INFO( + logger_, "Auto climb entering DASH (step %d).", auto_climb_stair_index_ + 1); + } + + return control; + } + + AutoClimbControl update_auto_climb_dash() { + AutoClimbControl control{ + .front_track_velocity = 0, + .back_climber_velocity = climber_back_control_velocity_abs_, + .override_chassis_vx = auto_climb_dash_chassis_velocity_, + }; + + double pitch = *chassis_pitch_imu_; + double leveled_pitch_threshold = auto_climb_stair_index_ == 0 + ? first_stair_dash_leveled_pitch_threshold_ + : second_stair_dash_leveled_pitch_threshold_; + bool is_leveled = + pitch < leveled_pitch_threshold && auto_climb_timer_ > kAutoClimbDashMinTicks; + bool timeout = auto_climb_timer_ > kAutoClimbDashTimeoutTicks; + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "DASH (step %d): pitch=%.3f, threshold=%.3f, timer=%d", + auto_climb_stair_index_ + 1, pitch, leveled_pitch_threshold, auto_climb_timer_); + + if (is_leveled || timeout) { + enter_auto_climb_state(AutoClimbState::SUPPORT_RETRACT); + + if (timeout) { + RCLCPP_WARN( + logger_, "Auto climb DASH timeout on step %d. Entering SUPPORT_RETRACT.", + auto_climb_stair_index_ + 1); + } else { + RCLCPP_INFO( + logger_, "Auto climb reached platform on step %d.", + auto_climb_stair_index_ + 1); + } + } + + return control; + } + + AutoClimbControl update_auto_climb_support_retract() { + AutoClimbControl control{ + .front_track_velocity = track_velocity_max_, + .back_climber_velocity = back_climber_recover_count <= 1200 + ? -auto_climb_support_retract_velocity_fast_abs_ + : -auto_climb_support_retract_velocity_slow_abs_, + .override_chassis_vx = auto_climb_support_retract_chassis_velocity_, + }; + + if (is_back_climber_blocked()) + auto_climb_support_block_count_++; + else + auto_climb_support_block_count_ = 0; + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "SUPPORT_RETRACT (step %d): blocked_ticks=%d", + auto_climb_stair_index_ + 1, auto_climb_support_block_count_); + + if (auto_climb_support_block_count_ >= kAutoClimbSupportRetractConfirmTicks) { + bool has_next_stair = auto_climb_stair_index_ + 1 < kAutoClimbMaxStairs; + + if (has_next_stair) { + auto_climb_stair_index_++; + enter_auto_climb_state(AutoClimbState::APPROACH); + RCLCPP_INFO( + logger_, "Auto climb continuing to step %d.", auto_climb_stair_index_ + 1); + } else { + int finished_steps = auto_climb_stair_index_ + 1; + stop_auto_climb(); + back_climber_zero_velocity_hold_ = true; + control.front_track_velocity = nan_; + control.back_climber_velocity = 0.0; + control.override_chassis_vx = nan_; + RCLCPP_INFO(logger_, "Auto climb completed (finished %d steps).", finished_steps); + } + } + + return control; + } + + void apply_climb_control(const AutoClimbControl& control) { + *climbing_forward_velocity_ = control.override_chassis_vx; + *auto_climb_active_ = auto_climb_state_ != AutoClimbState::IDLE; + if (back_climber_recover_count != 0) { + back_climber_recover_count--; + } + + dual_motor_sync_control( + control.front_track_velocity, *climber_front_left_velocity_, + *climber_front_right_velocity_, front_velocity_pid_calculator_, + *climber_front_left_requested_control_torque_, + *climber_front_right_requested_control_torque_); + + dual_motor_sync_control( + control.back_climber_velocity, *climber_back_left_velocity_, + *climber_back_right_velocity_, back_velocity_pid_calculator_, + *climber_back_left_control_torque_, *climber_back_right_control_torque_); + + if (back_climber_recover_count > 1200) { + limit_back_climber_retract_torque( + control.back_climber_velocity, *climber_back_left_control_torque_, + *climber_back_right_control_torque_, back_climber_retract_first_torque_); + } else { + limit_back_climber_retract_torque( + control.back_climber_velocity, *climber_back_left_control_torque_, + *climber_back_right_control_torque_, back_climber_retract_second_torque_); + } + + *front_power_budget_active_ = is_front_power_budget_active(); + *front_power_demand_estimate_ = estimate_front_power( + *climber_front_left_requested_control_torque_, + *climber_front_right_requested_control_torque_, *climber_front_left_velocity_, + *climber_front_right_velocity_, front_power_estimate_bias_, + front_power_estimate_k_tau2_, front_power_estimate_k_mech_); + } + + void reset_all_controls() { + *climber_front_left_requested_control_torque_ = nan_; + *climber_front_right_requested_control_torque_ = nan_; + *climber_back_left_control_torque_ = nan_; + *climber_back_right_control_torque_ = nan_; + *climbing_forward_velocity_ = nan_; + *auto_climb_active_ = false; + *front_power_budget_active_ = false; + *front_power_demand_estimate_ = 0.0; + stop_manual_support(); + stop_auto_climb(); + back_climber_zero_velocity_hold_ = false; + } + + void stop_manual_support() { + back_climber_recover_count = 1500; + manual_support_retracting_ = false; + manual_support_retract_block_count_ = 0; + } + + void start_back_climber_retract(const char* source) { + if (!back_climber_recover_count) { + back_climber_recover_count = 1500; + } + manual_support_retracting_ = true; + manual_support_retract_block_count_ = 0; + back_climber_zero_velocity_hold_ = false; + RCLCPP_INFO(logger_, "%s back climber retract started.", source); + } + + void stop_auto_climb() { + auto_climb_state_ = AutoClimbState::IDLE; + auto_climb_timer_ = 0; + auto_climb_stair_index_ = 0; + auto_climb_align_stable_count_ = 0; + auto_climb_support_block_count_ = 0; + } + + void enter_auto_climb_state(AutoClimbState state) { + if (state == auto_climb_state_) + return; + auto_climb_state_ = state; + auto_climb_timer_ = 0; + auto_climb_align_stable_count_ = 0; + auto_climb_support_block_count_ = 0; + } + + bool is_back_climber_blocked() const { + return (std::abs(*climber_back_left_torque_) > kBackClimberBlockedTorqueThreshold + && std::abs(*climber_back_left_velocity_) < kBackClimberBlockedVelocityThreshold) + || (std::abs(*climber_back_right_torque_) > kBackClimberBlockedTorqueThreshold + && std::abs(*climber_back_right_velocity_) < kBackClimberBlockedVelocityThreshold); + } + + bool is_front_power_budget_active() const { + return auto_climb_state_ == AutoClimbState::APPROACH + || auto_climb_state_ == AutoClimbState::SUPPORT_RETRACT; + } + + void dual_motor_sync_control( + double setpoint, double left_velocity, double right_velocity, + pid::MatrixPidCalculator<2>& pid_calculator, double& left_torque_out, + double& right_torque_out) { + + if (std::isnan(setpoint)) { + left_torque_out = nan_; + right_torque_out = nan_; + return; + } + + Eigen::Vector2d setpoint_error{setpoint - left_velocity, setpoint - right_velocity}; + Eigen::Vector2d relative_velocity{ + left_velocity - right_velocity, right_velocity - left_velocity}; + + Eigen::Vector2d control_error = setpoint_error - sync_coefficient_ * relative_velocity; + auto control_torques = pid_calculator.update(control_error); + + left_torque_out = control_torques[0]; + right_torque_out = control_torques[1]; + } + + void limit_back_climber_retract_torque( + double back_climber_velocity_setpoint, double& left_torque, double& right_torque, + double max_torque) const { + + if (!std::isfinite(back_climber_velocity_setpoint) || back_climber_velocity_setpoint >= 0.0) + return; + if (!(max_torque > 0.0)) + return; + + const double peak = std::max(std::abs(left_torque), std::abs(right_torque)); + if (peak <= max_torque) + return; + + const double scale = max_torque / peak; + left_torque *= scale; + right_torque *= scale; + } + + rclcpp::Logger logger_; + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + static constexpr double kAutoClimbAlignThreshold = 0.10; + static constexpr double kAutoClimbAlignVelocityThreshold = 0.2; + static constexpr double kBackClimberBlockedTorqueThreshold = 0.1; + static constexpr double kBackClimberBlockedVelocityThreshold = 0.1; + static constexpr int kAutoClimbAlignConfirmTicks = 50; + static constexpr int kAutoClimbSupportConfirmTicks = 50; + static constexpr int kAutoClimbDashMinTicks = 100; + static constexpr int kAutoClimbDashTimeoutTicks = 3000; + static constexpr int kAutoClimbSupportRetractConfirmTicks = 50; + static constexpr int kAutoClimbMaxStairs = 2; + static constexpr int kManualSupportRetractConfirmTicks = 50; + + double sync_coefficient_; + double first_stair_approach_pitch_; + double second_stair_approach_pitch_; + + double track_velocity_max_; + double climber_back_control_velocity_abs_; + double auto_climb_support_retract_velocity_fast_abs_; + double auto_climb_support_retract_velocity_slow_abs_; + double auto_climb_approach_chassis_velocity_; + double auto_climb_support_deploy_chassis_velocity_; + double auto_climb_support_retract_chassis_velocity_; + double auto_climb_dash_chassis_velocity_; + double first_stair_dash_leveled_pitch_threshold_; + double second_stair_dash_leveled_pitch_threshold_; + double front_power_estimate_bias_; + double front_power_estimate_k_tau2_; + double front_power_estimate_k_mech_; + + AutoClimbState auto_climb_state_ = AutoClimbState::IDLE; + int auto_climb_timer_ = 0; + int auto_climb_stair_index_ = 0; + int auto_climb_align_stable_count_ = 0; + int auto_climb_support_block_count_ = 0; + bool manual_support_retracting_ = false; + int manual_support_retract_block_count_ = 0; + bool back_climber_zero_velocity_hold_ = false; + + OutputInterface climber_front_left_requested_control_torque_; + OutputInterface climber_front_right_requested_control_torque_; + OutputInterface climber_back_left_control_torque_; + OutputInterface climber_back_right_control_torque_; + OutputInterface climbing_forward_velocity_; + OutputInterface auto_climb_active_; + OutputInterface front_power_budget_active_; + OutputInterface front_power_demand_estimate_; + + InputInterface climber_front_left_velocity_; + InputInterface climber_front_right_velocity_; + InputInterface climber_back_left_velocity_; + InputInterface climber_back_right_velocity_; + + InputInterface climber_back_left_torque_; + InputInterface climber_back_right_torque_; + + InputInterface switch_right_; + InputInterface switch_left_; + InputInterface keyboard_; + InputInterface rotary_knob_switch_; + + InputInterface chassis_pitch_imu_; + InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_, gimbal_yaw_velocity_imu_; + + rmcs_msgs::Switch last_rotary_knob_switch_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + + pid::MatrixPidCalculator<2> front_velocity_pid_calculator_, back_velocity_pid_calculator_; + + std::shared_ptr front_power_limiter_; + + double back_climber_retract_first_torque_ = 8.0; + double back_climber_retract_second_torque_ = 0.5; + int back_climber_recover_count = 0; +}; +} // namespace rmcs_core::controller::chassis + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::ChassisClimberController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp index ddafc772..b76a7fbf 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp @@ -30,10 +30,15 @@ class ChassisController register_input("/remote/mouse/velocity", mouse_velocity_); register_input("/remote/mouse", mouse_); register_input("/remote/keyboard", keyboard_); + register_input("/remote/rotary_knob", rotary_knob_); + register_input("/gimbal/yaw/angle", gimbal_yaw_angle_, false); register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, false); + register_input("/chassis/velocity", chassis_velocity_, false); + register_input("/chassis/climbing_forward_velocity", climbing_forward_velocity_, false); + register_input("/rmcs_navigation/enable_control", navigation_enable_control_, false); register_input("/rmcs_navigation/chassis_velocity", navigation_command_velocity_, false); register_input("/rmcs_navigation/chassis_behavior", navigation_chassis_behavior_, false); @@ -55,6 +60,11 @@ class ChassisController get_logger(), "Failed to fetch \"/gimbal/yaw/control_angle_error\". Set to 0.0."); } + chassis_velocity_feedback_ready_ = chassis_velocity_.ready(); + if (!chassis_velocity_feedback_ready_) { + chassis_velocity_.make_and_bind_directly(0.0, 0.0, 0.0); + } + // Navigation Control Unavailable, Make It Manual Mode if (!navigation_enable_control_.ready()) { navigation_enable_control_.make_and_bind_directly(false); @@ -99,10 +109,17 @@ class ChassisController } } // Press Key: Z - // Switch ChassisMode To STEP_DOWN + // Switch ChassisMode To STEP_DOWN::BACK else if (!last_keyboard_.z && keyboard.z) { - mode = (mode == ChassisMode::STEP_DOWN) ? ChassisMode::AUTO - : ChassisMode::STEP_DOWN; + if (mode != rmcs_msgs::ChassisMode::STEP_DOWN + || (mode == rmcs_msgs::ChassisMode::STEP_DOWN + && step_down_facing_ == StepDownFacing::FRONT)) { + mode = rmcs_msgs::ChassisMode::STEP_DOWN; + step_down_facing_ = StepDownFacing::BACK; + } else { + mode = rmcs_msgs::ChassisMode::AUTO; + step_down_facing_ = StepDownFacing::BACK; + } } // Press Key: C // Switch ChassisMode To SPIN @@ -115,10 +132,17 @@ class ChassisController } } // Press Key: X - // Switch ChassisMode To LAUNCH_RAMP + // Switch ChassisMode To STEP_DOWN::FRONT else if (!last_keyboard_.x && keyboard.x) { - mode = (mode == ChassisMode::LAUNCH_RAMP) ? ChassisMode::AUTO - : ChassisMode::LAUNCH_RAMP; + if (mode != rmcs_msgs::ChassisMode::STEP_DOWN + || (mode == rmcs_msgs::ChassisMode::STEP_DOWN + && step_down_facing_ == StepDownFacing::BACK)) { + mode = rmcs_msgs::ChassisMode::STEP_DOWN; + step_down_facing_ = StepDownFacing::FRONT; + } else { + mode = rmcs_msgs::ChassisMode::AUTO; + step_down_facing_ = StepDownFacing::FRONT; + } } *mode_ = mode; @@ -135,6 +159,7 @@ class ChassisController void reset_all_controls() { *mode_ = ChassisMode::ALIGNMENT; *chassis_control_velocity_ = {kNaN, kNaN, kNaN}; + step_down_facing_ = StepDownFacing::FRONT; } void update_velocity_control() { @@ -148,6 +173,10 @@ class ChassisController } Eigen::Vector2d update_translational_velocity_control() { + if (!std::isnan(*climbing_forward_velocity_)) { + return Eigen::Vector2d{*climbing_forward_velocity_, 0.0}; + } + // Handle Navigation Control // // YawLink(LidarLink) -> ChassisLink @@ -180,10 +209,23 @@ class ChassisController auto angular_velocity = 0.0; auto chassis_control_angle = kNaN; - switch (*mode_) { - case ChassisMode::NONE: - case ChassisMode::AUTO: break; + if (!std::isnan(*climbing_forward_velocity_)) { + double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); + + constexpr double alignment = 2 * std::numbers::pi; + if (err > alignment / 2) + err -= alignment; + + angular_velocity = following_velocity_controller_.update(err); + + *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; + *chassis_control_angle_ = chassis_control_angle; + return angular_velocity; + } + switch (*mode_) { + case ChassisMode::NONE: + case rmcs_msgs::ChassisMode::AUTO: break; case ChassisMode::SPIN_FAST: { angular_velocity = 1.0 * (spinning_forward_ ? kAngularVelocityMax : -kAngularVelocityMax); @@ -194,20 +236,8 @@ class ChassisController } break; case ChassisMode::STEP_DOWN: { - double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); - - // err: [0, 2pi) -> [0, alignment) -> signed. - // In step-down mode, two sides of the chassis can be used for alignment. - // TODO: Dynamically determine the split angle based on chassis velocity. - constexpr double alignment = std::numbers::pi; - while (err > alignment / 2) { - chassis_control_angle -= alignment; - if (chassis_control_angle < 0) - chassis_control_angle += 2 * std::numbers::pi; - err -= alignment; - } - - angular_velocity = following_velocity_controller_.update(err); + angular_velocity = + update_following_angular_velocity(step_down_facing_, chassis_control_angle); } break; case ChassisMode::LAUNCH_RAMP: { double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); @@ -245,20 +275,14 @@ class ChassisController } double calculate_unsigned_chassis_angle_error(double& chassis_control_angle) { - chassis_control_angle = *gimbal_yaw_angle_error_; - if (chassis_control_angle < 0) - chassis_control_angle += 2 * std::numbers::pi; - // chassis_control_angle: [0, 2pi). + chassis_control_angle = normalize_positive_angle(*gimbal_yaw_angle_error_); // err = setpoint - measurement // ^ ^ // |gimbal_yaw_angle_error |chassis_angle // ^ // |(2pi - gimbal_yaw_angle) - double err = chassis_control_angle + *gimbal_yaw_angle_; - if (err >= 2 * std::numbers::pi) - err -= 2 * std::numbers::pi; - // err: [0, 2pi). + double err = normalize_positive_angle(chassis_control_angle + *gimbal_yaw_angle_); return err; } @@ -266,6 +290,37 @@ class ChassisController private: using ChassisMode = rmcs_msgs::ChassisMode; + enum class StepDownFacing { FRONT, BACK }; + + double update_following_angular_velocity( + StepDownFacing target_facing, double& chassis_control_angle) { + double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); + if (target_facing == StepDownFacing::BACK) { + chassis_control_angle = + normalize_positive_angle(chassis_control_angle + std::numbers::pi); + err = normalize_positive_angle(err + std::numbers::pi); + } + + err = normalize_signed_angle(err); + return following_velocity_controller_.update(err); + } + + static double normalize_positive_angle(double angle) { + constexpr double full_turn = 2 * std::numbers::pi; + while (angle >= full_turn) + angle -= full_turn; + while (angle < 0.0) + angle += full_turn; + return angle; + } + + static double normalize_signed_angle(double angle) { + angle = normalize_positive_angle(angle); + if (angle > std::numbers::pi) + angle -= 2 * std::numbers::pi; + return angle; + } + // Maximum control velocities static constexpr double kTranslationalVelocityMax = 20.0; static constexpr double kAngularVelocityMax = 1.2 * std::numbers::pi * 2.0; @@ -280,6 +335,7 @@ class ChassisController InputInterface mouse_velocity_; InputInterface mouse_; InputInterface keyboard_; + InputInterface rotary_knob_; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; @@ -298,6 +354,13 @@ class ChassisController }; OutputInterface chassis_control_velocity_; + InputInterface chassis_velocity_; + InputInterface climbing_forward_velocity_; + + bool chassis_velocity_feedback_ready_ = false; + + StepDownFacing step_down_facing_ = StepDownFacing::FRONT; + // For Navigation InputInterface navigation_enable_control_; InputInterface navigation_command_velocity_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp index b827bf00..7c701f63 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp @@ -41,6 +41,8 @@ class ChassisPowerController "/chassis/supercap/voltage/control_line", supercap_voltage_control_line_, 12.5); register_output("/chassis/supercap/voltage/base_line", supercap_voltage_base_line_, 12.0); register_output("/chassis/supercap/voltage/dead_line", supercap_voltage_dead_line_, 11.0); + + register_output("/chassis/climber/front/control_power_limit", control_power_limit_, 0.0); } void update() override { @@ -94,6 +96,7 @@ class ChassisPowerController virtual_buffer_energy_ = virtual_buffer_energy_limit_; boost_mode_ = false; *chassis_control_power_limit_ = 0.0; + *control_power_limit_ = 0.0; } void update_virtual_buffer_energy() { @@ -131,6 +134,7 @@ class ChassisPowerController power_limit *= virtual_buffer_energy_ / virtual_buffer_energy_limit_; *chassis_control_power_limit_ = power_limit; + *control_power_limit_ = power_limit; } void update_ui() { @@ -167,6 +171,7 @@ class ChassisPowerController OutputInterface supercap_voltage_control_line_; OutputInterface supercap_voltage_base_line_; OutputInterface supercap_voltage_dead_line_; + OutputInterface control_power_limit_; ui::Integer chassis_power_ui_{ui::Shape::Color::WHITE, 15, 2, ui::x_center, 100, 0}; ui::Integer chassis_control_power_limit_ui_{ diff --git a/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp index aae65ece..34e366bd 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp @@ -292,12 +292,19 @@ class Sentry {sentry, sentry_command, "/chassis/left_back_steering"}, {sentry, sentry_command, "/chassis/right_back_steering"}, {sentry, sentry_command, "/chassis/right_front_steering"}) + , chassis_front_climber_motor_( + {sentry, sentry_command, "/chassis/climber/left_front_motor"}, + {sentry, sentry_command, "/chassis/climber/right_front_motor"}) + , chassis_back_climber_motor_( + {sentry, sentry_command, "/chassis/climber/left_back_motor"}, + {sentry, sentry_command, "/chassis/climber/right_back_motor"}) , supercap_(sentry, sentry_command) { using namespace device; sentry.register_output("/referee/serial", referee_serial_); sentry.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0.0); + sentry.register_output("/chassis/pitch_imu", chassis_pitch_imu_, 0.0); referee_serial_->read = [this](std::byte* buffer, size_t size) { return referee_ring_buffer_receive_.pop_front_n( @@ -335,6 +342,21 @@ class Sentry .enable_multi_turn_angle()); } + chassis_front_climber_motor_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} + .set_reduction_ratio(19.)); + chassis_front_climber_motor_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 2} + .set_reversed() + .set_reduction_ratio(19.)); + chassis_back_climber_motor_[0].configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} + .set_reversed() + .enable_multi_turn_angle()); + chassis_back_climber_motor_[1].configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} + .enable_multi_turn_angle()); + board_ = std::make_unique(*this, board_serial); } @@ -350,10 +372,17 @@ class Sentry for (auto& motor : chassis_steer_motors_) motor.update_status(); + chassis_front_climber_motor_[0].update_status(); + chassis_front_climber_motor_[1].update_status(); + chassis_back_climber_motor_[0].update_status(); + chassis_back_climber_motor_[1].update_status(); + tf_->set_state( gimbal_bottom_yaw_motor_.angle()); if (const auto snapshot = bmi088_.snapshot()) { + const auto& q = snapshot->orientation; + *chassis_pitch_imu_ = -std::asin(2.0 * (q.w() * q.y() - q.z() * q.x())); *chassis_yaw_velocity_imu_ = snapshot->gyro_body.z(); } } @@ -388,7 +417,17 @@ class Sentry board_->start_transmit() .can_transmit( - Spec::kCans.kCan0, {.can_id = 0x141, .can_data = bottom_yaw_package.as_bytes()}) + Spec::kCans.kCan0, + { + .can_id = 0x142, + .can_data = chassis_back_climber_motor_[0].generate_command().as_bytes(), + }) + .can_transmit( + Spec::kCans.kCan0, + { + .can_id = 0x143, + .can_data = chassis_back_climber_motor_[1].generate_command().as_bytes(), + }) .can_transmit( Spec::kCans.kCan1, generate(std::views::counted(chassis_wheel_motors_, 2))) @@ -401,7 +440,21 @@ class Sentry Spec::kCans.kCan2, generate(std::views::counted(chassis_steer_motors_ + 2, 2))) .can_transmit( - Spec::kCans.kCan3, {.can_id = 0x1fe, .can_data = supercap_package.as_bytes()}); + Spec::kCans.kCan3, {.can_id = 0x1fe, .can_data = supercap_package.as_bytes()}) + .can_transmit( + Spec::kCans.kCan3, + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_front_climber_motor_[0].generate_command(), + chassis_front_climber_motor_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + }.as_bytes(), + }) + .can_transmit( + Spec::kCans.kCan3, {.can_id = 0x141, .can_data = bottom_yaw_package.as_bytes()}); } void can_receive_callback(const Spec::Can& can, const View::Can& data) override { @@ -412,8 +465,11 @@ class Sentry const auto& can_data = data.can_data; if (can == Spec::kCans.kCan0) { - if (can_id == 0x141) - gimbal_bottom_yaw_motor_.store_status(data.can_data); + if(can_id == 0x142) { + chassis_back_climber_motor_[0].store_status(data.can_data); + } else if(can_id == 0x143) { + chassis_back_climber_motor_[1].store_status(data.can_data); + } monitor_.tick("Chassis::Can0", can_id); @@ -436,8 +492,14 @@ class Sentry monitor_.tick("Chassis::Can2", can_id); } else if (can == Spec::kCans.kCan3) { - if (can_id == 0x300) - supercap_.store_status(data.can_data); + if (can_id == 0x300) { + supercap_.store_status(can_data); + } else if(can_id == 0x141) { + gimbal_bottom_yaw_motor_.store_status(data.can_data); + } else { + /*^^*/ chassis_front_climber_motor_[0].match_then_store_status(can_id, can_data) + || chassis_front_climber_motor_[1].match_then_store_status(can_id, can_data); + } monitor_.tick("Chassis::Can3", can_id); } @@ -482,11 +544,16 @@ class Sentry device::LkMotor gimbal_bottom_yaw_motor_; device::DjiMotor chassis_wheel_motors_[4]; device::DjiMotor chassis_steer_motors_[4]; + + device::DjiMotor chassis_front_climber_motor_[2]; + device::LkMotor chassis_back_climber_motor_[2]; + device::Supercap supercap_; rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; OutputInterface referee_serial_; OutputInterface chassis_yaw_velocity_imu_; + OutputInterface chassis_pitch_imu_; StatusMonitor monitor_{}; std::unique_ptr board_;