From 7e16876f0187addc2762d4352da73668d0fdaa24 Mon Sep 17 00:00:00 2001 From: Fin_Resect Date: Sun, 15 Mar 2026 21:57:40 +0800 Subject: [PATCH 1/7] Pick steering hero resources from branch steering-hero Co-authored-by: zhzy-star <2807406212@qq.com> Co-authored-by: floatpigeon Co-authored-by: fan --- .../rmcs_bringup/config/steering-hero.yaml | 257 +++++++ rmcs_ws/src/rmcs_core/plugins.xml | 155 +++- .../chassis/chassis_climber_controller.cpp | 185 +++++ .../controller/gimbal/dual_yaw_controller.cpp | 3 - .../bullet_feeder_controller_42mm.cpp | 61 +- .../controller/shooting/putter_controller.cpp | 387 ++++++++++ .../controller/shooting/shooting_recorder.cpp | 215 +++++- .../rmcs_core/src/hardware/steering-hero.cpp | 711 ++++++++++-------- 8 files changed, 1584 insertions(+), 390 deletions(-) create mode 100644 rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml new file mode 100644 index 00000000..7e7c551e --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml @@ -0,0 +1,257 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::SteeringHero -> hero_hardware + + - rmcs_core::referee::Status -> referee_status + - rmcs_core::referee::command::Interaction -> referee_interaction + # - rmcs_core::referee::command::interaction::Ui -> referee_ui + # - rmcs_core::referee::app::ui::Hero -> referee_ui_hero + - rmcs_core::referee::Command -> referee_command + + - rmcs_core::controller::gimbal::HeroGimbalController -> gimbal_controller + - rmcs_core::controller::gimbal::DualYawController -> dual_yaw_controller + - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller + + # - rmcs_core::controller::gimbal::PlayerViewer -> gimbal_player_viewer_controller + # - rmcs_core::controller::pid::ErrorPidController -> viewer_angle_pid_controller + + - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller + - rmcs_core::controller::shooting::HeatController -> heat_controller + - rmcs_core::controller::shooting::PutterController -> bullet_feeder_controller + - rmcs_core::controller::pid::PidController -> first_left_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> first_right_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> second_left_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> second_right_friction_velocity_pid_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_core::controller::chassis::ChassisClimberController -> climber_controller + + # - rmcs_auto_aim::AutoAimInitializer -> auto_aim_initializer + # - rmcs_auto_aim::AutoAimController -> auto_aim_controller + + - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + +hero_hardware: + ros__parameters: + usb_pid_top_board: 0xce3b + usb_pid_bottom_board_one: 0x902e + usb_pid_bottom_board_two: 0x317e + bottom_yaw_motor_zero_point: 50656 + pitch_motor_zero_point: 63917 + top_yaw_motor_zero_point: 59095 + viewer_motor_zero_point: 3030 + external_imu_port: /dev/ttyUSB0 + left_front_zero_point: 4438 + right_front_zero_point: 7167 + left_back_zero_point: 370 + right_back_zero_point: 5156 + q0: 0.0 + q1: 0.0 + q2: 0.0 + q3: 1.0 + # left_front_zero_point: 3473 + # left_back_zero_point: 6124 + # right_back_zero_point: 6157 + # right_front_zero_point: 2723 + +value_broadcaster: + ros__parameters: + forward_list: + - /chassis/left_front_steering/angle + - /chassis/left_back_steering/angle + - /chassis/right_back_steering/angle + - /chassis/right_front_steering/angle + - /chassis/left_front_steering/control_torque + - /chassis/left_back_steering/control_torque + - /chassis/right_back_steering/control_torque + - /chassis/right_front_steering/control_torque + - /gimbal/bottom_yaw/angle + +climber_controller: + ros__parameters: + front_climber_velocity: 50.0 + back_climber_velocity: 10.0 + front_kp: 2.0 + front_ki: 0.0 + front_kd: 0.5 + back_kp: 2.0 + back_ki: 0.0 + back_kd: 0.0 + +gimbal_controller: + ros__parameters: + upper_limit: -0.6266 + lower_limit: 0.4629 + +dual_yaw_controller: + ros__parameters: + top_yaw_angle_kp: 10.0 + top_yaw_angle_ki: 0.0 + top_yaw_angle_kd: 0.0 + top_yaw_velocity_kp: 50.0 + top_yaw_velocity_ki: 0.004 + top_yaw_velocity_kd: 0.0 + bottom_yaw_angle_kp: 5.0 + bottom_yaw_angle_ki: 0.0 + bottom_yaw_angle_kd: 0.0 + bottom_yaw_velocity_kp: 10.0 + bottom_yaw_velocity_ki: 0.0 + bottom_yaw_velocity_kd: 50.0 + +# dual_yaw_controller: +# ros__parameters: +# top_yaw_angle_kp: 24.5 +# top_yaw_angle_ki: 0.0 +# top_yaw_angle_kd: 0.0 +# top_yaw_velocity_kp: 77.4 +# top_yaw_velocity_ki: 0.004 +# top_yaw_velocity_kd: 1.0 +# bottom_yaw_angle_kp: 8.6 +# bottom_yaw_angle_ki: 0.0 +# bottom_yaw_angle_kd: 0.0 +# bottom_yaw_velocity_kp: 25.85 +# bottom_yaw_velocity_ki: 0.0 +# bottom_yaw_velocity_kd: 50.0 + +pitch_angle_pid_controller: + ros__parameters: + measurement: /gimbal/pitch/control_angle_error + control: /gimbal/pitch/control_velocity + kp: 28.00 + ki: 0.0 + kd: 0.6 + +pitch_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/pitch/velocity_imu + setpoint: /gimbal/pitch/control_velocity + control: /gimbal/pitch/control_torque + kp: 45.00 + ki: 0.00 + kd: 1.00 + +gimbal_player_viewer_controller: + ros__parameters: + upper_limit: 0.0101 + lower_limit: 0.6191 + +viewer_angle_pid_controller: + ros__parameters: + measurement: /gimbal/player_viewer/control_angle_error + control: /gimbal/player_viewer/control_velocity + kp: 17.00 + ki: 0.00 + kd: 2.00 + +friction_wheel_controller: + ros__parameters: + friction_wheels: + - /gimbal/first_left_friction + - /gimbal/first_right_friction + - /gimbal/second_left_friction + - /gimbal/second_right_friction + friction_velocities: + - 450.00 + - 450.00 + - -535.00 + - 535.00 + friction_soft_start_stop_time: 1.0 + +heat_controller: + ros__parameters: + heat_per_shot: 100000 + reserved_heat: 0 + +shooting_recorder: + ros__parameters: + friction_wheel_count: 2 + log_mode: 2 # 1: trigger, 2: timing + +first_left_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/first_left_friction/velocity + setpoint: /gimbal/first_left_friction/control_velocity + control: /gimbal/first_left_friction/control_torque + kp: 0.003436926 + ki: 0.00 + kd: 0.009373434 + +first_right_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/first_right_friction/velocity + setpoint: /gimbal/first_right_friction/control_velocity + control: /gimbal/first_right_friction/control_torque + kp: 0.003436926 + ki: 0.00 + kd: 0.009373434 + +second_left_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/second_left_friction/velocity + setpoint: /gimbal/second_left_friction/control_velocity + control: /gimbal/second_left_friction/control_torque + kp: 0.003436926 + ki: 0.00 + kd: 0.009373434 + +second_right_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/second_right_friction/velocity + setpoint: /gimbal/second_right_friction/control_velocity + control: /gimbal/second_right_friction/control_torque + kp: 0.003436926 + ki: 0.00 + kd: 0.009373434 + +steering_wheel_controller: + ros__parameters: + mess: 22.0 + moment_of_inertia: 1.08 + vehicle_radius: 0.28284271247462 + wheel_radius: 0.055 + friction_coefficient: 0.6 + k1: 2.958580e+00 + k2: 3.082190e-03 + no_load_power: 11.37 + +auto_aim_controller: + ros__parameters: + # capture + use_video: false # If true, use video stream instead of camera. + video_path: "/workspaces/RMCS/rmcs_ws/resources/1.avi" + exposure_time: 1 + invert_image: false + # identifier + armor_model_path: "/models/mlp.onnx" + # pnp + fx: 1.722231837421459e+03 + fy: 1.724876404292754e+03 + cx: 7.013056440882832e+02 + cy: 5.645821718351237e+02 + k1: -0.064232403853946 + k2: -0.087667493884102 + k3: 0.792381808294582 + # tracker + armor_predict_duration: 500 + # controller + gimbal_predict_duration: 100 + yaw_error: 0. + pitch_error: 0. + shoot_velocity: 28.0 + predict_sec: 0.095 + # etc + buff_predict_duration: 200 + buff_model_path: "/models/buff_nocolor_v6.onnx" + omni_exposure: 1000.0 + record_fps: 120 + debug: false # Setup in actual using.Debug mode is used when referee is not ready + debug_color: 0 # 0 For blue while 1 for red. mine + debug_robot_id: 4 + debug_buff_mode: false + record: false + raw_img_pub: false # Set false in actual use + image_viewer_type: 2 diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index f7847151..9df7668b 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -1,29 +1,128 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Steering wheel controller. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Gimbal player viewer + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + the recorder of Hero + + + Test plugin. + + + Test plugin. + + + Feedforward pid controller. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + \ No newline at end of file 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..3a524187 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp @@ -0,0 +1,185 @@ +#include "controller/pid/matrix_pid_calculator.hpp" +#include "rmcs_msgs/switch.hpp" +#include +#include +#include +#include +#include +#include +#include + +/* + 所有的电机运动方向均以令底盘升高为正方向 + 暂时使用手动控制，遥控器键位不够用了，屏蔽自瞄和小陀螺模式 +*/ + +namespace rmcs_core::controller::chassis { +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(); + + register_output( + "/chassis/climber/left_front_motor/control_torque", climber_front_left_control_torque_, + nan_); + register_output( + "/chassis/climber/right_front_motor/control_torque", + climber_front_right_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_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/joystick/right", joystick_right_); + } + + void update() override { + using namespace rmcs_msgs; + + auto switch_right = *switch_right_; + auto switch_left = *switch_left_; + + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + reset_all_controls(); + } else if (switch_left != Switch::DOWN) { + + if (last_switch_right_ == Switch::MIDDLE && switch_right == Switch::UP) { + front_climber_enable_ = !front_climber_enable_; + + } else if (last_switch_right_ == Switch::MIDDLE && switch_right == Switch::DOWN) { + back_climber_dir_ = -1 * back_climber_dir_; + back_climber_block_count_ = 0; + } + + double track_control_velocity = + front_climber_enable_ ? joystick_right_->x() * track_velocity_max_ : nan_; + double back_climber_control_velocity; + + if ((std::abs(*climber_back_left_torque_) > 0.1 + && std::abs(*climber_back_right_velocity_) < 0.1) + || (std::abs(*climber_back_left_velocity_) < 0.1 + && std::abs(*climber_back_right_torque_) > 0.1)) { + back_climber_block_count_++; + } + + dual_motor_sync_control( + track_control_velocity, *climber_front_left_velocity_, + *climber_front_right_velocity_, front_velocity_pid_calculator_, + *climber_front_left_control_torque_, *climber_front_right_control_torque_); + + if (back_climber_block_count_ >= 500) { + back_climber_control_velocity = 0; + // *climber_back_left_control_torque_ = 0; + // *climber_back_right_control_torque_ = 0; + // last_switch_left_ = switch_left; + // last_switch_right_ = switch_right; + // return; + } else { + back_climber_control_velocity = + climber_back_control_velocity_abs_ * back_climber_dir_; + } + + // dual_motor_sync_control( + // back_climber_control_velocity, *climber_back_left_velocity_, + // *climber_back_right_velocity_, back_velocity_pid_calculator_, + // *climber_back_left_control_torque_, *climber_back_right_control_torque_); + } + last_switch_left_ = switch_left; + last_switch_right_ = switch_right; + } + +private: + void reset_all_controls() { + *climber_front_left_control_torque_ = 0; + *climber_front_right_control_torque_ = 0; + *climber_back_left_control_torque_ = 0; + *climber_back_right_control_torque_ = 0; + front_climber_enable_ = false; + } + + 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) const { + + 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]; + } + + int back_climber_block_count_; + + rclcpp::Logger logger_; + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + double sync_coefficient_; + + bool front_climber_enable_ = false; + double back_climber_dir_ = -1; + + double track_velocity_max_; + double climber_back_control_velocity_abs_; + + OutputInterface climber_front_left_control_torque_; + OutputInterface climber_front_right_control_torque_; + OutputInterface climber_back_left_control_torque_; + OutputInterface climber_back_right_control_torque_; + + 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 joystick_right_; + + rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + + pid::MatrixPidCalculator<2> front_velocity_pid_calculator_, back_velocity_pid_calculator_; +}; +} // namespace rmcs_core::controller::chassis + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::ChassisClimberController, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp index 80d7a72e..20c09236 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp @@ -45,9 +45,6 @@ class DualYawController register_output("/gimbal/top_yaw/control_torque", top_yaw_control_torque_, 0.0); register_output("/gimbal/bottom_yaw/control_torque", bottom_yaw_control_torque_, 0.0); - register_output("/gimbal/top_yaw/control_angle", top_yaw_control_torque_, 0.0); - register_output("/gimbal/bottom_yaw/control_angle_shift", bottom_yaw_control_torque_, 0.0); - status_component_ = create_partner_component(get_component_name() + "_status"); } diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_42mm.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_42mm.cpp index bf3076e3..95d75cb9 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_42mm.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_42mm.cpp @@ -31,14 +31,15 @@ class BulletFeederController42mm register_input( "/gimbal/control_bullet_allowance/limited_by_heat", control_bullet_allowance_limited_by_heat_); + register_input("/gimbal/bullet_fired", bullet_fired_); - bullet_feeder_velocity_pid_.kp = 50.0; - bullet_feeder_velocity_pid_.ki = 10.0; - bullet_feeder_velocity_pid_.kd = 0.0; + bullet_feeder_velocity_pid_.kp = 50.0; + bullet_feeder_velocity_pid_.ki = 10.0; + bullet_feeder_velocity_pid_.kd = 0.0; bullet_feeder_velocity_pid_.integral_max = 60.0; bullet_feeder_velocity_pid_.integral_min = 0.0; - bullet_feeder_angle_pid_.kp = 50.0; + bullet_feeder_angle_pid_.kp = 60.0; bullet_feeder_angle_pid_.ki = 0.0; bullet_feeder_angle_pid_.kd = 2.0; @@ -51,9 +52,9 @@ class BulletFeederController42mm void update() override { const auto switch_right = *switch_right_; - const auto switch_left = *switch_left_; - const auto mouse = *mouse_; - const auto keyboard = *keyboard_; + const auto switch_left = *switch_left_; + const auto mouse = *mouse_; + const auto keyboard = *keyboard_; using namespace rmcs_msgs; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) @@ -61,7 +62,6 @@ class BulletFeederController42mm reset_all_controls(); return; } - overdrive_mode_ = keyboard.f; if (keyboard.ctrl && !last_keyboard_.r && keyboard.r) low_latency_mode_ = !low_latency_mode_; @@ -84,8 +84,8 @@ class BulletFeederController42mm } else { if (!*friction_ready_ || std::isnan(bullet_feeder_control_angle_)) { bullet_feeder_control_angle_ = *bullet_feeder_angle_; - shoot_stage_ = ShootStage::PRELOADED; - bullet_fed_count_ = static_cast( + shoot_stage_ = ShootStage::PRELOADED; + bullet_fed_count_ = static_cast( (*bullet_feeder_angle_ - bullet_feeder_compressed_zero_point_ - 0.1) / bullet_feeder_angle_per_bullet_); } @@ -135,24 +135,24 @@ class BulletFeederController42mm } last_switch_right_ = switch_right; - last_switch_left_ = switch_left; - last_mouse_ = mouse; - last_keyboard_ = keyboard; + last_switch_left_ = switch_left; + last_mouse_ = mouse; + last_keyboard_ = keyboard; } private: void reset_all_controls() { last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; - last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; - last_mouse_ = rmcs_msgs::Mouse::zero(); - last_keyboard_ = rmcs_msgs::Keyboard::zero(); + last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + last_mouse_ = rmcs_msgs::Mouse::zero(); + last_keyboard_ = rmcs_msgs::Keyboard::zero(); overdrive_mode_ = low_latency_mode_ = false; - shoot_stage_ = ShootStage::PRELOADED; + shoot_stage_ = ShootStage::PRELOADED; bullet_fed_count_ = std::numeric_limits::min(); - bullet_feeder_control_angle_ = nan_; + bullet_feeder_control_angle_ = nan_; bullet_feeder_angle_pid_.output_max = inf_; bullet_feeder_velocity_pid_.reset(); @@ -160,13 +160,13 @@ class BulletFeederController42mm *bullet_feeder_control_torque_ = nan_; bullet_feeder_faulty_count_ = 0; - bullet_feeder_cool_down_ = 0; + bullet_feeder_cool_down_ = 0; } void set_preloading() { RCLCPP_INFO(get_logger(), "PRELOADING"); bullet_fed_count_++; - shoot_stage_ = ShootStage::PRELOADING; + shoot_stage_ = ShootStage::PRELOADING; bullet_feeder_control_angle_ = bullet_feeder_compressed_zero_point_ + (bullet_fed_count_ + 0.5) * bullet_feeder_angle_per_bullet_; bullet_feeder_angle_pid_.output_max = 1.0; @@ -179,7 +179,7 @@ class BulletFeederController42mm void set_compressing() { RCLCPP_INFO(get_logger(), "COMPRESSING"); - shoot_stage_ = ShootStage::COMPRESSING; + shoot_stage_ = ShootStage::COMPRESSING; bullet_feeder_control_angle_ = bullet_feeder_compressed_zero_point_ + (bullet_fed_count_ + 1) * bullet_feeder_angle_per_bullet_; bullet_feeder_angle_pid_.output_max = 0.8; @@ -192,7 +192,7 @@ class BulletFeederController42mm void set_shooting() { RCLCPP_INFO(get_logger(), "SHOOTING"); - shoot_stage_ = ShootStage::SHOOTING; + shoot_stage_ = ShootStage::SHOOTING; bullet_feeder_control_angle_ = bullet_feeder_compressed_zero_point_ + (bullet_fed_count_ + 1.2) * bullet_feeder_angle_per_bullet_; bullet_feeder_angle_pid_.output_max = 1.0; @@ -215,7 +215,7 @@ class BulletFeederController42mm void enter_jam_protection() { bullet_feeder_control_angle_ = nan_; - bullet_feeder_cool_down_ = 1000; + bullet_feeder_cool_down_ = 1000; bullet_feeder_angle_pid_.reset(); bullet_feeder_velocity_pid_.reset(); RCLCPP_INFO(get_logger(), "Jammed!"); @@ -225,9 +225,10 @@ class BulletFeederController42mm static constexpr double inf_ = std::numeric_limits::infinity(); static constexpr double bullet_feeder_compressed_zero_point_ = 0.58; - static constexpr double bullet_feeder_angle_per_bullet_ = 2 * std::numbers::pi / 6; + static constexpr double bullet_feeder_angle_per_bullet_ = 2 * std::numbers::pi / 6; InputInterface friction_ready_; + InputInterface bullet_fired_; InputInterface switch_right_; InputInterface switch_left_; @@ -235,9 +236,9 @@ class BulletFeederController42mm InputInterface keyboard_; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; - rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; - rmcs_msgs::Mouse last_mouse_ = rmcs_msgs::Mouse::zero(); - rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Mouse last_mouse_ = rmcs_msgs::Mouse::zero(); + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); bool overdrive_mode_ = false, low_latency_mode_ = false; @@ -247,8 +248,8 @@ class BulletFeederController42mm InputInterface control_bullet_allowance_limited_by_heat_; enum class ShootStage { PRELOADING, PRELOADED, COMPRESSING, COMPRESSED, SHOOTING }; - ShootStage shoot_stage_ = ShootStage::PRELOADED; - int bullet_fed_count_ = std::numeric_limits::min(); + ShootStage shoot_stage_ = ShootStage::PRELOADED; + int bullet_fed_count_ = std::numeric_limits::min(); double bullet_feeder_control_angle_ = nan_; pid::PidCalculator bullet_feeder_velocity_pid_; @@ -256,7 +257,7 @@ class BulletFeederController42mm OutputInterface bullet_feeder_control_torque_; int bullet_feeder_faulty_count_ = 0; - int bullet_feeder_cool_down_ = 0; + int bullet_feeder_cool_down_ = 0; OutputInterface shoot_mode_; }; diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp new file mode 100644 index 00000000..4efdbf52 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp @@ -0,0 +1,387 @@ +#include +#include +#include +#include +#include +#include + +#include "controller/pid/pid_calculator.hpp" + +namespace rmcs_core::controller::shooting { + +/** + * @class PutterController + * @brief 推弹机构控制器 + * + * 发射机制说明： + * 由于光电门放置于弹舱口，经测试，双中先触发推杆向后复位，然后堵转检测复位完毕， + * 默认情况下会给一点点的力保证推杆不会滑下去，再然后上弹采用速度环，在光电门被触发时 + * 记录角度并转为角度环，然后推杆检测发弹根据两部分来检测，一是摩擦轮，二是推杆的行程 + * 整套方案以于暑假前完成压力测试 + */ +class PutterController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + PutterController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { + + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + register_input("/remote/mouse", mouse_); + register_input("/remote/keyboard", keyboard_); + + register_input("/gimbal/friction_ready", friction_ready_); + + register_input("/gimbal/bullet_feeder/angle", bullet_feeder_angle_); + register_input("/gimbal/bullet_feeder/velocity", bullet_feeder_velocity_); + + register_input( + "/gimbal/control_bullet_allowance/limited_by_heat", + control_bullet_allowance_limited_by_heat_); + + register_input("/gimbal/photoelectric_sensor", photoelectric_sensor_status_); + register_input("/gimbal/bullet_fired", bullet_fired_); + + register_input("/gimbal/putter/angle", putter_angle_); + register_input("/gimbal/putter/velocity", putter_velocity_); + + last_preload_flag_ = false; + + bullet_feeder_velocity_pid_.kp = 50.0; + bullet_feeder_velocity_pid_.ki = 10.0; + bullet_feeder_velocity_pid_.kd = 0.0; + bullet_feeder_velocity_pid_.integral_max = 60.0; + bullet_feeder_velocity_pid_.integral_min = 0.0; + + bullet_feeder_angle_pid_.kp = 8.0; + bullet_feeder_angle_pid_.ki = 0.0; + bullet_feeder_angle_pid_.kd = 2.0; + + puttter_return_velocity_pid_.kp = 0.0015; + puttter_return_velocity_pid_.ki = 0.00005; + puttter_return_velocity_pid_.kd = 0.; + puttter_return_velocity_pid_.integral_max = 0.; + puttter_return_velocity_pid_.integral_min = -0.03; + + putter_velocity_pid_.kp = 0.004; + putter_velocity_pid_.ki = 0.0001; + putter_velocity_pid_.kd = 0.001; + puttter_return_velocity_pid_.integral_max = 0.03; + puttter_return_velocity_pid_.integral_min = 0.; + + putter_return_angle_pid.kp = 0.0001; + // putter_return_angle_pid.ki = 0.000001; + putter_return_angle_pid.kd = 0.; + + register_output( + "/gimbal/bullet_feeder/control_torque", bullet_feeder_control_torque_, nan_); + register_output("/gimbal/putter/control_torque", putter_control_torque_, nan_); + + register_output("/gimbal/shooter/mode", shoot_mode_, rmcs_msgs::ShootMode::SINGLE); + } + + void update() override { + const auto switch_right = *switch_right_; + const auto switch_left = *switch_left_; + const auto mouse = *mouse_; + const auto keyboard = *keyboard_; + + using namespace rmcs_msgs; + + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + reset_all_controls(); + return; + } + + // 推杆已初始化后的正常控制流程 + if (putter_initialized) { + // 供弹轮卡弹保护冷却期间的处理 + if (bullet_feeder_cool_down_ > 0) { + bullet_feeder_cool_down_--; + + // 冷却期前期：反转供弹轮以解除卡弹 + if (bullet_feeder_cool_down_ > 500) + *bullet_feeder_control_torque_ = bullet_feeder_velocity_pid_.update( + -bullet_feeder_angle_per_bullet_ - *bullet_feeder_velocity_); + else { + // 冷却期后期：停止控制 + bullet_feeder_velocity_pid_.reset(); + *bullet_feeder_control_torque_ = 0.0; + } + + bullet_feeder_angle_pid_.reset(); + + if (!bullet_feeder_cool_down_) + RCLCPP_INFO(get_logger(), "Jamming Solved!"); + } else { + // 正常运行模式：摩擦轮就绪时才允许发射 + if (*friction_ready_) { + // 发射触发检测 + // RCLCPP_INFO(get_logger(), "%.2f", *bullet_feeder_angle_); + // RCLCPP_INFO(get_logger(), "%.2f", *bullet_feeder_velocity_); + if (switch_right != Switch::DOWN) { + if ((!last_mouse_.left && mouse.left) + || (last_switch_left_ == rmcs_msgs::Switch::MIDDLE + && switch_left == rmcs_msgs::Switch::DOWN)) { + if ( + // *control_bullet_allowance_limited_by_heat_ > 0 && + shoot_stage_ == ShootStage::PRELOADED) + set_shooting(); + } + } + + if (shoot_stage_ == ShootStage::PRELOADING) { + if (last_preload_flag_) { + // 角度环控制模式：光电门触发后精确定位 + const auto angle_err = + bullet_feeder_control_angle_ - *bullet_feeder_angle_; + if (angle_err < 0.1) { + set_preloaded(); + } + double velocity_err = + bullet_feeder_angle_pid_.update( + bullet_feeder_control_angle_ - *bullet_feeder_angle_) + - *bullet_feeder_velocity_; + *bullet_feeder_control_torque_ = + bullet_feeder_velocity_pid_.update(velocity_err); + } else { + // 速度环控制模式：等待光电门触发 + if (*photoelectric_sensor_status_) { + RCLCPP_INFO(get_logger(), "Photoelectric Sensor Triggered"); + last_preload_flag_ = true; + bullet_feeder_control_angle_ = + *bullet_feeder_angle_ + bullet_feeder_angle_per_bullet_ * 1.; + } else + *bullet_feeder_control_torque_ = bullet_feeder_velocity_pid_.update( + low_latency_velocity_ - *bullet_feeder_velocity_); + } + update_jam_detection(); + } else { + // 其他状态：角度环保持角度不变防止弹链退弹 + double velocity_err = + bullet_feeder_angle_pid_.update( + bullet_feeder_control_angle_ - *bullet_feeder_angle_) + - *bullet_feeder_velocity_; + *bullet_feeder_control_torque_ = + bullet_feeder_velocity_pid_.update(velocity_err); + } + + if (shoot_stage_ == ShootStage::SHOOTING) { + // 发射状态：检测子弹是否发出 + if (*bullet_fired_ + || *putter_angle_ - putter_startpoint >= putter_stroke_) { + shooted = true; + } + + update_putter_jam_detection(); + + if (shooted) { + // 子弹已发出：推杆复位 + const auto angle_err = putter_startpoint - *putter_angle_; + if (angle_err > -0.05) { + *putter_control_torque_ = 0.; + set_preloading(); + shooted = false; + } + + *putter_control_torque_ = + puttter_return_velocity_pid_.update(-80. - *putter_velocity_); + } else { + // 子弹未发出：继续推进 + *putter_control_torque_ = + puttter_return_velocity_pid_.update(60. - *putter_velocity_); + } + } + } else { + // 摩擦轮未就绪：停止供弹轮 + *bullet_feeder_control_torque_ = 0.; + } + + // 非发射状态：推杆给少许力保持位置 + if (shoot_stage_ != ShootStage::SHOOTING) + *putter_control_torque_ = -0.02; + } + } else { + // 推杆未初始化：执行复位操作 + *putter_control_torque_ = puttter_return_velocity_pid_.update(-80. - *putter_velocity_); + update_putter_jam_detection(); + } + + // 保存当前状态用于下次比较 + last_switch_right_ = switch_right; + last_switch_left_ = switch_left; + last_mouse_ = mouse; + last_keyboard_ = keyboard; + } + +private: + void reset_all_controls() { + last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; + last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + last_mouse_ = rmcs_msgs::Mouse::zero(); + last_keyboard_ = rmcs_msgs::Keyboard::zero(); + + overdrive_mode_ = false; + + bullet_feeder_control_angle_ = nan_; + bullet_feeder_angle_pid_.output_max = inf_; + bullet_feeder_velocity_pid_.reset(); + bullet_feeder_angle_pid_.reset(); + *bullet_feeder_control_torque_ = nan_; + + // shoot_stage_ = ShootStage::PRELOADING; + + putter_initialized = false; + putter_startpoint = nan_; + puttter_return_velocity_pid_.reset(); + putter_velocity_pid_.reset(); + putter_return_angle_pid.reset(); + *putter_control_torque_ = nan_; + + last_preload_flag_ = false; + last_photoelectric_sensor_status_ = false; + + bullet_feeder_faulty_count_ = 0; + bullet_feeder_cool_down_ = 0; + } + + void set_preloading() { + RCLCPP_INFO(get_logger(), "PRELOADING"); + shoot_stage_ = ShootStage::PRELOADING; + } + + void set_preloaded() { + RCLCPP_INFO(get_logger(), "PRELOADED"); + shoot_stage_ = ShootStage::PRELOADED; + last_preload_flag_ = false; + bullet_feeder_control_angle_ = *bullet_feeder_angle_ - 0.3; + } + + void set_shooting() { + RCLCPP_INFO(get_logger(), "SHOOTING"); + shoot_stage_ = ShootStage::SHOOTING; + } + + void update_jam_detection() { + // RCLCPP_INFO(get_logger(), "%.2f --", *bullet_feeder_control_torque_); + if (*bullet_feeder_control_torque_ < 300.0 || std::isnan(*bullet_feeder_control_torque_)) { + bullet_feeder_faulty_count_ = 0; + return; + } + + // 扭矩持续过大时累计故障计数 + if (bullet_feeder_faulty_count_ < 1000) + bullet_feeder_faulty_count_++; + else { + bullet_feeder_faulty_count_ = 0; + if (last_preload_flag_) + set_preloaded(); + else + enter_jam_protection(); + } + } + + void update_putter_jam_detection() { + if ((*putter_control_torque_ > -0.03 && shoot_stage_ == ShootStage::PRELOADING) + || (*putter_control_torque_ < 0.05 && shoot_stage_ == ShootStage::SHOOTING) + || std::isnan(*putter_control_torque_)) { + putter_faulty_count_ = 0; + return; + } + + // 扭矩异常时累计故障计数 + if (putter_faulty_count_ < 500) + ++putter_faulty_count_; + else { + putter_faulty_count_ = 0; + if (shoot_stage_ != ShootStage::SHOOTING) { + // 非发射状态下检测到堵转：推杆已到位，设置为已初始化 + putter_initialized = true; + putter_startpoint = *putter_angle_; + } else { + // 发射状态下检测到堵转：认为子弹已发出 + shooted = true; + } + } + } + + void enter_jam_protection() { + bullet_feeder_control_angle_ = nan_; + bullet_feeder_cool_down_ = 1000; + bullet_feeder_angle_pid_.reset(); + bullet_feeder_velocity_pid_.reset(); + RCLCPP_INFO(get_logger(), "Jammed!"); + } + + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); ///< 非数值常量 + static constexpr double inf_ = std::numeric_limits::infinity(); ///< 无穷大常量 + + static constexpr double low_latency_velocity_ = 5.0; ///< + // 低延迟预装弹速度 + + static constexpr double putter_stroke_ = 11.5; ///< 推杆行程长度 + + static constexpr double max_bullet_feeder_control_torque_ = 0.1; + static constexpr double bullet_feeder_angle_per_bullet_ = 2 * std::numbers::pi / 6; + + InputInterface photoelectric_sensor_status_; + bool last_photoelectric_sensor_status_; + InputInterface bullet_fired_; + bool shooted{false}; + + InputInterface friction_ready_; + + InputInterface switch_right_; + InputInterface switch_left_; + InputInterface mouse_; + InputInterface keyboard_; + + rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Mouse last_mouse_ = rmcs_msgs::Mouse::zero(); + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + + bool overdrive_mode_ = false; + + InputInterface bullet_feeder_angle_; + InputInterface bullet_feeder_velocity_; + + InputInterface control_bullet_allowance_limited_by_heat_; + + bool last_preload_flag_ = false; + + bool putter_initialized = false; + int putter_faulty_count_ = 0; + double putter_startpoint = nan_; + pid::PidCalculator puttter_return_velocity_pid_; + InputInterface putter_velocity_; + + pid::PidCalculator putter_velocity_pid_; + + enum class ShootStage { PRELOADING, PRELOADED, SHOOTING }; + ShootStage shoot_stage_ = ShootStage::PRELOADING; + double bullet_feeder_control_angle_ = nan_; + + pid::PidCalculator bullet_feeder_velocity_pid_; + pid::PidCalculator bullet_feeder_angle_pid_; + OutputInterface bullet_feeder_control_torque_; + + InputInterface putter_angle_; + pid::PidCalculator putter_return_angle_pid; + OutputInterface putter_control_torque_; + + int bullet_feeder_faulty_count_ = 0; + int bullet_feeder_cool_down_ = 0; + + OutputInterface shoot_mode_; +}; + +} // namespace rmcs_core::controller::shooting + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::shooting::PutterController, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/shooting_recorder.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/shooting_recorder.cpp index 5500b15c..c0ef5af2 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/shooting_recorder.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/shooting_recorder.cpp @@ -1,7 +1,10 @@ +#include +#include #include #include #include #include +#include namespace rmcs_core::controller::shooting { @@ -18,44 +21,71 @@ class ShootingRecorder log_mode_ = static_cast(get_parameter("log_mode").as_int()); + aim_velocity = get_parameter("aim_velocity").as_double(); + register_input("/referee/shooter/initial_speed", initial_speed_); register_input("/referee/shooter/shoot_timestamp", shoot_timestamp_); - register_input("/friction_wheels/temperature", fractional_temperature_); if (friction_wheel_count_ == 2) { - const auto topic = std::array{ - "/gimbal/left_friction/control_velocity", - "/gimbal/right_friction/control_velocity", - }; - for (int i = 0; i < 2; i++) - register_input(topic[i], friction_wheels_velocity_[i]); - } else if (friction_wheel_count_ == 4) { const auto topic = std::array{ "/gimbal/first_left_friction/control_velocity", "/gimbal/first_right_friction/control_velocity", - "/gimbal/second_left_friction/control_velocity", - "/gimbal/second_right_friction/control_velocity", }; - for (int i = 0; i < 4; i++) + for (int i = 0; i < 2; i++) register_input(topic[i], friction_wheels_velocity_[i]); + register_input("/gimbal/first_left_friction/velocity", friction_velocities_[0]); + register_input("/gimbal/first_right_friction/velocity", friction_velocities_[1]); } using namespace std::chrono; auto now = high_resolution_clock::now(); - auto ms = duration_cast(now.time_since_epoch()).count(); + auto ms = duration_cast(now.time_since_epoch()).count(); auto file = fmt::format("/robot_shoot/{}.log", ms); log_stream_.open(file); + + std::ofstream outFile("shoot_recorder"); + RCLCPP_INFO(get_logger(), "ShootingRecorder initialized, log file: %s", file.c_str()); } ~ShootingRecorder() { log_stream_.close(); } void update() override { + // if (*friction_velocities_[0] <= 366.0 && *friction_velocities_[1] <= 366.0) + // return; + // if (start_time_ == 0.0) { + // start_time_ = GetTime(); + // } + + // int flag = 0; + // if (GetTime() - start_time_ > 10.0) { + // if (flag == 0) { + // RCLCPP_INFO(get_logger(), "vv = %f", vv); + // flag = 1; + // } + // return; + // } + + // vv = std::max(vv, abs(*friction_velocities_[0] + *friction_velocities_[1])); + + // auto log_text = std::string{}; + // log_text = fmt::format( + // "{:.3f}, {:.3f}, {:.3f}", *friction_velocities_[0], *friction_velocities_[1], + // (*friction_velocities_[0] + *friction_velocities_[1])); + + // log_stream_ << log_text << std::endl; + // RCLCPP_INFO(get_logger(), "%s", log_text.c_str()); + + // std::ofstream outFile("shoot_recorder", std::ios::app); + // if (outFile.is_open()) { + // outFile << log_text << std::endl; + // outFile.close(); + // } switch (log_mode_) { case LogMode::TRIGGER: // It will be triggered by shooting action - if (*shoot_timestamp_ == last_shoot_timestamp_) + if (*shoot_timestamp_ == last_shoot_timestamp_ || v == *shoot_timestamp_) return; break; case LogMode::TIMING: @@ -64,38 +94,45 @@ class ShootingRecorder return; break; } + v = *shoot_timestamp_; - auto log_text = std::string{}; + velocities.push_back(*initial_speed_); + + analysis3(); + + auto log_text = std::string{}; auto timestamp = timestamp_to_string(*shoot_timestamp_); if (friction_wheel_count_ == 2) { log_text = fmt::format( - "{},{},{},{},{}", timestamp, *initial_speed_, // - *friction_wheels_velocity_[0], *friction_wheels_velocity_[1], - *fractional_temperature_); - } else if (friction_wheel_count_ == 4) { - log_text = fmt::format( - "{},{},{},{},{},{},{}", timestamp, *initial_speed_, // - *friction_wheels_velocity_[0], *friction_wheels_velocity_[1], - *friction_wheels_velocity_[2], *friction_wheels_velocity_[3], - *fractional_temperature_); + "{},{},{:.3f},{:.3f},{:.3f},{:.3f},{:.3f},{:.3f},{:.3f}", *initial_speed_, + (int)velocities.size(), // + velocity_, excellence_rate_, pass_rate_, range_, range2_, velocity_max, + velocity_min); } log_stream_ << log_text << std::endl; RCLCPP_INFO(get_logger(), "%s", log_text.c_str()); + log_velocity = fmt::format("{:.3f}", *initial_speed_); + std::ofstream outFile("shoot_recorder", std::ios::app); + if (outFile.is_open()) { + outFile << log_velocity << std::endl; + outFile.close(); + } + last_shoot_timestamp_ = *shoot_timestamp_; } private: /// @brief Component interface + std::array, 2> friction_velocities_; + InputInterface initial_speed_; InputInterface shoot_timestamp_; - InputInterface fractional_temperature_; - std::size_t friction_wheel_count_ = 2; - std::array, 4> friction_wheels_velocity_; + std::array, 2> friction_wheels_velocity_; /// @brief For log enum class LogMode { TRIGGER = 1, TIMING = 2 }; @@ -103,25 +140,147 @@ class ShootingRecorder double last_shoot_timestamp_ = 0; std::ofstream log_stream_; + std::string log_velocity; std::size_t log_count_ = 0; + std::vector velocities; + + double velocity_; + + double excellence_rate_; + double pass_rate_; + + double range_; + double range2_; + + double velocity_min; + double velocity_max; + + double v; + double aim_velocity; + + double start_time_ = 0.0; + double vv = 0.0; + private: static std::string timestamp_to_string(double timestamp) { - auto time = static_cast(timestamp); + auto time = static_cast(timestamp); auto local_time = std::localtime(&time); char buffer[100]; std::strftime(buffer, sizeof(buffer), "%Y-%m-%d %H:%M:%S", local_time); double fractional_seconds = timestamp - std::floor(timestamp); - int milliseconds = static_cast(fractional_seconds * 1000); + int milliseconds = static_cast(fractional_seconds * 1000); char result[150]; std::snprintf(result, sizeof(result), "%s.%03d", buffer, milliseconds); return result; } + void analysis1() { + double sum = 0.0; + for (const auto& v : velocities) { + sum += v; + } + velocity_ = sum / double(velocities.size()); + + sort(velocities.begin(), velocities.end()); + + range_ = velocities.back() - velocities.front(); + range2_ = velocities[int(velocities.size() - 2)] - velocities[1]; + + velocity_max = velocities.back(); + velocity_min = velocities.front(); + + int excellence_count = 0; + int pass_count = 0; + for (int i = 0; i < int(velocities.size()); i++) { + if (velocities[i] >= velocity_ - 0.1 && velocities[i] <= velocity_ + 0.1) { + pass_count += 1; + } + if (velocities[i] >= velocity_ - 0.05 && velocities[i] <= velocity_ + 0.05) { + excellence_count += 1; + } + } + excellence_rate_ = double(excellence_count) / double(velocities.size()); + pass_rate_ = double(pass_count) / double(velocities.size()); + } + + void analysis2() { + double sum = 0.0; + for (const auto& v : velocities) { + sum += v; + } + + sort(velocities.begin(), velocities.end()); + + velocity_max = velocities.back(); + velocity_min = velocities.front(); + + int n_adjust = std::max(1, int((int)velocities.size() * 0.05)); + + for (int i = 0; i < n_adjust; i++) { + sum -= velocities[i]; + sum -= velocities[velocities.size() - 1 - i]; + } + + velocity_ = sum / double(velocities.size() - 2 * n_adjust); + + range_ = velocities.back() - velocities.front(); + range2_ = velocities[int(velocities.size() - 2)] - velocities[1]; + + int excellence_count = 0; + int pass_count = 0; + for (int i = 0; i < int(velocities.size()); i++) { + if (velocities[i] >= velocity_ - 0.1 && velocities[i] <= velocity_ + 0.1) { + pass_count += 1; + } + if (velocities[i] >= velocity_ - 0.05 && velocities[i] <= velocity_ + 0.05) { + excellence_count += 1; + } + } + excellence_rate_ = double(excellence_count) / double(velocities.size()); + pass_rate_ = double(pass_count) / double(velocities.size()); + } + + void analysis3() { + double sum = 0.0; + for (const auto& v : velocities) { + sum += v; + } + velocity_ = sum / double(velocities.size()); + + int excellence_count = 0; + int pass_count = 0; + + for (const auto& v : velocities) { + if (v >= aim_velocity - 0.05 && v <= aim_velocity + 0.05) { + excellence_count += 1; + } + if (v >= aim_velocity - 0.1 && v <= aim_velocity + 0.1) { + pass_count += 1; + } + } + excellence_rate_ = double(excellence_count) / double(velocities.size()); + pass_rate_ = double(pass_count) / double(velocities.size()); + + sort(velocities.begin(), velocities.end()); + velocity_max = velocities.back(); + velocity_min = velocities.front(); + + range_ = velocities.back() - velocities.front(); + range2_ = velocities[int(velocities.size() - 2)] - velocities[1]; + } + + static double GetTime() { + using namespace std::chrono; + static auto start = high_resolution_clock::now(); + auto now = high_resolution_clock::now(); + duration elapsed = now - start; + return elapsed.count(); + } }; } // namespace rmcs_core::controller::shooting diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp index d30c1ce0..8f89cd2a 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp @@ -1,29 +1,22 @@ +#include +#include #include -#include +#include #include -#include -#include -#include -#include - -#include -#include -#include +#include +#include #include +#include + +#include #include -#include -#include -#include #include #include #include -#include #include #include -#include "hardware/device/benewake.hpp" #include "hardware/device/bmi088.hpp" -#include "hardware/device/can_packet.hpp" #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" #include "hardware/device/lk_motor.hpp" @@ -36,16 +29,14 @@ class SteeringHero , public rclcpp::Node { public: SteeringHero() - : Node{ - get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , command_component_( create_partner_component( get_component_name() + "_command", *this)) { + using namespace rmcs_description; register_output("/tf", tf_); - tf_->set_transform( - Eigen::Translation3d{0.16, 0.0, 0.15}); + tf_->set_transform(Eigen::Translation3d{0.16, 0.0, 0.15}); gimbal_calibrate_subscription_ = create_subscription( "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { @@ -53,67 +44,66 @@ class SteeringHero }); top_board_ = std::make_unique( - *this, *command_component_, get_parameter("board_serial_top_board").as_string()); - bottom_board_ = std::make_unique( - *this, *command_component_, get_parameter("board_serial_bottom_board").as_string()); - - temperature_logging_timer_.reset(1000); + *this, *command_component_, + static_cast(get_parameter("usb_pid_top_board").as_int())); + bottom_board_one_ = std::make_unique( + *this, *command_component_, + static_cast(get_parameter("usb_pid_bottom_board_one").as_int())); + bottom_board_two_ = std::make_unique( + *this, *command_component_, + static_cast(get_parameter("usb_pid_bottom_board_two").as_int())); + + // temperature_logging_timer_.reset(1000); } - SteeringHero(const SteeringHero&) = delete; - SteeringHero& operator=(const SteeringHero&) = delete; - SteeringHero(SteeringHero&&) = delete; - SteeringHero& operator=(SteeringHero&&) = delete; - ~SteeringHero() override = default; void update() override { top_board_->update(); - bottom_board_->update(); - - if (temperature_logging_timer_.tick()) { - temperature_logging_timer_.reset(1000); - RCLCPP_INFO( - get_logger(), - "Temperature: pitch: %.1f, top_yaw: %.1f, bottom_yaw: %.1f, feeder: %.1f", - top_board_->gimbal_pitch_motor_.temperature(), - top_board_->gimbal_top_yaw_motor_.temperature(), - bottom_board_->gimbal_bottom_yaw_motor_.temperature(), - top_board_->gimbal_bullet_feeder_.temperature()); - } + bottom_board_one_->update(); + bottom_board_two_->update(); + + // if (temperature_logging_timer_.tick()) { + // temperature_logging_timer_.reset(1000); + // RCLCPP_INFO( + // get_logger(), + // "Temperature: pitch: %.1f, top_yaw: %.1f, bottom_yaw: %.1f, feeder: %.1f", + // top_board_->gimbal_pitch_motor_.temperature(), + // top_board_->gimbal_top_yaw_motor_.temperature(), + // bottom_board_->gimbal_bottom_yaw_motor_.temperature(), + // top_board_->gimbal_bullet_feeder_.temperature()); + // } } void command_update() { top_board_->command_update(); - bottom_board_->command_update(); + bottom_board_one_->command_update(); + bottom_board_two_->command_update(); } private: void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { RCLCPP_INFO( get_logger(), "[gimbal calibration] New bottom yaw offset: %ld", - bottom_board_->gimbal_bottom_yaw_motor_.calibrate_zero_point()); + bottom_board_two_->gimbal_bottom_yaw_motor_.calibrate_zero_point()); RCLCPP_INFO( get_logger(), "[gimbal calibration] New pitch offset: %ld", top_board_->gimbal_pitch_motor_.calibrate_zero_point()); RCLCPP_INFO( get_logger(), "[gimbal calibration] New top yaw offset: %ld", top_board_->gimbal_top_yaw_motor_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New viewer offset: %ld", - top_board_->gimbal_player_viewer_motor_.calibrate_zero_point()); RCLCPP_INFO( get_logger(), "[chassis calibration] left front steering offset: %d", - bottom_board_->chassis_steering_motors_[0].calibrate_zero_point()); + bottom_board_one_->chassis_steering_motors_[0].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[chassis calibration] right front steering offset: %d", + bottom_board_one_->chassis_steering_motors_[1].calibrate_zero_point()); RCLCPP_INFO( get_logger(), "[chassis calibration] left back steering offset: %d", - bottom_board_->chassis_steering_motors_[1].calibrate_zero_point()); + bottom_board_two_->chassis_steering_motors2_[0].calibrate_zero_point()); RCLCPP_INFO( get_logger(), "[chassis calibration] right back steering offset: %d", - bottom_board_->chassis_steering_motors_[2].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[chassis calibration] right front steering offset: %d", - bottom_board_->chassis_steering_motors_[3].calibrate_zero_point()); + bottom_board_two_->chassis_steering_motors2_[1].calibrate_zero_point()); } class SteeringHeroCommand : public rmcs_executor::Component { @@ -123,61 +113,54 @@ class SteeringHero void update() override { hero_.command_update(); } - private: SteeringHero& hero_; }; std::shared_ptr command_component_; - class TopBoard final : private librmcs::agent::CBoard { + class TopBoard final : private librmcs::client::CBoard { public: friend class SteeringHero; - explicit TopBoard( - SteeringHero& hero, SteeringHeroCommand& hero_command, - std::string_view board_serial = {}) - : librmcs::agent::CBoard(board_serial) + explicit TopBoard(SteeringHero& hero, SteeringHeroCommand& hero_command, int usb_pid = + -1) + : librmcs::client::CBoard(usb_pid) + , logger_(hero.get_logger()) , tf_(hero.tf_) , imu_(1000, 0.2, 0.0) - , benewake_(hero, "/gimbal/auto_aim/laser_distance") , gimbal_top_yaw_motor_( hero, hero_command, "/gimbal/top_yaw", - device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} + device::LkMotor::Config{device::LkMotor::Type::MG5010E_I10} .set_encoder_zero_point( static_cast( hero.get_parameter("top_yaw_motor_zero_point").as_int()))) , gimbal_pitch_motor_( hero, hero_command, "/gimbal/pitch", - device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} + device::LkMotor::Config{device::LkMotor::Type::MG5010E_I10} .set_encoder_zero_point( static_cast(hero.get_parameter("pitch_motor_zero_point").as_int()))) , gimbal_friction_wheels_( - {hero, hero_command, "/gimbal/second_left_friction", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio( - 1.)}, {hero, hero_command, "/gimbal/first_left_friction", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio( - 1.)}, + device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(1.)}, {hero, hero_command, "/gimbal/first_right_friction", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + device::DjiMotor::Config{device::DjiMotor::Type::M3508} .set_reduction_ratio(1.) .set_reversed()}, + {hero, hero_command, "/gimbal/second_left_friction", + device::DjiMotor::Config{device::DjiMotor::Type::M3508} + .set_reduction_ratio(1.) + .set_reversed()}, {hero, hero_command, "/gimbal/second_right_friction", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + device::DjiMotor::Config{device::DjiMotor::Type::M3508} .set_reduction_ratio(1.) .set_reversed()}) , gimbal_bullet_feeder_( hero, hero_command, "/gimbal/bullet_feeder", - device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} + device::LkMotor::Config{device::LkMotor::Type::MG5010E_I10} .set_reversed() .enable_multi_turn_angle()) - , gimbal_scope_motor_( - hero, hero_command, "/gimbal/scope", - device::DjiMotor::Config{device::DjiMotor::Type::kM2006}) - , gimbal_player_viewer_motor_( - hero, hero_command, "/gimbal/player_viewer", - device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} - .set_encoder_zero_point( - static_cast(hero.get_parameter("viewer_motor_zero_point").as_int())) - .set_reversed()) { + , putter_motor_{hero, hero_command, "/gimbal/putter", + device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(1.).enable_multi_turn_angle()} + , transmit_buffer_(*this, 32) + , event_thread_([this]() { handle_events(); }) { imu_.set_coordinate_mapping([](double x, double y, double z) { // Get the mapping with the following code. @@ -194,24 +177,26 @@ class SteeringHero hero.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); hero.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); - } - TopBoard(const TopBoard&) = delete; - TopBoard& operator=(const TopBoard&) = delete; - TopBoard(TopBoard&&) = delete; - TopBoard& operator=(TopBoard&&) = delete; + hero.register_output( + "/gimbal/photoelectric_sensor", photoelectric_sensor_status_, false); + hero.register_output( + "/auto_aim/image_capturer/timestamp", camera_capturer_trigger_timestamp_, 0); + hero.register_output("/auto_aim/image_capturer/trigger", camera_capturer_trigger_, 0); + } - ~TopBoard() final = default; + ~TopBoard() final { + stop_handling_events(); + event_thread_.join(); + } void update() { imu_.update_status(); - const Eigen::Quaterniond gimbal_imu_pose{imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()}; + Eigen::Quaterniond gimbal_imu_pose{imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()}; tf_->set_transform( gimbal_imu_pose.conjugate()); - benewake_.update_status(); - *gimbal_yaw_velocity_imu_ = imu_.gz(); *gimbal_pitch_velocity_imu_ = imu_.gy(); @@ -221,123 +206,97 @@ class SteeringHero tf_->set_state( gimbal_pitch_motor_.angle()); - gimbal_player_viewer_motor_.update_status(); - tf_->set_state( - gimbal_player_viewer_motor_.angle()); - - gimbal_scope_motor_.update_status(); - for (auto& motor : gimbal_friction_wheels_) motor.update_status(); gimbal_bullet_feeder_.update_status(); + putter_motor_.update_status(); + + if (last_camera_capturer_trigger_timestamp_ != *camera_capturer_trigger_timestamp_) + *camera_capturer_trigger_ = true; + last_camera_capturer_trigger_timestamp_ = *camera_capturer_trigger_timestamp_; } void command_update() { - auto builder = start_transmit(); - - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_friction_wheels_[0].generate_command(), - gimbal_friction_wheels_[1].generate_command(), - gimbal_friction_wheels_[2].generate_command(), - gimbal_friction_wheels_[3].generate_command(), - } - .as_bytes(), - }); + uint16_t batch_commands[4]{}; + + batch_commands[0] = gimbal_friction_wheels_[3].generate_command(); + batch_commands[1] = gimbal_friction_wheels_[1].generate_command(); + batch_commands[2] = gimbal_friction_wheels_[2].generate_command(); + batch_commands[3] = gimbal_friction_wheels_[0].generate_command(); + transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(batch_commands)); + + frequency_control_flag_ = !frequency_control_flag_; + if (frequency_control_flag_) { + batch_commands[0] = putter_motor_.generate_command(); + transmit_buffer_.add_can1_transmission( + 0x1ff, std::bit_cast(batch_commands)); + } - builder.can1_transmit({ - .can_id = 0x141, - .can_data = gimbal_bullet_feeder_ - .generate_torque_command(gimbal_bullet_feeder_.control_torque()) - .as_bytes(), - }); + transmit_buffer_.add_can1_transmission( + 0x141, gimbal_bullet_feeder_.generate_torque_command( + gimbal_bullet_feeder_.control_torque())); - builder.can2_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_scope_motor_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); + transmit_buffer_.add_can2_transmission(0x141, gimbal_top_yaw_motor_.generate_command()); - builder.can2_transmit({ - .can_id = 0x143, - .can_data = - gimbal_player_viewer_motor_ - .generate_velocity_command(gimbal_player_viewer_motor_.control_velocity()) - .as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x141, - .can_data = gimbal_top_yaw_motor_.generate_command().as_bytes(), - }); + transmit_buffer_.add_can2_transmission(0x142, gimbal_pitch_motor_.generate_command()); - builder.can2_transmit({ - .can_id = 0x142, - .can_data = gimbal_pitch_motor_.generate_command().as_bytes(), - }); + transmit_buffer_.trigger_transmission(); } private: - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + void can1_receive_callback( + uint32_t can_id, uint64_t can_data, bool is_extended_can_id, + bool is_remote_transmission, uint8_t can_data_length) override { + if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] return; - auto can_id = data.can_id; - if (can_id == 0x201) { - gimbal_friction_wheels_[0].store_status(data.can_data); + if (can_id == 0x204) { + gimbal_friction_wheels_[0].store_status(can_data); } else if (can_id == 0x202) { - gimbal_friction_wheels_[1].store_status(data.can_data); + gimbal_friction_wheels_[1].store_status(can_data); } else if (can_id == 0x203) { - gimbal_friction_wheels_[2].store_status(data.can_data); - } else if (can_id == 0x204) { - gimbal_friction_wheels_[3].store_status(data.can_data); + gimbal_friction_wheels_[2].store_status(can_data); + } else if (can_id == 0x201) { + gimbal_friction_wheels_[3].store_status(can_data); + } else if (can_id == 0x205) { + putter_motor_.store_status(can_data); } else if (can_id == 0x141) { - gimbal_bullet_feeder_.store_status(data.can_data); + gimbal_bullet_feeder_.store_status(can_data); } } - void can2_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + void can2_receive_callback( + uint32_t can_id, uint64_t can_data, bool is_extended_can_id, + bool is_remote_transmission, uint8_t can_data_length) override { + if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] return; - auto can_id = data.can_id; if (can_id == 0x141) { - gimbal_top_yaw_motor_.store_status(data.can_data); + gimbal_top_yaw_motor_.store_status(can_data); } else if (can_id == 0x142) { - gimbal_pitch_motor_.store_status(data.can_data); - } else if (can_id == 0x143) { - gimbal_player_viewer_motor_.store_status(data.can_data); - } else if (can_id == 0x201) { - gimbal_scope_motor_.store_status(data.can_data); + gimbal_pitch_motor_.store_status(can_data); } } - void uart2_receive_callback(const librmcs::data::UartDataView& data) override { - benewake_.store_status(data.uart_data.data(), data.uart_data.size()); + void accelerometer_receive_callback(int16_t x, int16_t y, int16_t z) override { + imu_.store_accelerometer_status(x, y, z); } - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - imu_.store_accelerometer_status(data.x, data.y, data.z); + void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { + imu_.store_gyroscope_status(x, y, z); } - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - imu_.store_gyroscope_status(data.x, data.y, data.z); - } + void putter_receive_callback(bool status) { *photoelectric_sensor_status_ = status; } + rclcpp::Logger logger_; OutputInterface& tf_; + OutputInterface photoelectric_sensor_status_; + OutputInterface camera_capturer_trigger_; + OutputInterface camera_capturer_trigger_timestamp_; + std::time_t last_camera_capturer_trigger_timestamp_{0}; device::Bmi088 imu_; - device::Benewake benewake_; OutputInterface gimbal_yaw_velocity_imu_; OutputInterface gimbal_pitch_velocity_imu_; @@ -348,230 +307,379 @@ class SteeringHero device::DjiMotor gimbal_friction_wheels_[4]; device::LkMotor gimbal_bullet_feeder_; - device::DjiMotor gimbal_scope_motor_; - device::LkMotor gimbal_player_viewer_motor_; + bool frequency_control_flag_{false}; + device::DjiMotor putter_motor_; + + librmcs::client::CBoard::TransmitBuffer transmit_buffer_; + std::thread event_thread_; }; - class BottomBoard final : private librmcs::agent::CBoard { + class BottomBoard_one final : private librmcs::client::CBoard { public: friend class SteeringHero; - explicit BottomBoard( - SteeringHero& hero, SteeringHeroCommand& hero_command, - std::string_view board_serial = {}) - : librmcs::agent::CBoard(board_serial) + explicit BottomBoard_one( + SteeringHero& hero, SteeringHeroCommand& hero_command, int usb_pid = -1) + : librmcs::client::CBoard(usb_pid) + , logger_(hero.get_logger()) , imu_(1000, 0.2, 0.0) - , tf_(hero.tf_) - , dr16_(hero) + , chassis_front_climber_motor_( + {hero, hero_command, "/chassis/climber/left_front_motor", + device::DjiMotor::Config{device::DjiMotor::Type::M3508} + .set_reversed() + .set_reduction_ratio(19.)}, + {hero, hero_command, "/chassis/climber/right_front_motor", + device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio( + 19.)}) + , chassis_back_climber_motor_( + {hero, hero_command, "/chassis/climber/left_back_motor", + device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio( + 19.)}, + {hero, hero_command, "/chassis/climber/right_back_motor", + device::DjiMotor::Config{device::DjiMotor::Type::M3508} + .set_reversed() + .set_reduction_ratio(19.)}) , chassis_steering_motors_( {hero, hero_command, "/chassis/left_front_steering", - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + device::DjiMotor::Config{device::DjiMotor::Type::GM6020} .set_encoder_zero_point( static_cast(hero.get_parameter("left_front_zero_point").as_int())) .set_reversed()}, - {hero, hero_command, "/chassis/left_back_steering", - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_encoder_zero_point( - static_cast(hero.get_parameter("left_back_zero_point").as_int())) - .set_reversed()}, - {hero, hero_command, "/chassis/right_back_steering", - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_encoder_zero_point( - static_cast(hero.get_parameter("right_back_zero_point").as_int())) - .set_reversed()}, {hero, hero_command, "/chassis/right_front_steering", - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + device::DjiMotor::Config{device::DjiMotor::Type::GM6020} .set_encoder_zero_point( static_cast(hero.get_parameter("right_front_zero_point").as_int())) .set_reversed()}) , chassis_wheel_motors_( {hero, hero_command, "/chassis/left_front_wheel", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + device::DjiMotor::Config{device::DjiMotor::Type::M3508} .set_reversed() .set_reduction_ratio(2232. / 169.)}, + {hero, hero_command, "/chassis/right_front_wheel", + device::DjiMotor::Config{device::DjiMotor::Type::M3508} + .set_reversed() + .set_reduction_ratio(2232. / 169.)}) + + , transmit_buffer_(*this, 32) + , event_thread_([this]() { handle_events(); }) { + + // hero.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); + // hero.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); + + hero.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); + } + + ~BottomBoard_one() final { + stop_handling_events(); + event_thread_.join(); + } + + void update() { + imu_.update_status(); + + *chassis_yaw_velocity_imu_ = imu_.gz(); + + 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(); + + for (auto& motor : chassis_wheel_motors_) + motor.update_status(); + for (auto& motor : chassis_steering_motors_) + motor.update_status(); + } + + void command_update() { + uint16_t batch_commands[4]{}; + + batch_commands[0] = chassis_wheel_motors_[1].generate_command(); + batch_commands[1] = chassis_wheel_motors_[0].generate_command(); + batch_commands[2] = 0; + batch_commands[3] = 0; + transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(batch_commands)); + + batch_commands[0] = 0; + batch_commands[1] = chassis_steering_motors_[0].generate_command(); + batch_commands[2] = 0; + batch_commands[3] = chassis_steering_motors_[1].generate_command(); + transmit_buffer_.add_can1_transmission(0x1FE, std::bit_cast(batch_commands)); + + batch_commands[0] = chassis_back_climber_motor_[0].generate_command(); + batch_commands[1] = chassis_back_climber_motor_[1].generate_command(); + batch_commands[2] = chassis_front_climber_motor_[0].generate_command(); + batch_commands[3] = chassis_front_climber_motor_[1].generate_command(); + transmit_buffer_.add_can2_transmission(0x200, std::bit_cast(batch_commands)); + + transmit_buffer_.trigger_transmission(); + } + + private: + void can1_receive_callback( + uint32_t can_id, uint64_t can_data, bool is_extended_can_id, + bool is_remote_transmission, uint8_t can_data_length) override { + if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + return; + + if (can_id == 0x201) { + chassis_wheel_motors_[1].store_status(can_data); + } else if (can_id == 0x202) { + chassis_wheel_motors_[0].store_status(can_data); + } + if (can_id == 0x206) { + chassis_steering_motors_[0].store_status(can_data); + } else if (can_id == 0x208) { + chassis_steering_motors_[1].store_status(can_data); + } + } + + void can2_receive_callback( + uint32_t can_id, uint64_t can_data, bool is_extended_can_id, + bool is_remote_transmission, uint8_t can_data_length) override { + if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + return; + + if (can_id == 0x203) { + chassis_front_climber_motor_[0].store_status(can_data); + } else if (can_id == 0x204) { + chassis_front_climber_motor_[1].store_status(can_data); + } else if (can_id == 0x201) { + chassis_back_climber_motor_[0].store_status(can_data); + } else if (can_id == 0x202) { + chassis_back_climber_motor_[1].store_status(can_data); + } + } + rclcpp::Logger logger_; + // test + std::chrono::steady_clock::time_point last_time_; + + void calc_can_fps(double can_id) { + auto now = std::chrono::steady_clock::now(); + auto delta = + std::chrono::duration_cast(now - last_time_).count(); + + RCLCPP_INFO(logger_, "can id :%lf,fps:%ld", can_id, 1000000 / delta); + last_time_ = now; + } + + void accelerometer_receive_callback(int16_t x, int16_t y, int16_t z) override { + imu_.store_accelerometer_status(x, y, z); + } + + void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { + imu_.store_gyroscope_status(x, y, z); + } + device::Bmi088 imu_; + + OutputInterface chassis_yaw_velocity_imu_; + + device::DjiMotor chassis_front_climber_motor_[2]; + device::DjiMotor chassis_back_climber_motor_[2]; + + device::DjiMotor chassis_steering_motors_[2]; + device::DjiMotor chassis_wheel_motors_[2]; + + librmcs::utility::RingBuffer referee_ring_buffer_receive_{256}; + OutputInterface referee_serial_; + + librmcs::client::CBoard::TransmitBuffer transmit_buffer_; + std::thread event_thread_; + + OutputInterface gimbal_yaw_velocity_imu_; + OutputInterface gimbal_pitch_velocity_imu_; + }; + + class BottomBoard_two final : private librmcs::client::CBoard { + public: + friend class SteeringHero; + explicit BottomBoard_two( + SteeringHero& hero, SteeringHeroCommand& hero_command, int usb_pid = -1) + : librmcs::client::CBoard(usb_pid) + , logger_(hero.get_logger()) + , imu_(1000, 0.2, 0.0) + , tf_(hero.tf_) + , dr16_(hero) + , supercap_(hero, hero_command) + , chassis_steering_motors2_( + {hero, hero_command, "/chassis/left_back_steering", + device::DjiMotor::Config{device::DjiMotor::Type::GM6020} + .set_encoder_zero_point( + static_cast(hero.get_parameter("left_back_zero_point").as_int())) + .set_reversed()}, + {hero, hero_command, "/chassis/right_back_steering", + device::DjiMotor::Config{device::DjiMotor::Type::GM6020} + .set_encoder_zero_point( + static_cast(hero.get_parameter("right_back_zero_point").as_int())) + .set_reversed()}) + , chassis_wheel_motors2_( {hero, hero_command, "/chassis/left_back_wheel", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + device::DjiMotor::Config{device::DjiMotor::Type::M3508} .set_reversed() .set_reduction_ratio(2232. / 169.)}, {hero, hero_command, "/chassis/right_back_wheel", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)}, - {hero, hero_command, "/chassis/right_front_wheel", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + device::DjiMotor::Config{device::DjiMotor::Type::M3508} .set_reversed() .set_reduction_ratio(2232. / 169.)}) - , supercap_(hero, hero_command) , gimbal_bottom_yaw_motor_( hero, hero_command, "/gimbal/bottom_yaw", - device::LkMotor::Config{device::LkMotor::Type::kMG6012Ei8} + device::LkMotor::Config{device::LkMotor::Type::MG6012E_I8} .set_reversed() .set_encoder_zero_point( static_cast( - hero.get_parameter("bottom_yaw_motor_zero_point").as_int()))) { + hero.get_parameter("bottom_yaw_motor_zero_point").as_int()))) + , transmit_buffer_(*this, 32) + , event_thread_([this]() { handle_events(); }) { hero.register_output("/referee/serial", referee_serial_); referee_serial_->read = [this](std::byte* buffer, size_t size) { - return referee_ring_buffer_receive_.pop_front_n( - [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); + return referee_ring_buffer_receive_.pop_front_multi( + [&buffer](std::byte byte) { *buffer++ = byte; }, size); }; referee_serial_->write = [this](const std::byte* buffer, size_t size) { - start_transmit().uart1_transmit({ - .uart_data = std::span{buffer, size} - }); + transmit_buffer_.add_uart1_transmission(buffer, size); return size; }; - hero.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); - hero.register_output( "/chassis/powermeter/control_enable", powermeter_control_enabled_, false); hero.register_output( "/chassis/powermeter/charge_power_limit", powermeter_charge_power_limit_, 0.); - } + hero.register_output("/chassis/climber/angle_imu", chassis_climber_angle_imu_); - BottomBoard(const BottomBoard&) = delete; - BottomBoard& operator=(const BottomBoard&) = delete; - BottomBoard(BottomBoard&&) = delete; - BottomBoard& operator=(BottomBoard&&) = delete; + q0 = hero.get_parameter("q0").as_double(); + q1 = hero.get_parameter("q1").as_double(); + q2 = hero.get_parameter("q2").as_double(); + q3 = hero.get_parameter("q3").as_double(); + } - ~BottomBoard() final = default; + ~BottomBoard_two() final { + stop_handling_events(); + event_thread_.join(); + } void update() { imu_.update_status(); dr16_.update_status(); supercap_.update_status(); - *chassis_yaw_velocity_imu_ = imu_.gz(); + Eigen::Quaterniond q_used{q0, q1, q2, q3}; + Eigen::Quaterniond q_now{imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()}; + Eigen::Quaterniond chassis_climber_angle_pose = q_used.conjugate() * q_now; - for (auto& motor : chassis_wheel_motors_) + *chassis_climber_angle_imu_ = + chassis_climber_angle_pose.toRotationMatrix().eulerAngles(0, 1, 2).y(); + + for (auto& motor : chassis_wheel_motors2_) motor.update_status(); - for (auto& motor : chassis_steering_motors_) + for (auto& motor : chassis_steering_motors2_) motor.update_status(); + gimbal_bottom_yaw_motor_.update_status(); + tf_->set_state( gimbal_bottom_yaw_motor_.angle()); } void command_update() { - auto builder = start_transmit(); - - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[0].generate_command(), - chassis_wheel_motors_[1].generate_command(), - chassis_wheel_motors_[2].generate_command(), - chassis_wheel_motors_[3].generate_command(), - } - .as_bytes(), - }); - - builder.can1_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - supercap_.generate_command(), - } - .as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - chassis_steering_motors_[0].generate_command(), - chassis_steering_motors_[1].generate_command(), - chassis_steering_motors_[2].generate_command(), - chassis_steering_motors_[3].generate_command(), - } - .as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x141, - .can_data = gimbal_bottom_yaw_motor_.generate_command().as_bytes(), - }); + uint16_t batch_commands[4]{}; + + batch_commands[0] = 0; + batch_commands[1] = 0; + batch_commands[2] = chassis_wheel_motors2_[1].generate_command(); + batch_commands[3] = chassis_wheel_motors2_[0].generate_command(); + transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(batch_commands)); + + batch_commands[0] = chassis_steering_motors2_[0].generate_command(); + batch_commands[1] = 0; + batch_commands[2] = chassis_steering_motors2_[1].generate_command(); + batch_commands[3] = supercap_.generate_command(); + transmit_buffer_.add_can1_transmission(0x1FE, std::bit_cast(batch_commands)); + + transmit_buffer_.add_can2_transmission( + 0x141, gimbal_bottom_yaw_motor_.generate_command()); } private: - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + void can1_receive_callback( + uint32_t can_id, uint64_t can_data, bool is_extended_can_id, + bool is_remote_transmission, uint8_t can_data_length) override { + if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] return; - auto can_id = data.can_id; - if (can_id == 0x201) { - chassis_wheel_motors_[0].store_status(data.can_data); - } else if (can_id == 0x202) { - chassis_wheel_motors_[1].store_status(data.can_data); - } else if (can_id == 0x203) { - chassis_wheel_motors_[2].store_status(data.can_data); + if (can_id == 0x203) { + chassis_wheel_motors2_[1].store_status(can_data); } else if (can_id == 0x204) { - chassis_wheel_motors_[3].store_status(data.can_data); + chassis_wheel_motors2_[0].store_status(can_data); + } + if (can_id == 0x205) { + chassis_steering_motors2_[0].store_status(can_data); + } else if (can_id == 0x207) { + chassis_steering_motors2_[1].store_status(can_data); } else if (can_id == 0x300) { - supercap_.store_status(data.can_data); + supercap_.store_status(can_data); } } - void can2_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + void can2_receive_callback( + uint32_t can_id, uint64_t can_data, bool is_extended_can_id, + bool is_remote_transmission, uint8_t can_data_length) override { + if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] return; - auto can_id = data.can_id; - if (can_id == 0x205) { - chassis_steering_motors_[0].store_status(data.can_data); - } else if (can_id == 0x206) { - chassis_steering_motors_[1].store_status(data.can_data); - } else if (can_id == 0x207) { - chassis_steering_motors_[2].store_status(data.can_data); - } else if (can_id == 0x208) { - chassis_steering_motors_[3].store_status(data.can_data); - } else if (can_id == 0x141) { - gimbal_bottom_yaw_motor_.store_status(data.can_data); + if (can_id == 0x141) { + gimbal_bottom_yaw_motor_.store_status(can_data); } } + rclcpp::Logger logger_; + // test + std::chrono::steady_clock::time_point last_time_; - void uart1_receive_callback(const librmcs::data::UartDataView& data) override { - const auto* uart_data = data.uart_data.data(); - referee_ring_buffer_receive_.emplace_back_n( - [&uart_data](std::byte* storage) noexcept { *storage = *uart_data++; }, - data.uart_data.size()); + void calc_can_fps(double can_id) { + auto now = std::chrono::steady_clock::now(); + auto delta = + std::chrono::duration_cast(now - last_time_).count(); + + RCLCPP_INFO(logger_, "can id :%lf,fps:%ld", can_id, 1000000 / delta); + last_time_ = now; + } + // test + void uart1_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { + referee_ring_buffer_receive_.emplace_back_multi( + [&uart_data](std::byte* storage) { *storage = *uart_data++; }, uart_data_length); } - void dbus_receive_callback(const librmcs::data::UartDataView& data) override { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { + dr16_.store_status(uart_data, uart_data_length); } - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - imu_.store_accelerometer_status(data.x, data.y, data.z); + void accelerometer_receive_callback(int16_t x, int16_t y, int16_t z) override { + imu_.store_accelerometer_status(x, y, z); } - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - imu_.store_gyroscope_status(data.x, data.y, data.z); + void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { + imu_.store_gyroscope_status(x, y, z); } + double q0, q1, q2, q3; + device::Bmi088 imu_; OutputInterface& tf_; - OutputInterface chassis_yaw_velocity_imu_; - OutputInterface powermeter_control_enabled_; OutputInterface powermeter_charge_power_limit_; + OutputInterface chassis_climber_angle_imu_; device::Dr16 dr16_; - - device::DjiMotor chassis_steering_motors_[4]; - device::DjiMotor chassis_wheel_motors_[4]; device::Supercap supercap_; + device::DjiMotor chassis_steering_motors2_[2]; + device::DjiMotor chassis_wheel_motors2_[2]; device::LkMotor gimbal_bottom_yaw_motor_; - rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; + librmcs::utility::RingBuffer referee_ring_buffer_receive_{256}; OutputInterface referee_serial_; + + librmcs::client::CBoard::TransmitBuffer transmit_buffer_; + std::thread event_thread_; }; OutputInterface tf_; @@ -579,7 +687,8 @@ class SteeringHero rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; std::unique_ptr top_board_; - std::unique_ptr bottom_board_; + std::unique_ptr bottom_board_one_; + std::unique_ptr bottom_board_two_; rmcs_utility::TickTimer temperature_logging_timer_; }; @@ -588,4 +697,4 @@ class SteeringHero #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringHero, rmcs_executor::Component) +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringHero, rmcs_executor::Component) \ No newline at end of file From a2765250597e3780b939463d46383df08f6ef0c3 Mon Sep 17 00:00:00 2001 From: Fin_Resect Date: Mon, 16 Mar 2026 01:03:18 +0800 Subject: [PATCH 2/7] Update to librmcs v3.1.0b0 --- .../rmcs_bringup/config/steering-hero.yaml | 6 +- rmcs_ws/src/rmcs_core/CMakeLists.txt | 4 +- .../rmcs_core/src/hardware/steering-hero.cpp | 747 +++++++++--------- 3 files changed, 381 insertions(+), 376 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml index 7e7c551e..49957f14 100644 --- a/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml @@ -37,9 +37,9 @@ rmcs_executor: hero_hardware: ros__parameters: - usb_pid_top_board: 0xce3b - usb_pid_bottom_board_one: 0x902e - usb_pid_bottom_board_two: 0x317e + board_serial_top_board: "D4-2BCA-2E47-76CD-23BC-0B78-684B" + board_serial_bottom_board_one: "D4-3674-7174-8768-879E-E44A-3931" + board_serial_bottom_board_two: "D4-7973-19A9-EA40-4A3E-306F-10F9" bottom_yaw_motor_zero_point: 50656 pitch_motor_zero_point: 63917 top_yaw_motor_zero_point: 59095 diff --git a/rmcs_ws/src/rmcs_core/CMakeLists.txt b/rmcs_ws/src/rmcs_core/CMakeLists.txt index dd3c8579..ec80d3ed 100644 --- a/rmcs_ws/src/rmcs_core/CMakeLists.txt +++ b/rmcs_ws/src/rmcs_core/CMakeLists.txt @@ -17,8 +17,8 @@ include(FetchContent) set(BUILD_STATIC_LIBRMCS ON CACHE BOOL "Build static librmcs SDK" FORCE) FetchContent_Declare( librmcs - URL https://github.com/Alliance-Algorithm/librmcs/releases/download/v3.0.0/librmcs-sdk-src-3.0.0.zip - URL_HASH SHA256=b39f51c21baacdcbf3f0176119b8850137a108b88a67e12395d37d89e5ef53e8 + URL https://github.com/Alliance-Algorithm/librmcs/releases/download/v3.1.0b0/librmcs-sdk-src-3.1.0-beta.0.zip + URL_HASH SHA256=7f3203e8db8cb7954ae0ac2ee226f93e32b367b3956b1f0bb5c7ae203c2ccbef DOWNLOAD_EXTRACT_TIMESTAMP TRUE ) FetchContent_MakeAvailable(librmcs) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp index 8f89cd2a..ae4a6a50 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp @@ -1,22 +1,28 @@ -#include -#include #include -#include +#include #include -#include +#include +#include +#include +#include + +#include +#include +#include #include #include -#include - -#include #include +#include +#include +#include #include #include #include -#include +#include #include #include "hardware/device/bmi088.hpp" +#include "hardware/device/can_packet.hpp" #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" #include "hardware/device/lk_motor.hpp" @@ -29,50 +35,43 @@ class SteeringHero , public rclcpp::Node { public: SteeringHero() - : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + : Node( + get_component_name(), + rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) , command_component_( create_partner_component( get_component_name() + "_command", *this)) { - using namespace rmcs_description; - register_output("/tf", tf_); - tf_->set_transform(Eigen::Translation3d{0.16, 0.0, 0.15}); - gimbal_calibrate_subscription_ = create_subscription( - "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { - gimbal_calibrate_subscription_callback(std::move(msg)); - }); + // gimbal_calibrate_subscription_ = create_subscription( + // "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { + // gimbal_calibrate_subscription_callback(std::move(msg)); + // }); top_board_ = std::make_unique( - *this, *command_component_, - static_cast(get_parameter("usb_pid_top_board").as_int())); + *this, *command_component_, get_parameter("board_serial_top_board").as_string()); + bottom_board_one_ = std::make_unique( - *this, *command_component_, - static_cast(get_parameter("usb_pid_bottom_board_one").as_int())); + *this, *command_component_, get_parameter("board_serial_bottom_board_one").as_string()); + bottom_board_two_ = std::make_unique( - *this, *command_component_, - static_cast(get_parameter("usb_pid_bottom_board_two").as_int())); + *this, *command_component_, get_parameter("board_serial_bottom_board_two").as_string()); - // temperature_logging_timer_.reset(1000); + tf_->set_transform( + Eigen::Translation3d{0.06603, 0.0, 0.082}); } + SteeringHero(const SteeringHero&) = delete; + SteeringHero& operator=(const SteeringHero&) = delete; + SteeringHero(SteeringHero&&) = delete; + SteeringHero& operator=(SteeringHero&&) = delete; + ~SteeringHero() override = default; void update() override { top_board_->update(); bottom_board_one_->update(); bottom_board_two_->update(); - - // if (temperature_logging_timer_.tick()) { - // temperature_logging_timer_.reset(1000); - // RCLCPP_INFO( - // get_logger(), - // "Temperature: pitch: %.1f, top_yaw: %.1f, bottom_yaw: %.1f, feeder: %.1f", - // top_board_->gimbal_pitch_motor_.temperature(), - // top_board_->gimbal_top_yaw_motor_.temperature(), - // bottom_board_->gimbal_bottom_yaw_motor_.temperature(), - // top_board_->gimbal_bullet_feeder_.temperature()); - // } } void command_update() { @@ -117,79 +116,80 @@ class SteeringHero }; std::shared_ptr command_component_; - class TopBoard final : private librmcs::client::CBoard { + class TopBoard final : private librmcs::agent::CBoard { public: friend class SteeringHero; - explicit TopBoard(SteeringHero& hero, SteeringHeroCommand& hero_command, int usb_pid = - -1) - : librmcs::client::CBoard(usb_pid) - , logger_(hero.get_logger()) - , tf_(hero.tf_) + explicit TopBoard( + SteeringHero& steering_hero, SteeringHeroCommand& steering_hero_command, + std::string_view board_serial = {}) + : librmcs::agent::CBoard(board_serial) + , logger_(steering_hero.get_logger()) + , tf_(steering_hero.tf_) , imu_(1000, 0.2, 0.0) - , gimbal_top_yaw_motor_( - hero, hero_command, "/gimbal/top_yaw", - device::LkMotor::Config{device::LkMotor::Type::MG5010E_I10} - .set_encoder_zero_point( - static_cast( - hero.get_parameter("top_yaw_motor_zero_point").as_int()))) - , gimbal_pitch_motor_( - hero, hero_command, "/gimbal/pitch", - device::LkMotor::Config{device::LkMotor::Type::MG5010E_I10} - .set_encoder_zero_point( - static_cast(hero.get_parameter("pitch_motor_zero_point").as_int()))) + , gimbal_top_yaw_motor_(steering_hero, steering_hero_command, "/gimbal/top_yaw") + , gimbal_pitch_motor_(steering_hero, steering_hero_command, "/gimbal/pitch") , gimbal_friction_wheels_( - {hero, hero_command, "/gimbal/first_left_friction", - device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(1.)}, - {hero, hero_command, "/gimbal/first_right_friction", - device::DjiMotor::Config{device::DjiMotor::Type::M3508} - .set_reduction_ratio(1.) - .set_reversed()}, - {hero, hero_command, "/gimbal/second_left_friction", - device::DjiMotor::Config{device::DjiMotor::Type::M3508} - .set_reduction_ratio(1.) - .set_reversed()}, - {hero, hero_command, "/gimbal/second_right_friction", - device::DjiMotor::Config{device::DjiMotor::Type::M3508} - .set_reduction_ratio(1.) - .set_reversed()}) - , gimbal_bullet_feeder_( - hero, hero_command, "/gimbal/bullet_feeder", - device::LkMotor::Config{device::LkMotor::Type::MG5010E_I10} - .set_reversed() - .enable_multi_turn_angle()) - , putter_motor_{hero, hero_command, "/gimbal/putter", - device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(1.).enable_multi_turn_angle()} - , transmit_buffer_(*this, 32) - , event_thread_([this]() { handle_events(); }) { + {steering_hero, steering_hero_command, "/gimbal/first_left_friction"}, + {steering_hero, steering_hero_command, "/gimbal/first_right_friction"}, + {steering_hero, steering_hero_command, "/gimbal/second_left_friction"}, + {steering_hero, steering_hero_command, "/gimbal/second_right_friction"}) + , gimbal_bullet_feeder_(steering_hero, steering_hero_command, "/gimbal/bullet_feeder") + , putter_motor_(steering_hero, steering_hero_command, "/gimbal/putter") { + + gimbal_top_yaw_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10}.set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("top_yaw_motor_zero_point").as_int()))); + gimbal_pitch_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10}.set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("pitch_motor_zero_point").as_int()))); + gimbal_friction_wheels_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); + gimbal_friction_wheels_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); + gimbal_friction_wheels_[2].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); + gimbal_friction_wheels_[3].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); + gimbal_bullet_feeder_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} + .set_reversed() + .enable_multi_turn_angle()); + putter_motor_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reduction_ratio(1.) + .enable_multi_turn_angle()); + + steering_hero.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); + steering_hero.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); + + steering_hero.register_output( + "/gimbal/photoelectric_sensor", photoelectric_sensor_status_, false); + steering_hero.register_output( + "/auto_aim/image_capturer/timestamp", camera_capturer_trigger_timestamp_, 0); + steering_hero.register_output( + "/auto_aim/image_capturer/trigger", camera_capturer_trigger_, 0); imu_.set_coordinate_mapping([](double x, double y, double z) { // Get the mapping with the following code. - // The rotation angle must be an exact multiple of 90 degrees, otherwise use a - // matrix. + // The rotation angle must be an exact multiple of 90 degrees, otherwise + // use a matrix. - // Eigen::AngleAxisd pitch_link_to_imu_link{ - // std::numbers::pi, Eigen::Vector3d::UnitZ()}; - // Eigen::Vector3d mapping = pitch_link_to_imu_link * Eigen::Vector3d{1, 2, 3}; + // Eigen::AngleAxisd pitch_link_to_bmi088_link{ + // std::numbers::pi / 2, Eigen::Vector3d::UnitZ()}; + // Eigen::Vector3d mapping = pitch_link_to_bmi088_link * Eigen::Vector3d{1, 2, 3}; // std::cout << mapping << std::endl; - - return std::make_tuple(-x, -y, z); + return std::make_tuple(x, y, z); }); - - hero.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); - hero.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); - - hero.register_output( - "/gimbal/photoelectric_sensor", photoelectric_sensor_status_, false); - hero.register_output( - "/auto_aim/image_capturer/timestamp", camera_capturer_trigger_timestamp_, 0); - hero.register_output("/auto_aim/image_capturer/trigger", camera_capturer_trigger_, 0); } - ~TopBoard() final { - stop_handling_events(); - event_thread_.join(); - } + TopBoard(const TopBoard&) = delete; + TopBoard& operator=(const TopBoard&) = delete; + TopBoard(TopBoard&&) = delete; + TopBoard& operator=(TopBoard&&) = delete; + ~TopBoard() final = default; void update() { imu_.update_status(); Eigen::Quaterniond gimbal_imu_pose{imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()}; @@ -218,160 +218,163 @@ class SteeringHero } void command_update() { - uint16_t batch_commands[4]{}; - - batch_commands[0] = gimbal_friction_wheels_[3].generate_command(); - batch_commands[1] = gimbal_friction_wheels_[1].generate_command(); - batch_commands[2] = gimbal_friction_wheels_[2].generate_command(); - batch_commands[3] = gimbal_friction_wheels_[0].generate_command(); - transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(batch_commands)); - - frequency_control_flag_ = !frequency_control_flag_; - if (frequency_control_flag_) { - batch_commands[0] = putter_motor_.generate_command(); - transmit_buffer_.add_can1_transmission( - 0x1ff, std::bit_cast(batch_commands)); - } + auto builder = start_transmit(); + + builder.can1_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + gimbal_friction_wheels_[3].generate_command(), + gimbal_friction_wheels_[1].generate_command(), + gimbal_friction_wheels_[2].generate_command(), + gimbal_friction_wheels_[0].generate_command(), + } + .as_bytes(), + }); - transmit_buffer_.add_can1_transmission( - 0x141, gimbal_bullet_feeder_.generate_torque_command( - gimbal_bullet_feeder_.control_torque())); + builder.can1_transmit({ + .can_id = 0x1FF, + .can_data = + device::CanPacket8{ + putter_motor_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); - transmit_buffer_.add_can2_transmission(0x141, gimbal_top_yaw_motor_.generate_command()); + builder.can1_transmit({ + .can_id = 0x141, + .can_data = gimbal_bullet_feeder_.generate_torque_command().as_bytes(), + }); - transmit_buffer_.add_can2_transmission(0x142, gimbal_pitch_motor_.generate_command()); + builder.can2_transmit({ + .can_id = 0x141, + .can_data = gimbal_top_yaw_motor_.generate_torque_command().as_bytes(), + }); - transmit_buffer_.trigger_transmission(); + builder.can2_transmit({ + .can_id = 0x142, + .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), + }); } private: - void can1_receive_callback( - uint32_t can_id, uint64_t can_data, bool is_extended_can_id, - bool is_remote_transmission, uint8_t can_data_length) override { - if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + void can1_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] return; - + auto can_id = data.can_id; if (can_id == 0x204) { - gimbal_friction_wheels_[0].store_status(can_data); + gimbal_friction_wheels_[0].store_status(data.can_data); } else if (can_id == 0x202) { - gimbal_friction_wheels_[1].store_status(can_data); + gimbal_friction_wheels_[1].store_status(data.can_data); } else if (can_id == 0x203) { - gimbal_friction_wheels_[2].store_status(can_data); + gimbal_friction_wheels_[2].store_status(data.can_data); } else if (can_id == 0x201) { - gimbal_friction_wheels_[3].store_status(can_data); + gimbal_friction_wheels_[3].store_status(data.can_data); } else if (can_id == 0x205) { - putter_motor_.store_status(can_data); + putter_motor_.store_status(data.can_data); } else if (can_id == 0x141) { - gimbal_bullet_feeder_.store_status(can_data); + gimbal_bullet_feeder_.store_status(data.can_data); } } - void can2_receive_callback( - uint32_t can_id, uint64_t can_data, bool is_extended_can_id, - bool is_remote_transmission, uint8_t can_data_length) override { - if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + void can2_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] return; + auto can_id = data.can_id; if (can_id == 0x141) { - gimbal_top_yaw_motor_.store_status(can_data); + gimbal_top_yaw_motor_.store_status(data.can_data); } else if (can_id == 0x142) { - gimbal_pitch_motor_.store_status(can_data); + gimbal_pitch_motor_.store_status(data.can_data); } } - void accelerometer_receive_callback(int16_t x, int16_t y, int16_t z) override { - imu_.store_accelerometer_status(x, y, z); + void accelerometer_receive_callback( + const librmcs::data::AccelerometerDataView& data) override { + imu_.store_accelerometer_status(data.x, data.y, data.z); } - void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { - imu_.store_gyroscope_status(x, y, z); + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + imu_.store_gyroscope_status(data.x, data.y, data.z); } void putter_receive_callback(bool status) { *photoelectric_sensor_status_ = status; } rclcpp::Logger logger_; OutputInterface& tf_; - OutputInterface photoelectric_sensor_status_; - OutputInterface camera_capturer_trigger_; - OutputInterface camera_capturer_trigger_timestamp_; + std::time_t last_camera_capturer_trigger_timestamp_{0}; device::Bmi088 imu_; - - OutputInterface gimbal_yaw_velocity_imu_; - OutputInterface gimbal_pitch_velocity_imu_; - device::LkMotor gimbal_top_yaw_motor_; device::LkMotor gimbal_pitch_motor_; - device::DjiMotor gimbal_friction_wheels_[4]; device::LkMotor gimbal_bullet_feeder_; - - bool frequency_control_flag_{false}; device::DjiMotor putter_motor_; - librmcs::client::CBoard::TransmitBuffer transmit_buffer_; - std::thread event_thread_; + OutputInterface gimbal_yaw_velocity_imu_; + OutputInterface gimbal_pitch_velocity_imu_; + OutputInterface photoelectric_sensor_status_; + OutputInterface camera_capturer_trigger_; + OutputInterface camera_capturer_trigger_timestamp_; }; - class BottomBoard_one final : private librmcs::client::CBoard { + class BottomBoard_one final : private librmcs::agent::CBoard { public: friend class SteeringHero; explicit BottomBoard_one( - SteeringHero& hero, SteeringHeroCommand& hero_command, int usb_pid = -1) - : librmcs::client::CBoard(usb_pid) - , logger_(hero.get_logger()) + SteeringHero& steering_hero, SteeringHeroCommand& steering_hero_command, + std::string_view board_serial = {}) + : librmcs::agent::CBoard(board_serial) + , logger_(steering_hero.get_logger()) , imu_(1000, 0.2, 0.0) , chassis_front_climber_motor_( - {hero, hero_command, "/chassis/climber/left_front_motor", - device::DjiMotor::Config{device::DjiMotor::Type::M3508} - .set_reversed() - .set_reduction_ratio(19.)}, - {hero, hero_command, "/chassis/climber/right_front_motor", - device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio( - 19.)}) + {steering_hero, steering_hero_command, "/chassis/climber/left_front_motor"}, + {steering_hero, steering_hero_command, "/chassis/climber/right_front_motor"}) , chassis_back_climber_motor_( - {hero, hero_command, "/chassis/climber/left_back_motor", - device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio( - 19.)}, - {hero, hero_command, "/chassis/climber/right_back_motor", - device::DjiMotor::Config{device::DjiMotor::Type::M3508} - .set_reversed() - .set_reduction_ratio(19.)}) + {steering_hero, steering_hero_command, "/chassis/climber/left_back_motor"}, + {steering_hero, steering_hero_command, "/chassis/climber/right_back_motor"}) , chassis_steering_motors_( - {hero, hero_command, "/chassis/left_front_steering", - device::DjiMotor::Config{device::DjiMotor::Type::GM6020} - .set_encoder_zero_point( - static_cast(hero.get_parameter("left_front_zero_point").as_int())) - .set_reversed()}, - {hero, hero_command, "/chassis/right_front_steering", - device::DjiMotor::Config{device::DjiMotor::Type::GM6020} - .set_encoder_zero_point( - static_cast(hero.get_parameter("right_front_zero_point").as_int())) - .set_reversed()}) + {steering_hero, steering_hero_command, "/chassis/left_front_steering"}, + {steering_hero, steering_hero_command, "/chassis/right_front_steering"}) , chassis_wheel_motors_( - {hero, hero_command, "/chassis/left_front_wheel", - device::DjiMotor::Config{device::DjiMotor::Type::M3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)}, - {hero, hero_command, "/chassis/right_front_wheel", - device::DjiMotor::Config{device::DjiMotor::Type::M3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)}) - - , transmit_buffer_(*this, 32) - , event_thread_([this]() { handle_events(); }) { - - // hero.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); - // hero.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); - - hero.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); + {steering_hero, steering_hero_command, "/chassis/left_front_wheel"}, + {steering_hero, steering_hero_command, "/chassis/right_front_wheel"}) { + // + + chassis_steering_motors_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("left_front_zero_point").as_int())) + .set_reversed()); + chassis_steering_motors_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("right_front_zero_point").as_int())) + .set_reversed()); + chassis_wheel_motors_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(2232. / 169.)); + chassis_wheel_motors_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(2232. / 169.)); + + steering_hero.register_output( + "/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); } + BottomBoard_one(const BottomBoard_one&) = delete; + BottomBoard_one& operator=(const BottomBoard_one&) = delete; + BottomBoard_one(BottomBoard_one&&) = delete; + BottomBoard_one& operator=(BottomBoard_one&&) = delete; - ~BottomBoard_one() final { - stop_handling_events(); - event_thread_.join(); - } + ~BottomBoard_one() final = default; void update() { imu_.update_status(); @@ -390,184 +393,176 @@ class SteeringHero } void command_update() { - uint16_t batch_commands[4]{}; - - batch_commands[0] = chassis_wheel_motors_[1].generate_command(); - batch_commands[1] = chassis_wheel_motors_[0].generate_command(); - batch_commands[2] = 0; - batch_commands[3] = 0; - transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(batch_commands)); - - batch_commands[0] = 0; - batch_commands[1] = chassis_steering_motors_[0].generate_command(); - batch_commands[2] = 0; - batch_commands[3] = chassis_steering_motors_[1].generate_command(); - transmit_buffer_.add_can1_transmission(0x1FE, std::bit_cast(batch_commands)); - - batch_commands[0] = chassis_back_climber_motor_[0].generate_command(); - batch_commands[1] = chassis_back_climber_motor_[1].generate_command(); - batch_commands[2] = chassis_front_climber_motor_[0].generate_command(); - batch_commands[3] = chassis_front_climber_motor_[1].generate_command(); - transmit_buffer_.add_can2_transmission(0x200, std::bit_cast(batch_commands)); - - transmit_buffer_.trigger_transmission(); + auto builder = start_transmit(); + + builder.can1_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[1].generate_command(), + chassis_wheel_motors_[0].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + + builder.can1_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + chassis_steering_motors_[0].generate_command(), + device::CanPacket8::PaddingQuarter{}, + chassis_steering_motors_[1].generate_command(), + } + .as_bytes(), + }); + + builder.can2_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_back_climber_motor_[0].generate_command(), + chassis_back_climber_motor_[1].generate_command(), + chassis_front_climber_motor_[0].generate_command(), + chassis_front_climber_motor_[1].generate_command(), + } + .as_bytes(), + }); } private: - void can1_receive_callback( - uint32_t can_id, uint64_t can_data, bool is_extended_can_id, - bool is_remote_transmission, uint8_t can_data_length) override { - if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + void can1_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] return; - + auto can_id = data.can_id; if (can_id == 0x201) { - chassis_wheel_motors_[1].store_status(can_data); + chassis_wheel_motors_[1].store_status(data.can_data); } else if (can_id == 0x202) { - chassis_wheel_motors_[0].store_status(can_data); + chassis_wheel_motors_[0].store_status(data.can_data); } if (can_id == 0x206) { - chassis_steering_motors_[0].store_status(can_data); + chassis_steering_motors_[0].store_status(data.can_data); } else if (can_id == 0x208) { - chassis_steering_motors_[1].store_status(can_data); + chassis_steering_motors_[1].store_status(data.can_data); } } - void can2_receive_callback( - uint32_t can_id, uint64_t can_data, bool is_extended_can_id, - bool is_remote_transmission, uint8_t can_data_length) override { - if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + void can2_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] return; + auto can_id = data.can_id; if (can_id == 0x203) { - chassis_front_climber_motor_[0].store_status(can_data); + chassis_front_climber_motor_[0].store_status(data.can_data); } else if (can_id == 0x204) { - chassis_front_climber_motor_[1].store_status(can_data); + chassis_front_climber_motor_[1].store_status(data.can_data); } else if (can_id == 0x201) { - chassis_back_climber_motor_[0].store_status(can_data); + chassis_back_climber_motor_[0].store_status(data.can_data); } else if (can_id == 0x202) { - chassis_back_climber_motor_[1].store_status(can_data); + chassis_back_climber_motor_[1].store_status(data.can_data); } } - rclcpp::Logger logger_; - // test - std::chrono::steady_clock::time_point last_time_; - - void calc_can_fps(double can_id) { - auto now = std::chrono::steady_clock::now(); - auto delta = - std::chrono::duration_cast(now - last_time_).count(); - RCLCPP_INFO(logger_, "can id :%lf,fps:%ld", can_id, 1000000 / delta); - last_time_ = now; + void accelerometer_receive_callback( + const librmcs::data::AccelerometerDataView& data) override { + imu_.store_accelerometer_status(data.x, data.y, data.z); } - void accelerometer_receive_callback(int16_t x, int16_t y, int16_t z) override { - imu_.store_accelerometer_status(x, y, z); + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + imu_.store_gyroscope_status(data.x, data.y, data.z); } - void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { - imu_.store_gyroscope_status(x, y, z); - } - device::Bmi088 imu_; + // // test + // std::chrono::steady_clock::time_point last_time_; + // void calc_can_fps(double can_id) { + // auto now = std::chrono::steady_clock::now(); + // auto delta = + // std::chrono::duration_cast(now - last_time_).count(); - OutputInterface chassis_yaw_velocity_imu_; + // RCLCPP_INFO(logger_, "can id :%lf,fps:%ld", can_id, 1000000 / delta); + // last_time_ = now; + // } + + rclcpp::Logger logger_; + device::Bmi088 imu_; device::DjiMotor chassis_front_climber_motor_[2]; device::DjiMotor chassis_back_climber_motor_[2]; - device::DjiMotor chassis_steering_motors_[2]; device::DjiMotor chassis_wheel_motors_[2]; - librmcs::utility::RingBuffer referee_ring_buffer_receive_{256}; + OutputInterface chassis_yaw_velocity_imu_; OutputInterface referee_serial_; - - librmcs::client::CBoard::TransmitBuffer transmit_buffer_; - std::thread event_thread_; - OutputInterface gimbal_yaw_velocity_imu_; OutputInterface gimbal_pitch_velocity_imu_; }; - class BottomBoard_two final : private librmcs::client::CBoard { + class BottomBoard_two final : private librmcs::agent::CBoard { public: friend class SteeringHero; explicit BottomBoard_two( - SteeringHero& hero, SteeringHeroCommand& hero_command, int usb_pid = -1) - : librmcs::client::CBoard(usb_pid) - , logger_(hero.get_logger()) + SteeringHero& steering_hero, SteeringHeroCommand& steering_hero_command, + std::string_view board_serial = {}) + : librmcs::agent::CBoard(board_serial) + , logger_(steering_hero.get_logger()) , imu_(1000, 0.2, 0.0) - , tf_(hero.tf_) - , dr16_(hero) - , supercap_(hero, hero_command) + , tf_(steering_hero.tf_) + , dr16_(steering_hero) + , supercap_(steering_hero, steering_hero_command) , chassis_steering_motors2_( - {hero, hero_command, "/chassis/left_back_steering", - device::DjiMotor::Config{device::DjiMotor::Type::GM6020} - .set_encoder_zero_point( - static_cast(hero.get_parameter("left_back_zero_point").as_int())) - .set_reversed()}, - {hero, hero_command, "/chassis/right_back_steering", - device::DjiMotor::Config{device::DjiMotor::Type::GM6020} - .set_encoder_zero_point( - static_cast(hero.get_parameter("right_back_zero_point").as_int())) - .set_reversed()}) + {steering_hero, steering_hero_command, "/chassis/left_back_steering"}, + {steering_hero, steering_hero_command, "/chassis/right_back_steering"}) , chassis_wheel_motors2_( - {hero, hero_command, "/chassis/left_back_wheel", - device::DjiMotor::Config{device::DjiMotor::Type::M3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)}, - {hero, hero_command, "/chassis/right_back_wheel", - device::DjiMotor::Config{device::DjiMotor::Type::M3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)}) - , gimbal_bottom_yaw_motor_( - hero, hero_command, "/gimbal/bottom_yaw", - device::LkMotor::Config{device::LkMotor::Type::MG6012E_I8} - .set_reversed() - .set_encoder_zero_point( - static_cast( - hero.get_parameter("bottom_yaw_motor_zero_point").as_int()))) - , transmit_buffer_(*this, 32) - , event_thread_([this]() { handle_events(); }) { - - hero.register_output("/referee/serial", referee_serial_); - referee_serial_->read = [this](std::byte* buffer, size_t size) { - return referee_ring_buffer_receive_.pop_front_multi( - [&buffer](std::byte byte) { *buffer++ = byte; }, size); - }; - referee_serial_->write = [this](const std::byte* buffer, size_t size) { - transmit_buffer_.add_uart1_transmission(buffer, size); - return size; - }; - - hero.register_output( + {steering_hero, steering_hero_command, "/chassis/left_back_wheel"}, + {steering_hero, steering_hero_command, "/chassis/right_back_wheel"}) + , gimbal_bottom_yaw_motor_(steering_hero, steering_hero_command, "/gimbal/bottom_yaw") { + chassis_steering_motors2_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("left_back_zero_point").as_int())) + .set_reversed()); + chassis_steering_motors2_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("right_back_zero_point").as_int())) + .set_reversed()); + chassis_wheel_motors2_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(2232. / 169.)); + chassis_wheel_motors2_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(2232. / 169.)); + gimbal_bottom_yaw_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG6012Ei8} + .set_reversed() + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("bottom_yaw_motor_zero_point").as_int()))); + steering_hero.register_output("/referee/serial", referee_serial_); + steering_hero.register_output( "/chassis/powermeter/control_enable", powermeter_control_enabled_, false); - hero.register_output( + steering_hero.register_output( "/chassis/powermeter/charge_power_limit", powermeter_charge_power_limit_, 0.); - hero.register_output("/chassis/climber/angle_imu", chassis_climber_angle_imu_); - - q0 = hero.get_parameter("q0").as_double(); - q1 = hero.get_parameter("q1").as_double(); - q2 = hero.get_parameter("q2").as_double(); - q3 = hero.get_parameter("q3").as_double(); } - ~BottomBoard_two() final { - stop_handling_events(); - event_thread_.join(); - } + BottomBoard_two(const TopBoard&) = delete; + BottomBoard_two& operator=(const TopBoard&) = delete; + BottomBoard_two(TopBoard&&) = delete; + BottomBoard_two& operator=(TopBoard&&) = delete; + + ~BottomBoard_two() final = default; void update() { imu_.update_status(); dr16_.update_status(); supercap_.update_status(); - Eigen::Quaterniond q_used{q0, q1, q2, q3}; - Eigen::Quaterniond q_now{imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()}; - Eigen::Quaterniond chassis_climber_angle_pose = q_used.conjugate() * q_now; - - *chassis_climber_angle_imu_ = - chassis_climber_angle_pose.toRotationMatrix().eulerAngles(0, 1, 2).y(); - for (auto& motor : chassis_wheel_motors2_) motor.update_status(); for (auto& motor : chassis_steering_motors2_) @@ -580,55 +575,69 @@ class SteeringHero } void command_update() { - uint16_t batch_commands[4]{}; - - batch_commands[0] = 0; - batch_commands[1] = 0; - batch_commands[2] = chassis_wheel_motors2_[1].generate_command(); - batch_commands[3] = chassis_wheel_motors2_[0].generate_command(); - transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(batch_commands)); - - batch_commands[0] = chassis_steering_motors2_[0].generate_command(); - batch_commands[1] = 0; - batch_commands[2] = chassis_steering_motors2_[1].generate_command(); - batch_commands[3] = supercap_.generate_command(); - transmit_buffer_.add_can1_transmission(0x1FE, std::bit_cast(batch_commands)); - - transmit_buffer_.add_can2_transmission( - 0x141, gimbal_bottom_yaw_motor_.generate_command()); + + auto builder = start_transmit(); + + builder.can1_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + chassis_wheel_motors2_[1].generate_command(), + chassis_wheel_motors2_[0].generate_command(), + } + .as_bytes(), + }); + + builder.can1_transmit({ + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + chassis_steering_motors2_[0].generate_command(), + device::CanPacket8::PaddingQuarter{}, + chassis_steering_motors2_[1].generate_command(), + supercap_.generate_command(), + } + .as_bytes(), + }); + + builder.can2_transmit({ + .can_id = 0x141, + .can_data = gimbal_bottom_yaw_motor_.generate_command().as_bytes(), + }); } private: - void can1_receive_callback( - uint32_t can_id, uint64_t can_data, bool is_extended_can_id, - bool is_remote_transmission, uint8_t can_data_length) override { - if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + void can1_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] return; + auto can_id = data.can_id; if (can_id == 0x203) { - chassis_wheel_motors2_[1].store_status(can_data); + chassis_wheel_motors2_[1].store_status(data.can_data); } else if (can_id == 0x204) { - chassis_wheel_motors2_[0].store_status(can_data); + chassis_wheel_motors2_[0].store_status(data.can_data); } if (can_id == 0x205) { - chassis_steering_motors2_[0].store_status(can_data); + chassis_steering_motors2_[0].store_status(data.can_data); } else if (can_id == 0x207) { - chassis_steering_motors2_[1].store_status(can_data); + chassis_steering_motors2_[1].store_status(data.can_data); } else if (can_id == 0x300) { - supercap_.store_status(can_data); + supercap_.store_status(data.can_data); } } - void can2_receive_callback( - uint32_t can_id, uint64_t can_data, bool is_extended_can_id, - bool is_remote_transmission, uint8_t can_data_length) override { - if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + void can2_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] return; + auto can_id = data.can_id; if (can_id == 0x141) { - gimbal_bottom_yaw_motor_.store_status(can_data); + gimbal_bottom_yaw_motor_.store_status(data.can_data); } } + rclcpp::Logger logger_; // test std::chrono::steady_clock::time_point last_time_; @@ -642,31 +651,31 @@ class SteeringHero last_time_ = now; } // test - void uart1_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { - referee_ring_buffer_receive_.emplace_back_multi( - [&uart_data](std::byte* storage) { *storage = *uart_data++; }, uart_data_length); + void uart1_receive_callback(const librmcs::data::UartDataView& data) override { + const auto* uart_data = data.uart_data.data(); + referee_ring_buffer_receive_.emplace_back_n( + [&uart_data](std::byte* storage) noexcept { *storage = *uart_data++; }, + data.uart_data.size()); } - void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { - dr16_.store_status(uart_data, uart_data_length); + void dbus_receive_callback(const librmcs::data::UartDataView& data) override { + dr16_.store_status(data.uart_data.data(), data.uart_data.size()); } - void accelerometer_receive_callback(int16_t x, int16_t y, int16_t z) override { - imu_.store_accelerometer_status(x, y, z); + void accelerometer_receive_callback( + const librmcs::data::AccelerometerDataView& data) override { + imu_.store_accelerometer_status(data.x, data.y, data.z); } - void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { - imu_.store_gyroscope_status(x, y, z); + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + imu_.store_gyroscope_status(data.x, data.y, data.z); } - double q0, q1, q2, q3; - device::Bmi088 imu_; OutputInterface& tf_; OutputInterface powermeter_control_enabled_; OutputInterface powermeter_charge_power_limit_; - OutputInterface chassis_climber_angle_imu_; device::Dr16 dr16_; device::Supercap supercap_; @@ -675,22 +684,18 @@ class SteeringHero device::DjiMotor chassis_wheel_motors2_[2]; device::LkMotor gimbal_bottom_yaw_motor_; - librmcs::utility::RingBuffer referee_ring_buffer_receive_{256}; + rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; OutputInterface referee_serial_; - - librmcs::client::CBoard::TransmitBuffer transmit_buffer_; - std::thread event_thread_; }; OutputInterface tf_; rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; - std::unique_ptr top_board_; - std::unique_ptr bottom_board_one_; - std::unique_ptr bottom_board_two_; - - rmcs_utility::TickTimer temperature_logging_timer_; + std::shared_ptr command_component; + std::shared_ptr top_board_; + std::shared_ptr bottom_board_one_; + std::shared_ptr bottom_board_two_; }; } // namespace rmcs_core::hardware From 5b219ae5b4210bb4d14554a12aba86e438c6d440 Mon Sep 17 00:00:00 2001 From: Fin_Resect Date: Mon, 16 Mar 2026 02:06:23 +0800 Subject: [PATCH 3/7] Fixed board match, test AC Co-authored-by: zhzy-star <2807406212@qq.com> Co-authored-by: floatpigeon --- .../rmcs_bringup/config/steering-hero.yaml | 4 +- .../rmcs_core/src/hardware/steering-hero.cpp | 63 +++++++++++-------- 2 files changed, 38 insertions(+), 29 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml index 49957f14..cdcc1064 100644 --- a/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml @@ -38,8 +38,8 @@ rmcs_executor: hero_hardware: ros__parameters: board_serial_top_board: "D4-2BCA-2E47-76CD-23BC-0B78-684B" - board_serial_bottom_board_one: "D4-3674-7174-8768-879E-E44A-3931" - board_serial_bottom_board_two: "D4-7973-19A9-EA40-4A3E-306F-10F9" + board_serial_bottom_board_one: "D4-7973-19A9-EA40-4A3E-306F-10F9" + board_serial_bottom_board_two: "D4-3674-7174-8768-879E-E44A-3931" bottom_yaw_motor_zero_point: 50656 pitch_motor_zero_point: 63917 top_yaw_motor_zero_point: 59095 diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp index ae4a6a50..4e74ddb0 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp @@ -4,7 +4,6 @@ #include #include #include -#include #include #include @@ -41,12 +40,13 @@ class SteeringHero , command_component_( create_partner_component( get_component_name() + "_command", *this)) { + register_output("/tf", tf_); - // gimbal_calibrate_subscription_ = create_subscription( - // "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { - // gimbal_calibrate_subscription_callback(std::move(msg)); - // }); + gimbal_calibrate_subscription_ = create_subscription( + "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { + gimbal_calibrate_subscription_callback(std::move(msg)); + }); top_board_ = std::make_unique( *this, *command_component_, get_parameter("board_serial_top_board").as_string()); @@ -289,7 +289,9 @@ class SteeringHero gimbal_top_yaw_motor_.store_status(data.can_data); } else if (can_id == 0x142) { gimbal_pitch_motor_.store_status(data.can_data); + RCLCPP_INFO(logger_, "0x142 receive"); } + RCLCPP_INFO(logger_, "canid: %d", can_id); } void accelerometer_receive_callback( @@ -408,7 +410,7 @@ class SteeringHero }); builder.can1_transmit({ - .can_id = 0x200, + .can_id = 0x1FE, .can_data = device::CanPacket8{ device::CanPacket8::PaddingQuarter{}, @@ -441,8 +443,7 @@ class SteeringHero chassis_wheel_motors_[1].store_status(data.can_data); } else if (can_id == 0x202) { chassis_wheel_motors_[0].store_status(data.can_data); - } - if (can_id == 0x206) { + } else if (can_id == 0x206) { chassis_steering_motors_[0].store_status(data.can_data); } else if (can_id == 0x208) { chassis_steering_motors_[1].store_status(data.can_data); @@ -474,16 +475,16 @@ class SteeringHero imu_.store_gyroscope_status(data.x, data.y, data.z); } - // // test - // std::chrono::steady_clock::time_point last_time_; - // void calc_can_fps(double can_id) { - // auto now = std::chrono::steady_clock::now(); - // auto delta = - // std::chrono::duration_cast(now - last_time_).count(); + // test + std::chrono::steady_clock::time_point last_time_; + void calc_can_fps(double can_id) { + auto now = std::chrono::steady_clock::now(); + auto delta = + std::chrono::duration_cast(now - last_time_).count(); - // RCLCPP_INFO(logger_, "can id :%lf,fps:%ld", can_id, 1000000 / delta); - // last_time_ = now; - // } + RCLCPP_INFO(logger_, "can id :%lf,fps:%ld", can_id, 1000000 / delta); + last_time_ = now; + } rclcpp::Logger logger_; @@ -494,7 +495,6 @@ class SteeringHero device::DjiMotor chassis_wheel_motors_[2]; OutputInterface chassis_yaw_velocity_imu_; - OutputInterface referee_serial_; OutputInterface gimbal_yaw_velocity_imu_; OutputInterface gimbal_pitch_velocity_imu_; }; @@ -545,16 +545,26 @@ class SteeringHero static_cast( steering_hero.get_parameter("bottom_yaw_motor_zero_point").as_int()))); steering_hero.register_output("/referee/serial", referee_serial_); + referee_serial_->read = [this](std::byte* buffer, size_t size) { + return referee_ring_buffer_receive_.pop_front_n( + [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); + }; + referee_serial_->write = [this](const std::byte* buffer, size_t size) { + start_transmit().uart1_transmit({ + .uart_data = std::span{buffer, size} + }); + return size; + }; steering_hero.register_output( "/chassis/powermeter/control_enable", powermeter_control_enabled_, false); steering_hero.register_output( "/chassis/powermeter/charge_power_limit", powermeter_charge_power_limit_, 0.); } - BottomBoard_two(const TopBoard&) = delete; - BottomBoard_two& operator=(const TopBoard&) = delete; - BottomBoard_two(TopBoard&&) = delete; - BottomBoard_two& operator=(TopBoard&&) = delete; + BottomBoard_two(const BottomBoard_two&) = delete; + BottomBoard_two& operator=(const BottomBoard_two&) = delete; + BottomBoard_two(BottomBoard_two&&) = delete; + BottomBoard_two& operator=(BottomBoard_two&&) = delete; ~BottomBoard_two() final = default; @@ -618,8 +628,7 @@ class SteeringHero chassis_wheel_motors2_[1].store_status(data.can_data); } else if (can_id == 0x204) { chassis_wheel_motors2_[0].store_status(data.can_data); - } - if (can_id == 0x205) { + } else if (can_id == 0x205) { chassis_steering_motors2_[0].store_status(data.can_data); } else if (can_id == 0x207) { chassis_steering_motors2_[1].store_status(data.can_data); @@ -639,9 +648,9 @@ class SteeringHero } rclcpp::Logger logger_; + // test std::chrono::steady_clock::time_point last_time_; - void calc_can_fps(double can_id) { auto now = std::chrono::steady_clock::now(); auto delta = @@ -651,6 +660,7 @@ class SteeringHero last_time_ = now; } // test + void uart1_receive_callback(const librmcs::data::UartDataView& data) override { const auto* uart_data = data.uart_data.data(); referee_ring_buffer_receive_.emplace_back_n( @@ -692,7 +702,6 @@ class SteeringHero rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; - std::shared_ptr command_component; std::shared_ptr top_board_; std::shared_ptr bottom_board_one_; std::shared_ptr bottom_board_two_; @@ -702,4 +711,4 @@ class SteeringHero #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringHero, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringHero, rmcs_executor::Component) From cef01f4111c2463f191ef80cd0583ebac6c249a4 Mon Sep 17 00:00:00 2001 From: floatpigeon Date: Mon, 16 Mar 2026 05:42:52 +0800 Subject: [PATCH 4/7] Fixed gimbal pitch control error and Update climber controller --- .../rmcs_bringup/config/steering-hero.yaml | 36 ++++-- rmcs_ws/src/rmcs_core/CMakeLists.txt | 4 +- .../chassis/chassis_climber_controller.cpp | 107 ++++++++++++------ .../controller/chassis/chassis_controller.cpp | 26 ++--- .../rmcs_core/src/hardware/steering-hero.cpp | 47 ++++---- 5 files changed, 131 insertions(+), 89 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml index cdcc1064..aa125924 100644 --- a/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml @@ -13,6 +13,7 @@ rmcs_executor: - rmcs_core::controller::gimbal::HeroGimbalController -> gimbal_controller - rmcs_core::controller::gimbal::DualYawController -> dual_yaw_controller - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller + - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller # - rmcs_core::controller::gimbal::PlayerViewer -> gimbal_player_viewer_controller # - rmcs_core::controller::pid::ErrorPidController -> viewer_angle_pid_controller @@ -28,7 +29,7 @@ rmcs_executor: - rmcs_core::controller::chassis::ChassisController -> chassis_controller - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - rmcs_core::controller::chassis::SteeringWheelController -> steering_wheel_controller - - rmcs_core::controller::chassis::ChassisClimberController -> climber_controller + # - rmcs_core::controller::chassis::ChassisClimberController -> climber_controller # - rmcs_auto_aim::AutoAimInitializer -> auto_aim_initializer # - rmcs_auto_aim::AutoAimController -> auto_aim_controller @@ -40,9 +41,9 @@ hero_hardware: board_serial_top_board: "D4-2BCA-2E47-76CD-23BC-0B78-684B" board_serial_bottom_board_one: "D4-7973-19A9-EA40-4A3E-306F-10F9" board_serial_bottom_board_two: "D4-3674-7174-8768-879E-E44A-3931" - bottom_yaw_motor_zero_point: 50656 + bottom_yaw_motor_zero_point: 52508 pitch_motor_zero_point: 63917 - top_yaw_motor_zero_point: 59095 + top_yaw_motor_zero_point: 58744 viewer_motor_zero_point: 3030 external_imu_port: /dev/ttyUSB0 left_front_zero_point: 4438 @@ -69,16 +70,29 @@ value_broadcaster: - /chassis/left_back_steering/control_torque - /chassis/right_back_steering/control_torque - /chassis/right_front_steering/control_torque - - /gimbal/bottom_yaw/angle + - /gimbal/pitch/control_torque + - /gimbal/pitch/velocity + - /gimbal/pitch/control_velocity + - /gimbal/pitch/control_angle_error + - /chassis/climber/left_back_motor/torque + - /chassis/climber/right_back_motor/torque + - /chassis/climber/left_back_motor/velocity + - /chassis/climber/right_back_motor/velocity + - /chassis/climber/right_back_motor/control_torque + - /chassis/climber/left_back_motor/control_torque climber_controller: ros__parameters: front_climber_velocity: 50.0 - back_climber_velocity: 10.0 - front_kp: 2.0 + back_climber_velocity: 20.0 + sync_coefficient: 0.3 + climb_timeout_limit: 800 + back_climber_burst_velocity: 20.0 + back_climber_burst_duration: 300 + front_kp: 1.0 front_ki: 0.0 front_kd: 0.5 - back_kp: 2.0 + back_kp: 1.0 back_ki: 0.0 back_kd: 0.0 @@ -121,18 +135,18 @@ pitch_angle_pid_controller: ros__parameters: measurement: /gimbal/pitch/control_angle_error control: /gimbal/pitch/control_velocity - kp: 28.00 + kp: 28.0 #28.00 ki: 0.0 - kd: 0.6 + kd: 0.6 #0.6 pitch_velocity_pid_controller: ros__parameters: measurement: /gimbal/pitch/velocity_imu setpoint: /gimbal/pitch/control_velocity control: /gimbal/pitch/control_torque - kp: 45.00 + kp: 45.0 #45.00 ki: 0.00 - kd: 1.00 + kd: 1.0 #1.00 gimbal_player_viewer_controller: ros__parameters: diff --git a/rmcs_ws/src/rmcs_core/CMakeLists.txt b/rmcs_ws/src/rmcs_core/CMakeLists.txt index ec80d3ed..dd3c8579 100644 --- a/rmcs_ws/src/rmcs_core/CMakeLists.txt +++ b/rmcs_ws/src/rmcs_core/CMakeLists.txt @@ -17,8 +17,8 @@ include(FetchContent) set(BUILD_STATIC_LIBRMCS ON CACHE BOOL "Build static librmcs SDK" FORCE) FetchContent_Declare( librmcs - URL https://github.com/Alliance-Algorithm/librmcs/releases/download/v3.1.0b0/librmcs-sdk-src-3.1.0-beta.0.zip - URL_HASH SHA256=7f3203e8db8cb7954ae0ac2ee226f93e32b367b3956b1f0bb5c7ae203c2ccbef + URL https://github.com/Alliance-Algorithm/librmcs/releases/download/v3.0.0/librmcs-sdk-src-3.0.0.zip + URL_HASH SHA256=b39f51c21baacdcbf3f0176119b8850137a108b88a67e12395d37d89e5ef53e8 DOWNLOAD_EXTRACT_TIMESTAMP TRUE ) FetchContent_MakeAvailable(librmcs) 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 index 3a524187..b24c2f96 100644 --- 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 @@ -1,18 +1,13 @@ #include "controller/pid/matrix_pid_calculator.hpp" #include "rmcs_msgs/switch.hpp" +#include #include #include -#include #include #include #include #include -/* - 所有的电机运动方向均以令底盘升高为正方向 - 暂时使用手动控制，遥控器键位不够用了，屏蔽自瞄和小陀螺模式 -*/ - namespace rmcs_core::controller::chassis { class ChassisClimberController : public rmcs_executor::Component @@ -32,6 +27,14 @@ class ChassisClimberController track_velocity_max_ = get_parameter("front_climber_velocity").as_double(); climber_back_control_velocity_abs_ = get_parameter("back_climber_velocity").as_double(); + sync_coefficient_ = get_parameter("sync_coefficient").as_double(); + climb_timeout_limit_ = get_parameter("climb_timeout_limit").as_int(); + + burst_velocity_abs_ = get_parameter("back_climber_burst_velocity").as_double(); + burst_duration_ = get_parameter("back_climber_burst_duration").as_int(); + + back_climber_block_count_ = 0; + back_climber_timer_ = 0; register_output( "/chassis/climber/left_front_motor/control_torque", climber_front_left_control_torque_, @@ -73,45 +76,63 @@ class ChassisClimberController if (last_switch_right_ == Switch::MIDDLE && switch_right == Switch::UP) { front_climber_enable_ = !front_climber_enable_; - } else if (last_switch_right_ == Switch::MIDDLE && switch_right == Switch::DOWN) { back_climber_dir_ = -1 * back_climber_dir_; - back_climber_block_count_ = 0; + reset_back_safety_counters(); } double track_control_velocity = front_climber_enable_ ? joystick_right_->x() * track_velocity_max_ : nan_; - double back_climber_control_velocity; - - if ((std::abs(*climber_back_left_torque_) > 0.1 - && std::abs(*climber_back_right_velocity_) < 0.1) - || (std::abs(*climber_back_left_velocity_) < 0.1 - && std::abs(*climber_back_right_torque_) > 0.1)) { - back_climber_block_count_++; - } dual_motor_sync_control( track_control_velocity, *climber_front_left_velocity_, *climber_front_right_velocity_, front_velocity_pid_calculator_, *climber_front_left_control_torque_, *climber_front_right_control_torque_); - if (back_climber_block_count_ >= 500) { - back_climber_control_velocity = 0; - // *climber_back_left_control_torque_ = 0; - // *climber_back_right_control_torque_ = 0; - // last_switch_left_ = switch_left; - // last_switch_right_ = switch_right; - // return; - } else { - back_climber_control_velocity = - climber_back_control_velocity_abs_ * back_climber_dir_; + double back_climber_control_velocity = 0.0; + + if (switch_left != Switch::DOWN) { + back_climber_timer_++; + bool force_zero_torque = false; + + if ((std::abs(*climber_back_left_torque_) > 0.1 + && std::abs(*climber_back_left_velocity_) < 0.1) + || (std::abs(*climber_back_right_torque_) > 0.1 + && std::abs(*climber_back_right_velocity_) < 0.1)) { + back_climber_block_count_++; + } + + if (back_climber_dir_ == -1) { + if (back_climber_timer_ < burst_duration_) { + back_climber_control_velocity = burst_velocity_abs_ * back_climber_dir_; + } else { + force_zero_torque = true; + } + } else { + back_climber_control_velocity = + climber_back_control_velocity_abs_ * back_climber_dir_; + } + + if (!force_zero_torque) { + if (back_climber_block_count_ >= 500) { + RCLCPP_WARN_THROTTLE(logger_, *get_clock(), 1000, "Back climber STALLED."); + back_climber_control_velocity = 0.0; + } else if (back_climber_timer_ >= climb_timeout_limit_) { + RCLCPP_WARN_THROTTLE(logger_, *get_clock(), 1000, "Back climber TIMEOUT."); + back_climber_control_velocity = 0.0; + } + + dual_motor_sync_control( + back_climber_control_velocity, *climber_back_left_velocity_, + *climber_back_right_velocity_, back_velocity_pid_calculator_, + *climber_back_left_control_torque_, *climber_back_right_control_torque_); + } else { + *climber_back_left_control_torque_ = 0.0; + *climber_back_right_control_torque_ = 0.0; + } } - - // dual_motor_sync_control( - // back_climber_control_velocity, *climber_back_left_velocity_, - // *climber_back_right_velocity_, back_velocity_pid_calculator_, - // *climber_back_left_control_torque_, *climber_back_right_control_torque_); } + last_switch_left_ = switch_left; last_switch_right_ = switch_right; } @@ -123,6 +144,12 @@ class ChassisClimberController *climber_back_left_control_torque_ = 0; *climber_back_right_control_torque_ = 0; front_climber_enable_ = false; + reset_back_safety_counters(); + } + + void reset_back_safety_counters() { + back_climber_block_count_ = 0; + back_climber_timer_ = 0; } void dual_motor_sync_control( @@ -130,13 +157,17 @@ class ChassisClimberController pid::MatrixPidCalculator<2>& pid_calculator, double& left_torque_out, double& right_torque_out) const { - Eigen::Vector2d setpoint_error{setpoint - left_velocity, setpoint - right_velocity}; + 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]; @@ -144,13 +175,19 @@ class ChassisClimberController } int back_climber_block_count_; + int back_climber_timer_; rclcpp::Logger logger_; static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + double sync_coefficient_; + int64_t climb_timeout_limit_; + + double burst_velocity_abs_; + int64_t burst_duration_; bool front_climber_enable_ = false; - double back_climber_dir_ = -1; + double back_climber_dir_ = 1; double track_velocity_max_; double climber_back_control_velocity_abs_; @@ -182,4 +219,4 @@ class ChassisClimberController #include PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::chassis::ChassisClimberController, rmcs_executor::Component) \ No newline at end of file + 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 da70a96f..a1fbb6fe 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 @@ -19,7 +19,7 @@ class ChassisController : Node( get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) - , following_velocity_controller_(7.0, 0.0, 0.0) { + , following_velocity_controller_(10.0, 0.0, 1.2) { following_velocity_controller_.output_max = angular_velocity_max; following_velocity_controller_.output_min = -angular_velocity_max; @@ -58,8 +58,8 @@ class ChassisController using namespace rmcs_msgs; auto switch_right = *switch_right_; - auto switch_left = *switch_left_; - auto keyboard = *keyboard_; + auto switch_left = *switch_left_; + auto keyboard = *keyboard_; do { if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) @@ -74,14 +74,14 @@ class ChassisController if (mode == rmcs_msgs::ChassisMode::SPIN) { mode = rmcs_msgs::ChassisMode::STEP_DOWN; } else { - mode = rmcs_msgs::ChassisMode::SPIN; + mode = rmcs_msgs::ChassisMode::SPIN; spinning_forward_ = !spinning_forward_; } } else if (!last_keyboard_.c && keyboard.c) { if (mode == rmcs_msgs::ChassisMode::SPIN) { mode = rmcs_msgs::ChassisMode::AUTO; } else { - mode = rmcs_msgs::ChassisMode::SPIN; + mode = rmcs_msgs::ChassisMode::SPIN; spinning_forward_ = !spinning_forward_; } } else if (!last_keyboard_.x && keyboard.x) { @@ -100,8 +100,8 @@ class ChassisController } while (false); last_switch_right_ = switch_right; - last_switch_left_ = switch_left; - last_keyboard_ = keyboard; + last_switch_left_ = switch_left; + last_keyboard_ = keyboard; } void reset_all_controls() { @@ -112,7 +112,7 @@ class ChassisController void update_velocity_control() { auto translational_velocity = update_translational_velocity_control(); - auto angular_velocity = update_angular_velocity_control(); + auto angular_velocity = update_angular_velocity_control(); chassis_control_velocity_->vector << translational_velocity, angular_velocity; } @@ -133,7 +133,7 @@ class ChassisController } double update_angular_velocity_control() { - double angular_velocity = 0.0; + double angular_velocity = 0.0; double chassis_control_angle = nan; switch (*mode_) { @@ -171,7 +171,7 @@ class ChassisController angular_velocity = following_velocity_controller_.update(err); } break; } - *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; + *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; *chassis_control_angle_ = chassis_control_angle; return angular_velocity; @@ -202,7 +202,7 @@ class ChassisController // Maximum control velocities static constexpr double translational_velocity_max = 10.0; - static constexpr double angular_velocity_max = 16.0; + static constexpr double angular_velocity_max = 16.0; InputInterface joystick_right_; InputInterface joystick_left_; @@ -214,8 +214,8 @@ class ChassisController InputInterface rotary_knob_; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; - rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; - rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_; OutputInterface chassis_angle_, chassis_control_angle_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp index 4e74ddb0..b7d6ada5 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp @@ -180,7 +180,7 @@ class SteeringHero // std::numbers::pi / 2, Eigen::Vector3d::UnitZ()}; // Eigen::Vector3d mapping = pitch_link_to_bmi088_link * Eigen::Vector3d{1, 2, 3}; // std::cout << mapping << std::endl; - return std::make_tuple(x, y, z); + return std::make_tuple(-x, -y, z); }); } @@ -251,12 +251,12 @@ class SteeringHero builder.can2_transmit({ .can_id = 0x141, - .can_data = gimbal_top_yaw_motor_.generate_torque_command().as_bytes(), + .can_data = gimbal_top_yaw_motor_.generate_command().as_bytes(), }); builder.can2_transmit({ .can_id = 0x142, - .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), + .can_data = gimbal_pitch_motor_.generate_command().as_bytes(), }); } @@ -289,9 +289,7 @@ class SteeringHero gimbal_top_yaw_motor_.store_status(data.can_data); } else if (can_id == 0x142) { gimbal_pitch_motor_.store_status(data.can_data); - RCLCPP_INFO(logger_, "0x142 receive"); } - RCLCPP_INFO(logger_, "canid: %d", can_id); } void accelerometer_receive_callback( @@ -368,6 +366,22 @@ class SteeringHero .set_reversed() .set_reduction_ratio(2232. / 169.)); + chassis_front_climber_motor_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(19.)); + chassis_front_climber_motor_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); + chassis_back_climber_motor_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .enable_multi_turn_angle() + .set_reduction_ratio(19.)); + chassis_back_climber_motor_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .enable_multi_turn_angle() + .set_reduction_ratio(19.)); + steering_hero.register_output( "/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); } @@ -475,17 +489,6 @@ class SteeringHero imu_.store_gyroscope_status(data.x, data.y, data.z); } - // test - std::chrono::steady_clock::time_point last_time_; - void calc_can_fps(double can_id) { - auto now = std::chrono::steady_clock::now(); - auto delta = - std::chrono::duration_cast(now - last_time_).count(); - - RCLCPP_INFO(logger_, "can id :%lf,fps:%ld", can_id, 1000000 / delta); - last_time_ = now; - } - rclcpp::Logger logger_; device::Bmi088 imu_; @@ -649,18 +652,6 @@ class SteeringHero rclcpp::Logger logger_; - // test - std::chrono::steady_clock::time_point last_time_; - void calc_can_fps(double can_id) { - auto now = std::chrono::steady_clock::now(); - auto delta = - std::chrono::duration_cast(now - last_time_).count(); - - RCLCPP_INFO(logger_, "can id :%lf,fps:%ld", can_id, 1000000 / delta); - last_time_ = now; - } - // test - void uart1_receive_callback(const librmcs::data::UartDataView& data) override { const auto* uart_data = data.uart_data.data(); referee_ring_buffer_receive_.emplace_back_n( From 901055a7c06a56a6fe24623377635a963039bc6b Mon Sep 17 00:00:00 2001 From: floatpigeon Date: Mon, 16 Mar 2026 06:50:23 +0800 Subject: [PATCH 5/7] Fixed friction direction error and Update putter controller without photogate --- .../rmcs_bringup/config/steering-hero.yaml | 3 +- .../chassis/chassis_climber_controller.cpp | 2 +- .../shooting/friction_wheel_controller.cpp | 4 +- .../controller/shooting/putter_controller.cpp | 103 ++++++++---------- .../controller/shooting/shooting_recorder.cpp | 15 ++- .../rmcs_core/src/hardware/steering-hero.cpp | 8 +- 6 files changed, 70 insertions(+), 65 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml index aa125924..5495b791 100644 --- a/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml @@ -25,6 +25,7 @@ rmcs_executor: - rmcs_core::controller::pid::PidController -> first_right_friction_velocity_pid_controller - rmcs_core::controller::pid::PidController -> second_left_friction_velocity_pid_controller - rmcs_core::controller::pid::PidController -> second_right_friction_velocity_pid_controller + # - rmcs_core::controller::shooting::ShootingRecorder -> shooting_recorder - rmcs_core::controller::chassis::ChassisController -> chassis_controller - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller @@ -171,7 +172,7 @@ friction_wheel_controller: friction_velocities: - 450.00 - 450.00 - - -535.00 + - 535.00 - 535.00 friction_soft_start_stop_time: 1.0 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 index b24c2f96..5d7384a3 100644 --- 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 @@ -155,7 +155,7 @@ class ChassisClimberController 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) const { + double& right_torque_out) { if (std::isnan(setpoint)) { left_torque_out = nan_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp index a83dc5f4..e38437a4 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp @@ -150,8 +150,8 @@ class FrictionWheelController bool detect_friction_faulty() { for (size_t i = 0; i < friction_count_; i++) { - if (*friction_velocities_[i] < *friction_control_velocities_[i] * 0.5) - return true; + if (abs(*friction_velocities_[i]) < abs(*friction_control_velocities_[i] * 0.5)) + return false; } return false; } diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp index 4efdbf52..fbf85761 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp @@ -60,17 +60,17 @@ class PutterController bullet_feeder_angle_pid_.ki = 0.0; bullet_feeder_angle_pid_.kd = 2.0; - puttter_return_velocity_pid_.kp = 0.0015; - puttter_return_velocity_pid_.ki = 0.00005; - puttter_return_velocity_pid_.kd = 0.; - puttter_return_velocity_pid_.integral_max = 0.; - puttter_return_velocity_pid_.integral_min = -0.03; + putter_return_velocity_pid_.kp = 0.0015; + putter_return_velocity_pid_.ki = 0.00005; + putter_return_velocity_pid_.kd = 0.; + putter_return_velocity_pid_.integral_max = 0.; + putter_return_velocity_pid_.integral_min = -0.03; putter_velocity_pid_.kp = 0.004; putter_velocity_pid_.ki = 0.0001; putter_velocity_pid_.kd = 0.001; - puttter_return_velocity_pid_.integral_max = 0.03; - puttter_return_velocity_pid_.integral_min = 0.; + putter_velocity_pid_.integral_max = 0.03; + putter_velocity_pid_.integral_min = 0.; putter_return_angle_pid.kp = 0.0001; // putter_return_angle_pid.ki = 0.000001; @@ -103,20 +103,17 @@ class PutterController if (bullet_feeder_cool_down_ > 0) { bullet_feeder_cool_down_--; - // 冷却期前期：反转供弹轮以解除卡弹 - if (bullet_feeder_cool_down_ > 500) - *bullet_feeder_control_torque_ = bullet_feeder_velocity_pid_.update( - -bullet_feeder_angle_per_bullet_ - *bullet_feeder_velocity_); - else { - // 冷却期后期：停止控制 - bullet_feeder_velocity_pid_.reset(); - *bullet_feeder_control_torque_ = 0.0; - } - - bullet_feeder_angle_pid_.reset(); + // 使用角度环反转到“后退一格”的位置以解除卡弹 + double velocity_err = + bullet_feeder_angle_pid_.update( + bullet_feeder_control_angle_ - *bullet_feeder_angle_) + - *bullet_feeder_velocity_; + *bullet_feeder_control_torque_ = bullet_feeder_velocity_pid_.update(velocity_err); - if (!bullet_feeder_cool_down_) - RCLCPP_INFO(get_logger(), "Jamming Solved!"); + if (!bullet_feeder_cool_down_) { + RCLCPP_INFO(get_logger(), "Jamming Solved, Retrying..."); + set_preloading(); + } } else { // 正常运行模式：摩擦轮就绪时才允许发射 if (*friction_ready_) { @@ -135,30 +132,22 @@ class PutterController } if (shoot_stage_ == ShootStage::PRELOADING) { - if (last_preload_flag_) { - // 角度环控制模式：光电门触发后精确定位 - const auto angle_err = - bullet_feeder_control_angle_ - *bullet_feeder_angle_; - if (angle_err < 0.1) { - set_preloaded(); - } - double velocity_err = - bullet_feeder_angle_pid_.update( - bullet_feeder_control_angle_ - *bullet_feeder_angle_) - - *bullet_feeder_velocity_; - *bullet_feeder_control_torque_ = - bullet_feeder_velocity_pid_.update(velocity_err); - } else { - // 速度环控制模式：等待光电门触发 - if (*photoelectric_sensor_status_) { - RCLCPP_INFO(get_logger(), "Photoelectric Sensor Triggered"); - last_preload_flag_ = true; - bullet_feeder_control_angle_ = - *bullet_feeder_angle_ + bullet_feeder_angle_per_bullet_ * 1.; - } else - *bullet_feeder_control_torque_ = bullet_feeder_velocity_pid_.update( - low_latency_velocity_ - *bullet_feeder_velocity_); + // 盲拨模式：始终执行角度环定位 + if (std::isnan(bullet_feeder_control_angle_)) { + bullet_feeder_control_angle_ = + *bullet_feeder_angle_ + bullet_feeder_angle_per_bullet_; + last_preload_flag_ = true; + } + + const auto angle_err = bullet_feeder_control_angle_ - *bullet_feeder_angle_; + if (angle_err < 0.1) { + set_preloaded(); } + double velocity_err = + bullet_feeder_angle_pid_.update(angle_err) - *bullet_feeder_velocity_; + *bullet_feeder_control_torque_ = + bullet_feeder_velocity_pid_.update(velocity_err); + update_jam_detection(); } else { // 其他状态：角度环保持角度不变防止弹链退弹 @@ -186,14 +175,14 @@ class PutterController *putter_control_torque_ = 0.; set_preloading(); shooted = false; + } else { + *putter_control_torque_ = + putter_return_velocity_pid_.update(-80. - *putter_velocity_); } - - *putter_control_torque_ = - puttter_return_velocity_pid_.update(-80. - *putter_velocity_); } else { // 子弹未发出：继续推进 *putter_control_torque_ = - puttter_return_velocity_pid_.update(60. - *putter_velocity_); + putter_return_velocity_pid_.update(60. - *putter_velocity_); } } } else { @@ -207,7 +196,7 @@ class PutterController } } else { // 推杆未初始化：执行复位操作 - *putter_control_torque_ = puttter_return_velocity_pid_.update(-80. - *putter_velocity_); + *putter_control_torque_ = putter_return_velocity_pid_.update(-80. - *putter_velocity_); update_putter_jam_detection(); } @@ -237,7 +226,7 @@ class PutterController putter_initialized = false; putter_startpoint = nan_; - puttter_return_velocity_pid_.reset(); + putter_return_velocity_pid_.reset(); putter_velocity_pid_.reset(); putter_return_angle_pid.reset(); *putter_control_torque_ = nan_; @@ -252,6 +241,11 @@ class PutterController void set_preloading() { RCLCPP_INFO(get_logger(), "PRELOADING"); shoot_stage_ = ShootStage::PRELOADING; + // 盲拨方案：直接增加目标角度 + if (!std::isnan(bullet_feeder_control_angle_)) { + bullet_feeder_control_angle_ += bullet_feeder_angle_per_bullet_; + } + last_preload_flag_ = true; } void set_preloaded() { @@ -278,10 +272,8 @@ class PutterController bullet_feeder_faulty_count_++; else { bullet_feeder_faulty_count_ = 0; - if (last_preload_flag_) - set_preloaded(); - else - enter_jam_protection(); + RCLCPP_WARN(get_logger(), "Jam Detected! Reversing 60 degrees..."); + enter_jam_protection(); } } @@ -310,7 +302,8 @@ class PutterController } void enter_jam_protection() { - bullet_feeder_control_angle_ = nan_; + // 设置目标角度为当前角度后退 60 度（一格） + bullet_feeder_control_angle_ = *bullet_feeder_angle_ - bullet_feeder_angle_per_bullet_; bullet_feeder_cool_down_ = 1000; bullet_feeder_angle_pid_.reset(); bullet_feeder_velocity_pid_.reset(); @@ -357,7 +350,7 @@ class PutterController bool putter_initialized = false; int putter_faulty_count_ = 0; double putter_startpoint = nan_; - pid::PidCalculator puttter_return_velocity_pid_; + pid::PidCalculator putter_return_velocity_pid_; InputInterface putter_velocity_; pid::PidCalculator putter_velocity_pid_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/shooting_recorder.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/shooting_recorder.cpp index c0ef5af2..4394bb8b 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/shooting_recorder.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/shooting_recorder.cpp @@ -96,7 +96,12 @@ class ShootingRecorder } v = *shoot_timestamp_; + static constexpr size_t max_velocities_size = 1000; + velocities.push_back(*initial_speed_); + if (velocities.size() > max_velocities_size) { + velocities.erase(velocities.begin()); + } analysis3(); @@ -160,9 +165,6 @@ class ShootingRecorder double v; double aim_velocity; - double start_time_ = 0.0; - double vv = 0.0; - private: static std::string timestamp_to_string(double timestamp) { auto time = static_cast(timestamp); @@ -189,7 +191,12 @@ class ShootingRecorder sort(velocities.begin(), velocities.end()); range_ = velocities.back() - velocities.front(); - range2_ = velocities[int(velocities.size() - 2)] - velocities[1]; + + if (velocities.size() >= 3) { + range2_ = velocities[int(velocities.size() - 2)] - velocities[1]; + } else { + range2_ = range_; + } velocity_max = velocities.back(); velocity_min = velocities.front(); diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp index b7d6ada5..64947ddc 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp @@ -147,11 +147,15 @@ class SteeringHero gimbal_friction_wheels_[0].configure( device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); gimbal_friction_wheels_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(1.)); gimbal_friction_wheels_[2].configure( device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); gimbal_friction_wheels_[3].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(1.)); gimbal_bullet_feeder_.configure( device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} .set_reversed() From 6bbeed296952cfb6d2fd9a0666e6513a09faa50f Mon Sep 17 00:00:00 2001 From: floatpigeon Date: Tue, 17 Mar 2026 09:47:33 +0800 Subject: [PATCH 6/7] Auto Climber Demo! --- .../rmcs_bringup/config/steering-hero.yaml | 2 +- .../chassis/chassis_climber_controller.cpp | 187 +++++++++++++++++- .../controller/chassis/chassis_controller.cpp | 12 ++ .../gimbal/hero_gimbal_controller.cpp | 5 +- .../rmcs_core/src/hardware/steering-hero.cpp | 4 + 5 files changed, 200 insertions(+), 10 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml index 5495b791..d971bdf4 100644 --- a/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml @@ -30,7 +30,7 @@ rmcs_executor: - rmcs_core::controller::chassis::ChassisController -> chassis_controller - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - rmcs_core::controller::chassis::SteeringWheelController -> steering_wheel_controller - # - rmcs_core::controller::chassis::ChassisClimberController -> climber_controller + - rmcs_core::controller::chassis::ChassisClimberController -> climber_controller # - rmcs_auto_aim::AutoAimInitializer -> auto_aim_initializer # - rmcs_auto_aim::AutoAimController -> auto_aim_controller 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 index 5d7384a3..e8faeda0 100644 --- 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 @@ -1,14 +1,20 @@ #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 #include namespace rmcs_core::controller::chassis { + +enum class AutoClimbState { IDLE, FRONT_UP, DASH, RETRACT }; + class ChassisClimberController : public rmcs_executor::Component , public rclcpp::Node { @@ -48,6 +54,7 @@ class ChassisClimberController 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_input("/chassis/climber/left_front_motor/velocity", climber_front_left_velocity_); register_input( @@ -61,6 +68,9 @@ class ChassisClimberController register_input("/remote/switch/right", switch_right_); register_input("/remote/switch/left", switch_left_); register_input("/remote/joystick/right", joystick_right_); + register_input("/remote/keyboard", keyboard_); + register_input("/remote/rotary_knob_switch", rotary_knob_switch_); + register_input("/chassis/pitch_imu", chassis_pitch_imu_); } void update() override { @@ -68,19 +78,166 @@ class ChassisClimberController auto switch_right = *switch_right_; auto switch_left = *switch_left_; + auto keyboard = *keyboard_; + auto rotary_knob_switch = *rotary_knob_switch_; + + bool rotary_knob_to_up = + (last_rotary_knob_switch_ != Switch::UP && rotary_knob_switch == Switch::UP); + bool rotary_knob_from_up = + (last_rotary_knob_switch_ == Switch::UP && rotary_knob_switch != Switch::UP); + RCLCPP_INFO(logger_, "%lf", *chassis_pitch_imu_); + // RCLCPP_INFO(logger_, "%hhu", static_cast(rotary_knob_switch)); + + if ((!last_keyboard_.g && keyboard.g) || rotary_knob_to_up) { + if (auto_climb_state_ == AutoClimbState::IDLE) { + auto_climb_state_ = AutoClimbState::FRONT_UP; + auto_climb_timer_ = 0; + climb_count_ = 0; + back_climber_block_count_ = 0; + auto_dash_started_ = false; + RCLCPP_INFO( + logger_, "Auto climb started by %s.", + rotary_knob_switch == Switch::UP ? "Rotary Knob" : "Keyboard G"); + } else { + auto_climb_state_ = AutoClimbState::IDLE; + reset_all_controls(); + RCLCPP_INFO(logger_, "Auto climb aborted (toggled again)."); + } + } else if (rotary_knob_from_up && auto_climb_state_ != AutoClimbState::IDLE) { + auto_climb_state_ = AutoClimbState::IDLE; + reset_all_controls(); + RCLCPP_INFO(logger_, "Auto climb aborted (rotary knob left UP)."); + } if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { reset_all_controls(); - } else if (switch_left != Switch::DOWN) { + auto_climb_state_ = AutoClimbState::IDLE; + } else if (auto_climb_state_ != AutoClimbState::IDLE) { + + auto_climb_timer_++; + double override_chassis_vx = nan_; + double track_control_velocity = 0.0; + double back_climber_control_velocity = 0.0; + + if (auto_climb_state_ == AutoClimbState::FRONT_UP) { + track_control_velocity = track_velocity_max_; // front climber active + override_chassis_vx = 1.0; // limit speed to leave distance + + double pitch = *chassis_pitch_imu_; + double target_pitch = + (climb_count_ == 0) ? 0.48 : 0.35; // ~25 deg for first, ~20 deg for second + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "FRONT_UP (step %d): pitch=%.3f, target>%.3f", + climb_count_ + 1, pitch, target_pitch); - if (last_switch_right_ == Switch::MIDDLE && switch_right == Switch::UP) { - front_climber_enable_ = !front_climber_enable_; - } else if (last_switch_right_ == Switch::MIDDLE && switch_right == Switch::DOWN) { - back_climber_dir_ = -1 * back_climber_dir_; - reset_back_safety_counters(); + if (pitch > target_pitch) { + auto_climb_state_ = AutoClimbState::DASH; + auto_climb_timer_ = 0; + back_climber_block_count_ = 0; + auto_dash_started_ = false; + RCLCPP_INFO(logger_, "Auto climb dashing phase (step %d).", climb_count_ + 1); + } + } else if (auto_climb_state_ == AutoClimbState::DASH) { + // lower rear pole to push up + back_climber_control_velocity = climber_back_control_velocity_abs_; + + if (!auto_dash_started_) { + if ((std::abs(*climber_back_left_torque_) > 0.1 + && std::abs(*climber_back_left_velocity_) < 0.1) + || (std::abs(*climber_back_right_torque_) > 0.1 + && std::abs(*climber_back_right_velocity_) < 0.1)) { + back_climber_block_count_++; + } else { + back_climber_block_count_ = 0; + } + + if (back_climber_block_count_ > 50) { // 50 ticks = 0.1s confirm + auto_dash_started_ = true; + } + } + + if (auto_dash_started_) { + track_control_velocity = track_velocity_max_; // Dash: front climber running + override_chassis_vx = 3.0; // dash speed + } else { + track_control_velocity = 0.0; // Stop front climber, wait for rear pole + override_chassis_vx = 0.0; // Stop chassis completely while pushing up + } + + double pitch = *chassis_pitch_imu_; + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "DASH: pitch=%.3f, timer=%d, dash_started=%d", + pitch, auto_climb_timer_, auto_dash_started_); + + bool is_leveled = + auto_dash_started_ && (std::abs(pitch) < 0.1) && (auto_climb_timer_ > 500); + bool timeout = (auto_climb_timer_ > 3000); + + if (is_leveled || timeout) { + auto_climb_state_ = AutoClimbState::RETRACT; + auto_climb_timer_ = 0; + climb_count_++; + if (timeout) { + RCLCPP_WARN(logger_, "Auto climb DASH timeout! Forcing retract."); + } else { + RCLCPP_INFO(logger_, "Auto climb DASH completed (Leveled)."); + } + } + } else if (auto_climb_state_ == AutoClimbState::RETRACT) { + if (rotary_knob_switch == Switch::UP) { + track_control_velocity = track_velocity_max_; // Keep front climbers running + override_chassis_vx = 3.0; // Keep moving forward (dash) for next step + } else { + track_control_velocity = 0.0; + override_chassis_vx = 3.0; // Keep dash moving forward even if finishing + } + back_climber_control_velocity = + -climber_back_control_velocity_abs_ * 2.0; // retract faster + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "RETRACT: timer=%d", auto_climb_timer_); + + if (auto_climb_timer_ > 1000) { // Retract for 1s + if (rotary_knob_switch == Switch::UP && climb_count_ < 2) { + auto_climb_state_ = AutoClimbState::FRONT_UP; + auto_climb_timer_ = 0; + back_climber_block_count_ = 0; + auto_dash_started_ = false; + RCLCPP_INFO( + logger_, "Auto climb RETRACT done, looping to FRONT_UP for step %d.", + climb_count_ + 1); + } else { + auto_climb_state_ = AutoClimbState::IDLE; + RCLCPP_INFO( + logger_, "Auto climb completed (finished %d steps).", climb_count_); + } + } } + *climbing_forward_velocity_ = override_chassis_vx; + + dual_motor_sync_control( + track_control_velocity, *climber_front_left_velocity_, + *climber_front_right_velocity_, front_velocity_pid_calculator_, + *climber_front_left_control_torque_, *climber_front_right_control_torque_); + + dual_motor_sync_control( + back_climber_control_velocity, *climber_back_left_velocity_, + *climber_back_right_velocity_, back_velocity_pid_calculator_, + *climber_back_left_control_torque_, *climber_back_right_control_torque_); + + } else if (switch_left != Switch::DOWN) { + *climbing_forward_velocity_ = nan_; + + // if (last_switch_right_ == Switch::MIDDLE && switch_right == Switch::UP) { + // front_climber_enable_ = !front_climber_enable_; + // } else if (last_switch_right_ == Switch::MIDDLE && switch_right == Switch::DOWN) { + // back_climber_dir_ = -1 * back_climber_dir_; + // reset_back_safety_counters(); + // } + double track_control_velocity = front_climber_enable_ ? joystick_right_->x() * track_velocity_max_ : nan_; @@ -135,6 +292,8 @@ class ChassisClimberController last_switch_left_ = switch_left; last_switch_right_ = switch_right; + last_keyboard_ = keyboard; + last_rotary_knob_switch_ = rotary_knob_switch; } private: @@ -144,6 +303,7 @@ class ChassisClimberController *climber_back_left_control_torque_ = 0; *climber_back_right_control_torque_ = 0; front_climber_enable_ = false; + *climbing_forward_velocity_ = nan_; reset_back_safety_counters(); } @@ -187,15 +347,22 @@ class ChassisClimberController int64_t burst_duration_; bool front_climber_enable_ = false; - double back_climber_dir_ = 1; + double back_climber_dir_ = -1; + + int climb_count_ = 0; double track_velocity_max_; double climber_back_control_velocity_abs_; + AutoClimbState auto_climb_state_ = AutoClimbState::IDLE; + int auto_climb_timer_ = 0; + bool auto_dash_started_ = false; + OutputInterface climber_front_left_control_torque_; OutputInterface climber_front_right_control_torque_; OutputInterface climber_back_left_control_torque_; OutputInterface climber_back_right_control_torque_; + OutputInterface climbing_forward_velocity_; InputInterface climber_front_left_velocity_; InputInterface climber_front_right_velocity_; @@ -208,9 +375,15 @@ class ChassisClimberController InputInterface switch_right_; InputInterface switch_left_; InputInterface joystick_right_; + InputInterface keyboard_; + InputInterface rotary_knob_switch_; + + InputInterface chassis_pitch_imu_; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + 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_; }; 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 a1fbb6fe..af18ba43 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 @@ -34,6 +34,7 @@ class ChassisController register_input("/gimbal/yaw/angle", gimbal_yaw_angle_, false); register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, false); + register_input("/chassis/climbing_forward_velocity", climbing_forward_velocity_, nan); register_output("/chassis/angle", chassis_angle_, nan); register_output("/chassis/control_angle", chassis_control_angle_, nan); @@ -118,6 +119,10 @@ class ChassisController } Eigen::Vector2d update_translational_velocity_control() { + if (!std::isnan(*climbing_forward_velocity_)) { + return Eigen::Vector2d{*climbing_forward_velocity_, 0.0}; + } + auto keyboard = *keyboard_; Eigen::Vector2d keyboard_move{keyboard.w - keyboard.s, keyboard.a - keyboard.d}; @@ -133,6 +138,12 @@ class ChassisController } double update_angular_velocity_control() { + if (!std::isnan(*climbing_forward_velocity_)) { + *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; + *chassis_control_angle_ = nan; + return 0.0; + } + double angular_velocity = 0.0; double chassis_control_angle = nan; @@ -218,6 +229,7 @@ class ChassisController rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_; + InputInterface climbing_forward_velocity_; OutputInterface chassis_angle_, chassis_control_angle_; OutputInterface mode_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp index f20bd98f..1c6dde8b 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp @@ -65,8 +65,9 @@ class HeroGimbalController else gimbal_mode_keyboard_ = GimbalMode::IMU; } - *gimbal_mode_ = - *switch_right_ == Switch::UP ? GimbalMode::ENCODER : gimbal_mode_keyboard_; + // *gimbal_mode_ = + // *switch_right_ == Switch::UP ? GimbalMode::ENCODER : gimbal_mode_keyboard_; + *gimbal_mode_ = gimbal_mode_keyboard_; if (*gimbal_mode_ == GimbalMode::IMU) { auto angle_error = update_imu_control(); diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp index 64947ddc..847698c2 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp @@ -388,6 +388,8 @@ class SteeringHero steering_hero.register_output( "/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); + steering_hero.register_output( + "/chassis/pitch_imu", chassis_pitch_imu_, 0.0); } BottomBoard_one(const BottomBoard_one&) = delete; BottomBoard_one& operator=(const BottomBoard_one&) = delete; @@ -400,6 +402,7 @@ class SteeringHero imu_.update_status(); *chassis_yaw_velocity_imu_ = imu_.gz(); + *chassis_pitch_imu_ = -std::asin(2.0 * (imu_.q0() * imu_.q2() - imu_.q3() * imu_.q1())); chassis_front_climber_motor_[0].update_status(); chassis_front_climber_motor_[1].update_status(); @@ -502,6 +505,7 @@ class SteeringHero device::DjiMotor chassis_wheel_motors_[2]; OutputInterface chassis_yaw_velocity_imu_; + OutputInterface chassis_pitch_imu_; OutputInterface gimbal_yaw_velocity_imu_; OutputInterface gimbal_pitch_velocity_imu_; }; From 040b4c4fba47db1677f29201408d93112d340e4d Mon Sep 17 00:00:00 2001 From: floatpigeon Date: Wed, 18 Mar 2026 03:48:29 +0800 Subject: [PATCH 7/7] AUTO CLIMB STABLE --- .../rmcs_bringup/config/steering-hero.yaml | 10 +- .../chassis/chassis_climber_controller.cpp | 480 +++++++++++------- 2 files changed, 290 insertions(+), 200 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml index d971bdf4..61946dff 100644 --- a/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml @@ -84,11 +84,15 @@ value_broadcaster: climber_controller: ros__parameters: - front_climber_velocity: 50.0 + front_climber_velocity: 60.0 back_climber_velocity: 20.0 + auto_climb_dash_velocity: 3.0 + auto_climb_support_retract_velocity: 30.0 sync_coefficient: 0.3 - climb_timeout_limit: 800 - back_climber_burst_velocity: 20.0 + climb_timeout_limit: 1000 + first_stair_approach_pitch: 0.485 + second_stair_approach_pitch: 0.35 + back_climber_burst_velocity: 15.0 back_climber_burst_duration: 300 front_kp: 1.0 front_ki: 0.0 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 index e8faeda0..76d0ffd0 100644 --- 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 @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include @@ -13,7 +14,7 @@ namespace rmcs_core::controller::chassis { -enum class AutoClimbState { IDLE, FRONT_UP, DASH, RETRACT }; +enum class AutoClimbState { IDLE, APPROACH, SUPPORT_DEPLOY, DASH, SUPPORT_RETRACT }; class ChassisClimberController : public rmcs_executor::Component @@ -33,8 +34,13 @@ class ChassisClimberController track_velocity_max_ = get_parameter("front_climber_velocity").as_double(); climber_back_control_velocity_abs_ = get_parameter("back_climber_velocity").as_double(); + auto_climb_dash_velocity_ = get_parameter("auto_climb_dash_velocity").as_double(); + auto_climb_support_retract_velocity_abs_ = + get_parameter("auto_climb_support_retract_velocity").as_double(); sync_coefficient_ = get_parameter("sync_coefficient").as_double(); climb_timeout_limit_ = get_parameter("climb_timeout_limit").as_int(); + first_stair_approach_pitch_ = get_parameter("first_stair_approach_pitch").as_double(); + second_stair_approach_pitch_ = get_parameter("second_stair_approach_pitch").as_double(); burst_velocity_abs_ = get_parameter("back_climber_burst_velocity").as_double(); burst_duration_ = get_parameter("back_climber_burst_duration").as_int(); @@ -81,222 +87,267 @@ class ChassisClimberController auto keyboard = *keyboard_; auto rotary_knob_switch = *rotary_knob_switch_; + // RCLCPP_INFO(logger_, "%lf", *chassis_pitch_imu_); + bool rotary_knob_to_up = (last_rotary_knob_switch_ != Switch::UP && rotary_knob_switch == Switch::UP); bool rotary_knob_from_up = (last_rotary_knob_switch_ == Switch::UP && rotary_knob_switch != Switch::UP); - RCLCPP_INFO(logger_, "%lf", *chassis_pitch_imu_); - // RCLCPP_INFO(logger_, "%hhu", static_cast(rotary_knob_switch)); - if ((!last_keyboard_.g && keyboard.g) || rotary_knob_to_up) { + 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_up, rotary_knob_from_up, + rotary_knob_switch); + + if (auto_climb_state_ != AutoClimbState::IDLE) { + apply_auto_climb_control(update_auto_climb_control()); + } else if (switch_left != Switch::DOWN) { + update_manual_control(); + } + } + + last_switch_left_ = switch_left; + last_switch_right_ = switch_right; + last_keyboard_ = keyboard; + last_rotary_knob_switch_ = rotary_knob_switch; + } + +private: + struct AutoClimbControl { + double front_track_velocity = 0.0; + double back_climber_velocity = 0.0; + double override_chassis_vx = nan_; + }; + + 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) { - auto_climb_state_ = AutoClimbState::FRONT_UP; - auto_climb_timer_ = 0; - climb_count_ = 0; - back_climber_block_count_ = 0; - auto_dash_started_ = false; - RCLCPP_INFO( - logger_, "Auto climb started by %s.", - rotary_knob_switch == Switch::UP ? "Rotary Knob" : "Keyboard G"); + start_auto_climb( + rotary_knob_switch == rmcs_msgs::Switch::UP ? "Rotary Knob" : "Keyboard G"); } else { - auto_climb_state_ = AutoClimbState::IDLE; - reset_all_controls(); - RCLCPP_INFO(logger_, "Auto climb aborted (toggled again)."); + abort_auto_climb("toggled again"); } - } else if (rotary_knob_from_up && auto_climb_state_ != AutoClimbState::IDLE) { - auto_climb_state_ = AutoClimbState::IDLE; - reset_all_controls(); - RCLCPP_INFO(logger_, "Auto climb aborted (rotary knob left UP)."); + } else if (abort_by_rotary && auto_climb_state_ != AutoClimbState::IDLE) { + abort_auto_climb("rotary knob left UP"); } + } - if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) - || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { - reset_all_controls(); - auto_climb_state_ = AutoClimbState::IDLE; - } else if (auto_climb_state_ != AutoClimbState::IDLE) { - - auto_climb_timer_++; - double override_chassis_vx = nan_; - double track_control_velocity = 0.0; - double back_climber_control_velocity = 0.0; - - if (auto_climb_state_ == AutoClimbState::FRONT_UP) { - track_control_velocity = track_velocity_max_; // front climber active - override_chassis_vx = 1.0; // limit speed to leave distance - - double pitch = *chassis_pitch_imu_; - double target_pitch = - (climb_count_ == 0) ? 0.48 : 0.35; // ~25 deg for first, ~20 deg for second - - RCLCPP_INFO_THROTTLE( - logger_, *get_clock(), 500, "FRONT_UP (step %d): pitch=%.3f, target>%.3f", - climb_count_ + 1, pitch, target_pitch); - - if (pitch > target_pitch) { - auto_climb_state_ = AutoClimbState::DASH; - auto_climb_timer_ = 0; - back_climber_block_count_ = 0; - auto_dash_started_ = false; - RCLCPP_INFO(logger_, "Auto climb dashing phase (step %d).", climb_count_ + 1); - } - } else if (auto_climb_state_ == AutoClimbState::DASH) { - // lower rear pole to push up - back_climber_control_velocity = climber_back_control_velocity_abs_; - - if (!auto_dash_started_) { - if ((std::abs(*climber_back_left_torque_) > 0.1 - && std::abs(*climber_back_left_velocity_) < 0.1) - || (std::abs(*climber_back_right_torque_) > 0.1 - && std::abs(*climber_back_right_velocity_) < 0.1)) { - back_climber_block_count_++; - } else { - back_climber_block_count_ = 0; - } - - if (back_climber_block_count_ > 50) { // 50 ticks = 0.1s confirm - auto_dash_started_ = true; - } - } - - if (auto_dash_started_) { - track_control_velocity = track_velocity_max_; // Dash: front climber running - override_chassis_vx = 3.0; // dash speed - } else { - track_control_velocity = 0.0; // Stop front climber, wait for rear pole - override_chassis_vx = 0.0; // Stop chassis completely while pushing up - } - - double pitch = *chassis_pitch_imu_; - RCLCPP_INFO_THROTTLE( - logger_, *get_clock(), 500, "DASH: pitch=%.3f, timer=%d, dash_started=%d", - pitch, auto_climb_timer_, auto_dash_started_); - - bool is_leveled = - auto_dash_started_ && (std::abs(pitch) < 0.1) && (auto_climb_timer_ > 500); - bool timeout = (auto_climb_timer_ > 3000); - - if (is_leveled || timeout) { - auto_climb_state_ = AutoClimbState::RETRACT; - auto_climb_timer_ = 0; - climb_count_++; - if (timeout) { - RCLCPP_WARN(logger_, "Auto climb DASH timeout! Forcing retract."); - } else { - RCLCPP_INFO(logger_, "Auto climb DASH completed (Leveled)."); - } - } - } else if (auto_climb_state_ == AutoClimbState::RETRACT) { - if (rotary_knob_switch == Switch::UP) { - track_control_velocity = track_velocity_max_; // Keep front climbers running - override_chassis_vx = 3.0; // Keep moving forward (dash) for next step - } else { - track_control_velocity = 0.0; - override_chassis_vx = 3.0; // Keep dash moving forward even if finishing - } - back_climber_control_velocity = - -climber_back_control_velocity_abs_ * 2.0; // retract faster - - RCLCPP_INFO_THROTTLE( - logger_, *get_clock(), 500, "RETRACT: timer=%d", auto_climb_timer_); - - if (auto_climb_timer_ > 1000) { // Retract for 1s - if (rotary_knob_switch == Switch::UP && climb_count_ < 2) { - auto_climb_state_ = AutoClimbState::FRONT_UP; - auto_climb_timer_ = 0; - back_climber_block_count_ = 0; - auto_dash_started_ = false; - RCLCPP_INFO( - logger_, "Auto climb RETRACT done, looping to FRONT_UP for step %d.", - climb_count_ + 1); - } else { - auto_climb_state_ = AutoClimbState::IDLE; - RCLCPP_INFO( - logger_, "Auto climb completed (finished %d steps).", climb_count_); - } - } + void start_auto_climb(const char* source) { + auto_climb_continue_ = true; + auto_climb_stair_index_ = 0; + auto_climb_support_block_count_ = 0; + enter_auto_climb_state(AutoClimbState::APPROACH); + + RCLCPP_INFO(logger_, "Auto climb started by %s.", source); + } + + void abort_auto_climb(const char* reason) { + reset_all_controls(); + RCLCPP_INFO(logger_, "Auto climb aborted (%s).", reason); + } + + AutoClimbControl update_auto_climb_control() { + auto_climb_timer_++; + + switch (auto_climb_state_) { + case AutoClimbState::IDLE: return {}; + 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_auto_climb_approach() { + AutoClimbControl control{ + .front_track_velocity = track_velocity_max_, + .back_climber_velocity = 0.0, + .override_chassis_vx = kAutoClimbApproachVelocity, + }; + + 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 = 0.2, + }; + + 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 = track_velocity_max_, + .back_climber_velocity = climber_back_control_velocity_abs_, + .override_chassis_vx = auto_climb_dash_velocity_, + }; + + double pitch = *chassis_pitch_imu_; + bool is_leveled = std::abs(pitch) < kAutoClimbLeveledPitchThreshold + && auto_climb_timer_ > kAutoClimbDashMinTicks; + bool timeout = auto_climb_timer_ > kAutoClimbDashTimeoutTicks; + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "DASH (step %d): pitch=%.3f, timer=%d", + auto_climb_stair_index_ + 1, pitch, 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); } + } - *climbing_forward_velocity_ = override_chassis_vx; + return control; + } - dual_motor_sync_control( - track_control_velocity, *climber_front_left_velocity_, - *climber_front_right_velocity_, front_velocity_pid_calculator_, - *climber_front_left_control_torque_, *climber_front_right_control_torque_); + AutoClimbControl update_auto_climb_support_retract() { + AutoClimbControl control{ + .front_track_velocity = track_velocity_max_, + .back_climber_velocity = -auto_climb_support_retract_velocity_abs_, + .override_chassis_vx = auto_climb_dash_velocity_, + }; - dual_motor_sync_control( - back_climber_control_velocity, *climber_back_left_velocity_, - *climber_back_right_velocity_, back_velocity_pid_calculator_, - *climber_back_left_control_torque_, *climber_back_right_control_torque_); + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "SUPPORT_RETRACT (step %d): timer=%d", + auto_climb_stair_index_ + 1, auto_climb_timer_); - } else if (switch_left != Switch::DOWN) { - *climbing_forward_velocity_ = nan_; + if (auto_climb_timer_ > kAutoClimbSupportRetractTicks) { + bool has_next_stair = + auto_climb_continue_ && (auto_climb_stair_index_ + 1 < kAutoClimbMaxStairs); - // if (last_switch_right_ == Switch::MIDDLE && switch_right == Switch::UP) { - // front_climber_enable_ = !front_climber_enable_; - // } else if (last_switch_right_ == Switch::MIDDLE && switch_right == Switch::DOWN) { - // back_climber_dir_ = -1 * back_climber_dir_; - // reset_back_safety_counters(); - // } + 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(); + RCLCPP_INFO(logger_, "Auto climb completed (finished %d steps).", finished_steps); + } + } - double track_control_velocity = - front_climber_enable_ ? joystick_right_->x() * track_velocity_max_ : nan_; + return control; + } - dual_motor_sync_control( - track_control_velocity, *climber_front_left_velocity_, - *climber_front_right_velocity_, front_velocity_pid_calculator_, - *climber_front_left_control_torque_, *climber_front_right_control_torque_); - - double back_climber_control_velocity = 0.0; - - if (switch_left != Switch::DOWN) { - back_climber_timer_++; - bool force_zero_torque = false; - - if ((std::abs(*climber_back_left_torque_) > 0.1 - && std::abs(*climber_back_left_velocity_) < 0.1) - || (std::abs(*climber_back_right_torque_) > 0.1 - && std::abs(*climber_back_right_velocity_) < 0.1)) { - back_climber_block_count_++; - } - - if (back_climber_dir_ == -1) { - if (back_climber_timer_ < burst_duration_) { - back_climber_control_velocity = burst_velocity_abs_ * back_climber_dir_; - } else { - force_zero_torque = true; - } - } else { - back_climber_control_velocity = - climber_back_control_velocity_abs_ * back_climber_dir_; - } - - if (!force_zero_torque) { - if (back_climber_block_count_ >= 500) { - RCLCPP_WARN_THROTTLE(logger_, *get_clock(), 1000, "Back climber STALLED."); - back_climber_control_velocity = 0.0; - } else if (back_climber_timer_ >= climb_timeout_limit_) { - RCLCPP_WARN_THROTTLE(logger_, *get_clock(), 1000, "Back climber TIMEOUT."); - back_climber_control_velocity = 0.0; - } - - dual_motor_sync_control( - back_climber_control_velocity, *climber_back_left_velocity_, - *climber_back_right_velocity_, back_velocity_pid_calculator_, - *climber_back_left_control_torque_, *climber_back_right_control_torque_); - } else { - *climber_back_left_control_torque_ = 0.0; - *climber_back_right_control_torque_ = 0.0; - } + void apply_auto_climb_control(const AutoClimbControl& control) { + *climbing_forward_velocity_ = control.override_chassis_vx; + + dual_motor_sync_control( + control.front_track_velocity, *climber_front_left_velocity_, + *climber_front_right_velocity_, front_velocity_pid_calculator_, + *climber_front_left_control_torque_, *climber_front_right_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_); + } + + void update_manual_control() { + *climbing_forward_velocity_ = nan_; + + // if (last_switch_right_ == Switch::MIDDLE && switch_right == Switch::UP) { + // front_climber_enable_ = !front_climber_enable_; + // } else if (last_switch_right_ == Switch::MIDDLE && switch_right == Switch::DOWN) { + // back_climber_dir_ = -1 * back_climber_dir_; + // reset_back_safety_counters(); + // } + + double track_control_velocity = + front_climber_enable_ ? joystick_right_->x() * track_velocity_max_ : nan_; + + dual_motor_sync_control( + track_control_velocity, *climber_front_left_velocity_, *climber_front_right_velocity_, + front_velocity_pid_calculator_, *climber_front_left_control_torque_, + *climber_front_right_control_torque_); + + double back_climber_control_velocity = 0.0; + back_climber_timer_++; + bool force_zero_torque = false; + + if (is_back_climber_blocked()) { + back_climber_block_count_++; + } + + if (back_climber_dir_ == -1) { + if (back_climber_timer_ < burst_duration_) { + back_climber_control_velocity = burst_velocity_abs_ * back_climber_dir_; + } else { + force_zero_torque = true; } + } else { + back_climber_control_velocity = climber_back_control_velocity_abs_ * back_climber_dir_; } - last_switch_left_ = switch_left; - last_switch_right_ = switch_right; - last_keyboard_ = keyboard; - last_rotary_knob_switch_ = rotary_knob_switch; + if (!force_zero_torque) { + if (back_climber_block_count_ >= 500) { + RCLCPP_WARN_THROTTLE(logger_, *get_clock(), 1000, "Back climber STALLED."); + back_climber_control_velocity = 0.0; + } else if (back_climber_timer_ >= climb_timeout_limit_) { + RCLCPP_WARN_THROTTLE(logger_, *get_clock(), 1000, "Back climber TIMEOUT."); + back_climber_control_velocity = 0.0; + } + + dual_motor_sync_control( + back_climber_control_velocity, *climber_back_left_velocity_, + *climber_back_right_velocity_, back_velocity_pid_calculator_, + *climber_back_left_control_torque_, *climber_back_right_control_torque_); + } else { + *climber_back_left_control_torque_ = 0.0; + *climber_back_right_control_torque_ = 0.0; + } } -private: void reset_all_controls() { *climber_front_left_control_torque_ = 0; *climber_front_right_control_torque_ = 0; @@ -304,14 +355,36 @@ class ChassisClimberController *climber_back_right_control_torque_ = 0; front_climber_enable_ = false; *climbing_forward_velocity_ = nan_; + stop_auto_climb(); reset_back_safety_counters(); } + void stop_auto_climb() { + auto_climb_state_ = AutoClimbState::IDLE; + auto_climb_timer_ = 0; + auto_climb_stair_index_ = 0; + auto_climb_continue_ = false; + auto_climb_support_block_count_ = 0; + } + + void enter_auto_climb_state(AutoClimbState state) { + auto_climb_state_ = state; + auto_climb_timer_ = 0; + auto_climb_support_block_count_ = 0; + } + void reset_back_safety_counters() { back_climber_block_count_ = 0; back_climber_timer_ = 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); + } + void dual_motor_sync_control( double setpoint, double left_velocity, double right_velocity, pid::MatrixPidCalculator<2>& pid_calculator, double& left_torque_out, @@ -339,9 +412,20 @@ class ChassisClimberController rclcpp::Logger logger_; static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + static constexpr double kAutoClimbApproachVelocity = 1.0; + static constexpr double kAutoClimbLeveledPitchThreshold = 0.1; + static constexpr double kBackClimberBlockedTorqueThreshold = 0.1; + static constexpr double kBackClimberBlockedVelocityThreshold = 0.1; + static constexpr int kAutoClimbSupportConfirmTicks = 50; + static constexpr int kAutoClimbDashMinTicks = 500; + static constexpr int kAutoClimbDashTimeoutTicks = 3000; + static constexpr int kAutoClimbSupportRetractTicks = 1000; + static constexpr int kAutoClimbMaxStairs = 2; double sync_coefficient_; int64_t climb_timeout_limit_; + double first_stair_approach_pitch_; + double second_stair_approach_pitch_; double burst_velocity_abs_; int64_t burst_duration_; @@ -349,14 +433,16 @@ class ChassisClimberController bool front_climber_enable_ = false; double back_climber_dir_ = -1; - int climb_count_ = 0; - double track_velocity_max_; double climber_back_control_velocity_abs_; + double auto_climb_dash_velocity_; + double auto_climb_support_retract_velocity_abs_; AutoClimbState auto_climb_state_ = AutoClimbState::IDLE; int auto_climb_timer_ = 0; - bool auto_dash_started_ = false; + int auto_climb_stair_index_ = 0; + int auto_climb_support_block_count_ = 0; + bool auto_climb_continue_ = false; OutputInterface climber_front_left_control_torque_; OutputInterface climber_front_right_control_torque_;