From 7e16876f0187addc2762d4352da73668d0fdaa24 Mon Sep 17 00:00:00 2001 From: Fin_Resect Date: Sun, 15 Mar 2026 21:57:40 +0800 Subject: [PATCH 01/32] 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 02/32] 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 03/32] 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 04/32] 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 05/32] 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 06/32] 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 07/32] 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_; From 7bf9b011ffaab63f285c202cf2c132a16a95491c Mon Sep 17 00:00:00 2001 From: zhzy-star <2807406212@qq.com> Date: Wed, 18 Mar 2026 03:01:09 +0800 Subject: [PATCH 08/32] Fixed putter controller : add photoelectric_sensor(demo) --- ...irmware-c_board-3.0.1-0.dev.4.gbaf538b.dfu | Bin 0 -> 95612 bytes .../rmcs_bringup/config/steering-hero.yaml | 1 + rmcs_ws/src/rmcs_core/CMakeLists.txt | 3 +- .../controller/shooting/putter_controller.cpp | 195 +++++++++++++----- .../controller/shooting/shooting_recorder.cpp | 54 ++--- .../rmcs_core/src/hardware/steering-hero.cpp | 23 ++- 6 files changed, 192 insertions(+), 84 deletions(-) create mode 100644 librmcs-firmware-c_board-3.0.1-0.dev.4.gbaf538b.dfu diff --git a/librmcs-firmware-c_board-3.0.1-0.dev.4.gbaf538b.dfu b/librmcs-firmware-c_board-3.0.1-0.dev.4.gbaf538b.dfu new file mode 100644 index 0000000000000000000000000000000000000000..c3e952e1b6d1f9d141b6f936f2de67159b810bd5 GIT binary patch literal 95612 zcmeGEdstl6)d!C6ea;Lszyt;+CNQ8GW}*U<7?P+>BsRkg9Il$1ZDV5V0ErF>sl>Dv zYg-QB1x$ADyjb0aL>cN4A%iizV_=v+L2KDG5Bx7 zWmW%Y?(I<74@3UIzo`4!W$B}rh5wU&VlWO??#hs#0{_WS_&=%tO=WIC{ftl>w|qle zw_ldF72zFl_rpB|w-4@{+MG>e=$#F3IFfgSr0Baza$@%yd1fhG8j%gJy*#Qn>^Q6W zQuIl!I4w@X*A6Z`9aa!5vrFMft(m07)_;zxP$lskN$%=1{JY+km z$Ttt_xLy*>d1KN~t$Kkz|FWX75ZO6Rtt*bqmOG{UdmnjY*iZA#Wqv>DK8pTmGT+bV zneL!=B9&!x^2D)VI$zESCYh30Urc$vE~?j5&SH9>seH3ehnP#&@YG@|E6aX!OeDad z1+~k0;W%RJb)>f}+gu(iQ%>WHFE1LaqVXun%8;&}@^;{!mwBGE|7k43aHYiCBPD8| z@8eP&=Vq4cyVnH7;^Hj*Nopa-xRklw4qGtg{E06|LNF4{4)RS>Y{$`YQZZjTI%Du`oa6u`d z5t;j(SD)CYQ18)EZM0U}rx0&p=A+K>eg^kGh4UKZ=qNIBYt)+Ij}oI4L)vVK@PXM{ zBbU`8$I7zPFqayEH0(!P$CO0NF=gRez2F`r28$4OfGGO+sg1}57xc}=Zfe;o^6hEI zDDV_neo9CC7{-iURqYW%ZaJ;j2vojeYGVg&QfGq?muB%Xs0-qQYd0QSS8fr^D494# zlF$48fw7f!X@#nGI;tlIfyj$47&~leokQzfd)cwV@??}gJBiX#QB9#ogK|W!xM0O> za+pC!$Gsh2_VYL3{S!Y)9r6Y14NeaFXn?DEWI|kY;oF3Ekqp zFZ=!&eV_L8fDiL}E(KTxX65!t(o7rJYU82}-45%T0J&EySOii}b|Ao`Pk=j+d;I zTvJ%CA0}J_uw@d6-7Xb*Xak!=+hk4$ZG6WMC}oP;$0b{NzfN}<29E%PzxT5i(mRSg ztWG$}{Zn$+yzDo#QQMyj&FlYOXhq5if9E(^^CZ@8RDaJ{6z>}TVEG!E7+RDFFLhUm zJz|TZ_p< z_oKhgCg;?^CpCH@$#Z-xisPpEm?$~zVqxu}n%d6%#OBTe&NZN9x&dP2Cdue#b8)5d zSaEH0=PM)5=rzMmI(~|!01B_Gmdz%kAgT^*6Ur3zd&eeC83ABOK z%UJD0Q?!{hrFgDSux08;IAWU8AwNp)o!VCW5-{2j&&CxuP=rm9CTy?d+Sz&vLn~#d z{pQq&|5;$;r4fzcSzu$&n5*&{;Z?s&9aSbqmC1c_>dSg;RDiv9du^AZ@rszQM$A{w zNP*@p^TN1iJ0EP1638MV|5AK5%PZH4hcG&;a4>6sr>4Jk#K2>GS(5|9HGK*h0yc=) z2~5)moe=_^;y=Ese=guUKK2-??@a5UBO+O+XwsLvE1i?RSefS>@}uIm+WJnDI;JfH zYlaK3PAnLeMjh3&BL;(Uij#{oO;g;M1vSu?uD(+gnmcLzw>ln=ixAc;n#eTwAJAf) z%#-C3xt4pvJF4O6)AgPI{r$%jeOVJ=1{Hjou4pzJB5H)A_X_jlVio%SBUts1Iu&2kD2@8TWX&|!w9Nhb5r(m+ z{cd2pWyC<-nbn=!E4K(c{M%Jr7Xa4<+!Lq19L=fDX1!608mZRp6o3aneI$g30g0wJ zcSenBqL3T4Q^0zBj%$dLPfc>ote55P?V#N&y0o=?twwNQ{Wf=soz<8r?}(Emm=mzG znw&3pa{WX1d&A?UUt+{|Vcto$2O6AHCqAjM%xd9BXpXDz{BFYUI$v?t3Evv{4OSI)#`zcH87vH@ z*Zf8y*b=dXe4KzRXR+BFca;FBcz#M7vtY7H(F&k?;ZOUqZ!e;ib040iu{`EVGs!nw zJbytfAFEY27L(b8U3=j|i~rTExBv2f5O>*E;PfS__x>N;<^3PrZTTARz8P>FEJS!*ipIVxYtna6n6K`?c2NLk z9R{xdwGoGkSwjf3DqVTxA?L#X7_)bO4YSV!tHD+ncY%3&gz**TIEKZma3V51KH;B_ z{mn2I+}~B;8XqH1ccRRN6@WUNFB0?~Cw5B{_KF!WFO`muYCvaR!>U>#(3^Z1|A%nt z=2&@#GbrIhPj`Nh87@7k(4jqWG&QmNNrgk08)CSs>Db10xnX}M8J$^quTYj53+|0m zXnK|NLTHEAPA@sqQnM2?w|XMGt-*hj(4e2`jA47%PzEeIfW-%aMTW!q!r8Cv-bfE= zxJf7zNcoySR|d~YMuRPo7^dwB;Npn>sx=F4)e0HH zd*Hd3<7X)x52syzvXGq+8=N$imlyK1KRt8M|0O3SgAZW&r2D}i5zF5pbJW1EgVW?E zlp8zZz!`jUl>Lt%CEamE<=ndgdAAx0o%h7Xm$7r5%imZ|R>XBPK7h5s<;RtKgHk~^ zi_2Y0G13`(b$hmtq~u78tkK{INz$U6DEWG2QU0PFVl|h|OzfDk=i74^<;1&ZCVVrz zkeb80lk*!uhmVea5e+PHsr@m+uY|)mg92HDor?!}Pjj3_+UyzY`0y-=ZOpzhpB!N2 zX;VTDU1}KR_(5Q+a(Y)5H@cAfq< z$Xhmp?^?*)Ge5u_GCZ*H>|uC9*c+7VEDVPXe+-XL0e>@gBG0MB46iv@aPXGXidc2< z*4lQg!@4yBv3w7wVPp0RLHqp7M9tIyCbt& z_2qpB9{pB?Cajs*|03xc8?#UOpH$+ERE{Rv5eLtY_)HCo#bO1gUJXAO)~I2UOypKE z;pdGCu{1bE`El`qyjTxE(!|b6^>=n}Ta+H8~5>i2zsDJ|T7d;jUrP4Z@X3*AUc z;T&`x{v+uGv4g9v82?_|9J%x22LDL;0Ag#~!Ww5L4o~O%?d7Kt&uL4>8uXGm@>3Tx zfU&t`W+H2v$hj&kFP42Pl$MUP)nCk!_g%cs-)tW2r1qGBudar0M#UayIEuHhc8Kiy zBF>+Z7hD;p%sc#Z;;;Hm6Kc%kPg4&XX}%#$=NkJyC&KCM2~6F_TTltnwOp#Tb^)#1Gv`Ucwg$j)7hS{ zlNah1WU`izqh;1^4rmT*x%DucJ(KL#J>kQAU1YNpftkCyGSa@+7xsJ(B$Tv^kDqDD zA@Z{qgD}lZJoW{v`_SQ^`hl?G^qF;vRVLk=ub}$exm5r0JgPUv23-%rENwRaP5v6x3wsXh z17kKD(VP6}B-;fXcENnsTN<@j1m?7*m*)sJSP95HS2<_T_Z`1~W%+T;UIK+iD>g|^ zqpo46Dbkf*=BfebQAbAjNHQ8ucW2!?kY65Y=er}z-zk4Lj#~1|HFmpiUB0<|I`vxV z#SzMbi<>020|KpUG&ecDo#7+z8temkpvAExd$=nkzJWBFO1ORNvE%f~+=1{>8qN(= zW%13ooLG%^>JsmqtR3@0O8aa7EPV-Sd~6St&89G#_{W#u`poI1$(+29JKMi3hs%`w ze*v^}O{|8|Dl_J2tc{^LvxkV+Qb94bbqtF*+0d zTVJ)Jsk;*rX`}NW{rN(44&$7sC-SDlaZWP6nOi>CnV%Jtx%&L3wz^9k$|;QH?w80L zTmE+W+4?N#nB0Z=Nq+=7wK;;;Zo(M>P2%d3dmni4DPW47g61-FIZ3i&MViZQN4RjT z3ZW=n|L7>92>RNWs~OZ9fjqV@>JmEp(!2s zfF!|q<;+C-1#oco(aUtMjE7?RRp54}ane?b#2O=*hAQ$Cz_;z;^-{vNyOPS#{!Ir>}Fjvt%IBT;t?MjvEF$KkKLV(f)gLgC|O4<|qE`7|Z|Yy8F!K zPFVx!pZ4E{6iN3xq*&FIU-|FOGnLcjrm{kej4Xc9SLdhq$H>u!TIu_FoAZt3{78Yi z>SKRi<@Cx;VyyW@Ck0*nY1{X5);rk>F=qa;ge+l{7-bnPm{)ZFuRsfIIOiXR9}H{M zu%*1!^O|}d{J4#!GhSe}4ATX(aDo43K>0o0qDxTTlxOr1>xXA~tG3<>nr*OXt&b^l zEM#%6j{^?~IX;2uu1I}NHvc@EjYpKF9#TgwHQ9eTOKc=nU>LWaP~^ov^+X+nhOA^Y9_iX6JPf%o*JBd+9%4J`KBy35gS%3m zq0pqsBK^Li88Ks4p2mI3MYNmNby{ih?uFFJYE`$-6+Te(mLh>aPLf_yqKt1SVc?6I z7WE}agMa9-zj8;%!3)IVuqWfrfu!07=jSD(y`RiHcj*pgR@2V(^iQFG*1JjVos}$_ zA<2*oAycFsW6&`@yhul(Nlg+oIkaJpJaU1{qs?NHdjA3#8Jv7RvG$y0jZES%PkL67oor4ZCkf4 z72|PBplsbo3L&QzJz6=m&Yd;j_R8;xYXaW21D+)0QrCccz>^{C(A!CcT(u_fpgRe> zxd-96!uXQfu#3IMq&~CCQX#z-Ki{O4q61`wsx-wcZcLbc>FTu8yX%H|l}@H%7&1BKt&(ypgPt^-=xq z7}*>B=2+$21KS<)(;^%Bf#P+!yPX73|GTjZUm5!wqsC~AeeGP_f*9AW3AC)!yZYT^ zQLS%v?&_RyPo=Xv@bk8B=TtiH>c5%&pJ4yw+%sa!_@J?6{10nc>`BCakJ!dC7CRu` zfHI=oC|3#hm&jFN=`;{Et< zgpk|T5VG_;FOC0g)kn%&W0EXpd;q`a44WNGz+<$??*L{aNdXsSj2ic!R?@temt2It zjccVdYXX+FAL2i4?R)rltoUE-|4PTCkwrT8z2^+OQ}IdAB{Uvw8MHn~~WaAF)@VMm|;3onY9jaA}}H zxPzq7qaV<(8IHOUw{{~@-<~7oT=h`ret{dQBzK_IKY?C8n03LYS;mb##F=EC@=`Ny zwoT=&>&lMtDW)1tcYV2Qm~-Kts(W!hKSC0XT?cVngp)(_o2Zl3!?H?78F@0x2n4TN-enP%-q5O#6U>0(P*nUq5wBia${?wORLK{f~JNvxTsU?tcZZv1IR#FQG`yB$W>UH{s z3w|~y7R<@T8J?7N0c!pg$T6G4(+U#kub{jCWaD1Y^UBv3-+H5R)mq2mMFKnd7AIVT z7`4$6Ln1d)keE5#-pjtCo7#HD!udb? z$)dB80dm}rF|J!@$f8dqJ@!JTDPecD^P|7erW4BbD}L>`(sf+9eq*mXHn!f__`V$3 zD|rd(eDrNxGfd-OEInM>r^F@K%EoX=nind;+iOQ$HtS^gx&IogzJ?z+ZaJxYq+KV} z2w|f%ETXT~)9R)v*Gp!dyO)=fC`}>qn&uGKGBfc{@a}P8ul4l-4+FYa`$*CXVCu+p zpSt2=d18OT$MTqz^EUieKkhMC68nz_%kt4?S;OZ%34_d{p)`qqj!7#s6K^SXezQ!Y z=SfGD6-Mnoohor`mn-OFiqS|Ai!t)gWeCthFJo=YOuVT41z|IG=Y9#j#eM8}NB4N& z?ZOJR%*@2jY3)d63@hWqihgq}mXef;Kt&rXOD!#qq+vnC8xu*W)i&_Y>1?Q*HZ$>kB_%Xw zHgcCtir9kkf&h4zvr?ECwXuV1+_YzkOpB}5l1!O{*eieh^kA-~@oAZscDSGQsG z1x38{Vkz3SKYa!*ujt3_bjsp-r%cwLL))`^Car68cmhqy(+a&C zlW|{B-ASlFpp&Cr4I8@5XUQSH4q1_Wc zS3q?A_a&Xlpza@#saUC5d_a~J8Eqh@h6@PW^@Cx#8^fuU>E5|Xek=nrJCiY3dGawC ziny%ooWV`ux*VEhS*B|0QQDzc)4IpjJl;Ka&*Np;vvt6jkT2={iY@{LSmH)DdRt(+ z*fmT|Z0rPks9Z7UZ`ysX100a`Eyb9EGV$|odHNr*4-m_nV>)MCwMUGr*!Yv`e`p>+ z+|6N@-0}lNV|R}&DAY-yib)6ZaGKL#6H3tv5F!}G)Q z2=(nu*toPqZ{1YLctSmR!m{k95^E^*7hdhhHZ>)!<#9Se3+uiY9HOHJWmw1h!oINX%%u1d?+m9E_^_`ufb6DeK z=lD8b71~`?poh*!Gs4Zq*`Hio=?V}RZP{?3{)Tr&hz@IiYtcL|uqr02hK zE-C7ndv9Lw)a6D8f$44Z?iuJ)ETGxmb9=F~ zEs^-*!`Ho1I+p27G{2clGb*)Ms$wmpcTA~pp z+@TUbb$HKJiMZWEnQi0-lo3%zdy4rExz6!6e1t=#n;dFRWGE*RS`5^Cl*FT6OQ>E; zs9p=|wS?;ZL9N#ks)tldtMUSYd{@lqZ!?qYK#3QX%$1FrDIYhYi;cVSq^DVbA2Dlw zShQP=1$VMSc%gJ;tHUaSqTWBrbU%t?dZPCYhK(RRmdJ9@PYbAb9jLcupF5#&A{EoJ>$aROxx+68fJ0T1s6@mNOUSN5lnr_r|U&3AY`Jy)127ZMe z%dmgc3G*`l?c+_kw9Nk^lSgL_8@dIdvHz> z+Xsp!ZBIuM&diyKxDo?aly+W!y43(p*}W-SSG1PbT*=K6z6~Y}myORjW;37=~oZYQIkh%ac!=p$8jEgAi76(_xq}Qbeg`qJ>@zw zDplR>PSMTp)7y#*TTA|s2f9<-TFB`0iTiuT=0KBHD)H7JH)U`$j!M%oJz>5$9G2GC zT5?)>r|PtFwB9Bg`PM>6lHap#J3i*h(4=tCfi9E6Qm&C;1q>-JB9^4y=z~p=Iea97 z=G4|X!v{1Y5!?yI7#UF;YmTUlJkpeB5{pX)!#?V5Epe$WF5!RP>*3RS&vMuIhN%`Q z>7I8Wr?P)d8Y}x>y`TFvBQ9XdXm`q9C-gRUmi~v{kNn{yt{B#14s0w5wEP8CfN&kD z|6(F`8r+-hi2sm!dK@Zz%ukb;b!oxmS?;~QH3897%y3Zt>(bN8N0o1nRbM@+)s9kN z&{_4zF>)>Bnr}_PB1=DN(RX4T*+vQTkCV>@nF?p$|DMv=|KWR|AL(sWuB^lQ*l+}t zp2$5_uPJff*A(-RJI^o@Md`|psp(N}_)TXxY+Ibe2CfF>y*ObZbFPHlf(jL#{2Anb zaYHe5Ak!0_7uV~-VoB$g zu(Y=jBJwpzaisI16bDisLOpl7CY0h%|C!XQi0Rr;TulEF;w|Z6p?FJrDdOIAT`2BN z--7s1x;_*iO1}efvMM|jC#!Bl+_EYn6t}D@K-{q^G8A{L%0%3~$`FdXSFJ)^Ts0>Y z7gt@6c+0A&P`qW;BE-F`jG?%9)j}WoUlknT&?@w;o-D(a=v56_*@zLk_7w9OJ$(zl zWr**#`-IDJheJGSZ6iOdjsb7+dbaGP=B63r3JmjYE zB{w785=ys((k(YdELnFKGIs?y#1-sBv=!P)4yY%ht?=5C`Lam7Clp1EyinNa5jFrRm7T~p7_T!#<}9xd3TRRs9Y$U z8^$?fWv@gyJrqu7>9c8U)wh;9Y#XXKVBU=B;L0)E#&l4Vz>tYrS_`KqlBQnJRE*#M zGC0}waK%N1jP6~cddZM_+{ec_qE zT$e0vAgOmrvs8%Q1bSluB?1rGIW>Kb4j*?iD)c1QSY5lb+Y=3bn5?Q3!Q4VGWlwa5xv?Sd#+0Y=J?UvykeY#*NGvM`3_ zW+7e(&$&3!J-sON%T#KO7qm!ybsF!Ujtv0@H`*zOQ~T@EAkRC^h_#a+44t%*~9?e8$;OZZFdbQ9{gst=yLRlylC>!g5VYgcO;gm$WI7(o;_DK%x-L z8$DqY8u2=SwZV8n#y|cLclIPXU1}{K2v5q@_;P>oG2XP;aC4q}KS{aZqxA`S8+NwV z#DM>Z;9JMZuHYVI)*mrDR|qjzUYz*Rw5^;r*)rep(|TjxhWk+Z7vE6&z7+yD8b>cq z#LmQ_4_2e?GMT#$GWTBaooxI@+&8+t2VrURsI#hlj&xAbBE-he-sK#?`-Yhb-3(P@ zN0o&}jnLGYaU?vKSR&=$EBagu=Kl2bKm8xU&Vtq(MlIxvi42UCz2RYV#pZ(b!`2MD z?PC7!`>CDty^sGO<|E7Bi(6lwluD&=lz8Wka7g$xh2oyAEZbBLSsuE&U`f&in=T&r zhQ4*>ktvDlERkat`=oC}`2!ui{XA}zh#^J@vuzV#@#4jcGVk=ncP_A-LvkzzYyO4{ zk@9~l=lv|aNq#_D(Nzac12}g3y^_^qf^LKICO4H6 zE@}nM5S8a&bRrm8LH(B(v$%y>)${W67i;|t=NP$#-b7Qw z2Atb?0UCHKwQMRX%Xa017e|{zzP6Il}sXD-`a za&=z%>n9Y=GTSi0JZY9imRRz7j+{{DhEnD(i`+)-T3-TYGjSP@77~}mAxxGrjVV_7 zQOF|8%CcDvmOFRZ89!@}g*D6mjMsHE`h69u0u@#UmEjwq+Fd&f)hm9og*bxigxWQN zR&s3vMs4hVX2mhi!`VcvAQSXt!Id*T8C*G&ldgmayAJDOvj^`J`Eg%yQJnDg8DaMk zv-7|UTFk_k^T2sFltf}aH$V@>_S63_j`u)ja@|pKr>Ds?;JK=#U3jXbLzpYmFm@Y% zwe&xbjcRbCOYQ*YeJi;}($49#dk`*bbT>c_cvD=9|I?yL>lmF7?-L|GGMJx+e4dOx zE3V}oqdrjx5W$AmfbWED>MepFsk^VY1uyCBoY1SwW=X~bh&RR zjO8bk9MIk2JH*qzsj$0yhv2W&?d-K`mi9fMOZ6=33kS~T2E+>2a2OxgeUMt#?(N%_yOM?NMKcq>m{D&~stdKO6sMND(`V|4 zm&^DfA1h0)^ZSBj>>i^Pqqy&s`zb%Y<|+5Dbm_etbe`UMI>%@gFysJ+T5bhqzFX=K zmKE)oRI2YU=efb!r)NAuD5_sZe9VyyTgO zQG@K8nJ~?e1DpIf9icM`pMmt2l!lyOoLA#F`vqAOM(ni`8^yzt!=5Q6_FL?CNNM(U zlF`1x*HEHKvLUabP>}Uu>)iMG?-H#3SpMohAt2sphm2*xSn4nqa#xstt$nfNwqJR9 zt$=vnI``&F{eH-#$Rrwq>Mb?)*_j}`Z-NWq{0$|+{=S3Imwh|X_lmEih)7xTl@tq7 zuK7xe5h+*tnH;;lvaE~u{6$v^uR2#+-&E{kxhzBR(lEOkE0_u4#9 z>Qee1%G>9OkqwYx_lx^HR5nn33c+izPd(7T$T5|_$fs5_sD5;)ok+C9p`A~~pRjgx z<9$EQwU$JJ3MhDqc$|-f=G?5it@r!7rF}olGnJYcXUgpGuGxX|{n%Bnime`sw%-=D z_`j}lYi`6n%tkQcZsu#?GuU4T?0tCSqXX-kzUuPqS{Umt5zN z!z)`$20L!ct5-3_u=74_*VpCal;q5J!2-FG%AK$xV;DHB5Eb%=#T-D=Ovif}4^Vcj zxP*Tt3#05&#OtWsj6T`<_k_a6c~Ae%`J2((`97QHm;Uvhb=>Old)rpPO0-_*>ifh) zQynF$wV7VC%b$l< zE5@&Av$0nBit+2)iag93IIYd+zbX$Wb3k0qPUc%fE$5--Ey>pY`D_$CPGYBhXETj93s^WJ;Sj?R(F1r>qY z+74}7ujd1frSztb5e|1+`E8$|bXoB$Biuc6!D|uA0{_*pvt;Ln*{%A^%M+wb@ln5x0x8USP{KvbxVu3MMB?RW$HZ} zVOg)AlWQn&4MX>GQ$!B4bD&n2I|3`~kx(1ep*A+R**s13d|z+KW`p5wUar1tw}@2RCTS(I3n6gicMA&V=W0_VkQV7hS%Yv`>x6 z`eQoO2~*1uTd>saGb4U=eqe_}5`IrMc4T9wd0EHrcIr;Vl+A{N947B+(y^P9xqWv+ zJ9u-!Az*B1{UP^Nh~1>d+|EPpZxuY`IfOMNbE&RF?!V}21f7u7=fn!%C^cL~?Mr~6 zwF3uq3;Pxqv|_jOyvfyydFN-aH*W}2=l+XnG2uC9R)4GKr|PI*8L5gM^I<+z*j!oR z162wUW3g9toHL8V&SCGJ(mk7WVSV#+70;ufVMFUV*Q4$PypT5Ktt@Hq*aB|M#+!VD zduHN$Gf{a_1@)e&+~Z?>EH50oV@}RV2k}G#j-O8xwY5fixc7t~FQz;P`R98-^z#;I z+g5Q7hOeJNAH-~W7Q$)rwYkk6Sc3xFRko9N^gF<1pbX3Z5z1Yj?_BYNX?GipQR0r9l*>~ZKzA|Dm_A-c>6YV@oaIftMyi zN6{rwlXQCH0Dqlc`lILC;VAtnypny~kv*)}{m~sZNk_vx*9=F6Ep^QeI95}4ueA48 zlEm-ZS(*_LGj?v-SxVva`}hM^Smw}CF@ugDT#B0`-Cj>OdWepSn$%6B1%2hM?M}BO z;-AR3SDseLCuLdCBO-M1V;0(iP(Tgcg^;X`E!k_K_`iQve{^;HhP4S6vGEy6kHJ~3*EM?@0Q;M-{&2Wug zH>sbXPefk}Um;IlJFM5!rx_;RMX7SX`Wp0cVD}Ofd)*&F+jNGzt^0Xb3amWz#*Y+( z@wb=a2E;YS-|(~f8pMh5Zn17j32xnb7z2ldMNtpHN$%h z+DXI3>xK=6xA5*gwqTrl)>GcQa9F2_zU3wN-#uO2J!;Hw%irClo-Y0_HKxC%)V;_v z&VOHxX>VEN-iJHb;QA*T_R}V);(I>O;|AAr|+U#HDdIqi$?s2$>A=`78=AA=~zZ9E-tzFoqSO77+ z-43nOe;}s46gzWsPY%3~JQA*{gnXgNTUSDz#8;NB?^;_JF3(It!vg#myBIc@ z+K0 zp5ZJ9vl0j5mLJ4E-6MA7AI!Q}V*f-wCblz9r*Mrd77e&hxe?xp@V#mn=gV_>C*9Ve z=TLT4|8MdC(@-nD;;+^EeC4yMzp%3P-?r0?b^vi22mWOq517+FQs&~dfr#+jG?KbX zV!7TtU-wCmsy#j~ zU_mL%Wg`g29k$wk)647sW>nW-2Ehunz2Y(mX5-@WP5%J|HJ6q7S9tyc_NcStoo#(~ zyt8p}dGTd1&0=A;)NEW_{^v6rw{VUcxE449_nMn?Y=oZA45wN z;p#oBS=UmMiqOX;SE?yXb5k5wZ(}Ki+cYT)C9``I?hbWajH|!Xo_BkpamAr2vOYFE zCHEd!`qkd?@zy(&Yq~YdS6d%-v~DF9*mB$4TLDKJG)y*&wMY#!eMj%v$aN{M7+1Xe z_L4)}l3+6);(HNgS8e(J?Z-&KkxxKc)-CZ zHO>Q%7S#KOkUoo8Om66&x*lx5H}}CEQ69XiZ|K=!_2`ohIAWYSoc;uM++^q~Xkpm5 zQ>dD*m9(C|kp1udKFwum1UA83bcxTXriEQrUmu<6v#6O33QJ5L$Q)ukbWinS&vsJ>scXz8jjkODuef%c(wY0HCpRxRf=JJysW7Jvn5mD5 zbaO+Iod^24Ri`<5Z0cvIE)D0GPR`rx7bg(bHlsV$E+3mwKjLsWl&>!?AH zy)2X@>nW(Yk(QE_-%8NVa5d zZydIk_`~_ZzDl$+=&SO_c#`CKG{(ISuOLLau8@|le1)ayk+vF^P5c<$(dxb&Vv_QL z6ogox4ZQ5C2$Z+68b;Kh*4c6G@Vjs>R0K-fcKBIey*^lccQf9;tMb+aUG94xo?CN% zhVX%*V$NZL&0lx13)O1V zL;Dy1&h+rQK}}vpIQ!bz0#fa8;`OcCKkWYKiuO^lg5rB?W!cQyj951oYI4@dvO$+5 zDmHIZp;3ruuaVG}vCEim%uDdZ;tN+AVNKwtW$jbv@S<6kXC?+`^hcr>aRb!C?mIYG zL?#LBRm#l7UVIaeHYU1RdJs1j;AbJ6zAI9dtI53~xwEdENl4Yhr74nj~63Gto0cn?!TF(|Ce! z-u(Ji!ifJSPs3NrP52<~F&mT0Kc6K_H+ee7t17JW5Vy%aU;4u~d`s(2oe%T{_r}3d zGM}xuN0fDzN0hZ`yanGkYo_UZ!!iFKJnY+BoYXlk-ZR8%ska(~p|w@k^)S9p7A0JP zx075FwQ4Y`-7_m*pPwI>o8Wn7iZ^YZgW1`MFEWj38<-Zas)7@AKEsf->> zmeRU1wpF*|U2ICeM6BMk8fyc_K`f0$oIJxGSV~$U&U3JYvsNRWQ%Ar-KV&p)@*MDR zfvS6HK$8*S8t_nXUlFeO5-M{v!gV`*3V)ykn&lr+c|}zGHmi$b*E+EaB3uVb!VMHu zAk~#IY@nPw!~JFnu_mF7H^HA$gHhPD6+L>HEvyq*tg7gg-YjhQSL2lvD0c&gZ?w?n zlV`(Yxd8dj%j!LijPw4X_IU!`cO^#ur|JF5x-@)O?Etmwgb2(*RfPjIvA?qwx=M#! zC2Eo@@GTQ&`$DcGh?B(?(tgY!weNzzF6}qP8hc{57N^cSoCn3A`){2!F1L9;d*nf7 zE&1%>IM1&iafBrm|N3FRH_n4MWhBl$Ep+1J99gp*?z(k zQTELCPadbxZaq`{$s=rhKbU515PP5R)5n?b^F(@IS*8ul7Vz@_aKBC%%Yi_Cojj=aaz^^A>iNuT5j)j3_tn zvTpx$FI`5{t;L@{ywpz@@AK8FJ=;P(uMTW(V|_pM|K|wzhTxD+{=xrEGx$8dPe?sJ z+{ixaiN&j&N3aX=or0^(_`({E{+6!}TJgFt*y@QVv{8MDvIR2arX2*|Y0z_0`BI2|F5+DG#0f9`U4d+t95zWqaUr zb?8pu40=oAOLgt+4Q^aEt9#;7-5=uB(l^(v3%j;29<{~j@A~S(o>L+H zo%9CoHKSND`*f6WZEdHnMst&qMKD zoFOhz=A-l6pa|qfr1h6V3&Q2O(>ly~+*XR$-b?WHJZa93Nme^Y6k5FD*Bn{jOKW1m3(ioNn(|7`g_f1IF~y%%o* z%}(0LzWpk0C$^qPdLGuxT^Dhd;U1=0OvCN?%@#&SK{Y9~@sindS}FMWBhN<9Nm%-E zN!7~D#(T43JXZ^grOr`tO{$b#aL9Q(dv&+vkn=lICSvQ=|22{e;jN*Z!jS)c>Dlqd z>km2ax>O@rbDhLEp}rzeQA1r(?9f5*Hf1!)l~~eOloW96tSefYljLG$s+_D0_1#_) zQ)~pa(h#GYCK1azX0z(8Rs>_U@@=@)a1OW(I47J7E)y;b?q(yS$N#sFd0*+{LkX{Z zl~qPsN9B|m+Fb6Rg(e6|5Z0qA%}y*~P=1*nmiu8*P0?XL(GcjI;55)ZIp7Ff9B}~8 zpwEkZyk|b%CF6@ofXOVq0*ua=4vqiJ*?@DN8yAZjoY5ga5`H<-b@0p9gQrEg{mUAh z8efyDSq_8bgxdhfgJw6g%U~Q7cslf_`v2ejVh#PP`DNex`hRzROJ1*2zKt2a!N=?c zzMaN6~%44I+t_pcAAT>mKNqUIIZgJhe=k1`Ous%{ugtrNiju_~LAE&8X{jcI{%_)+wtj_~wpWBZ4+~pzG_xE+TcH`^0MrR5&d1Psq_xcXXY* zTw31GzG_V5Ux~Q`M@QF2-{Q+bIm1xQwiEa+!y<4+$Kjt=e)eV&w0XY~@%p`#g`N?U zSPXVM!z4u*@#pM{LmPLVz0>`j{)VcTJ{rXSP2K(l+CE6^_>t6-wX`Vzaz5ah& z3wI6s+;5^*TJ-0>5H#TR*lYNG?k+!*^>O+QVT9h8ZN_aDyn8klU7J(oTrH7t(OupWZdE+bTjBjUj0dtD=kO}#lvl+WUfzle-d=5P*uu?87VC~y*u88U@tC|bpAylb{j6zaqj{mRc^Z~%rL6_Yq=iiZ7Z$&X5{ zeuh7O8@G+wZ>4o{MYt{SR+SiVTHZEf4V;11sm8ho_^|M0S+FcJMc`Wxj-j%wFe%sL zWn~L1i}|9oV-ocK*U643FZ>TsUIfG%lkcoHYf->anmMeEDj42t_?y}v+mpdLBl7RX zGDr#dvU92)-yPy)mrkcEgR6#{Ll{;c1V-5nJ@1+0F>j08c3L_3u_Yj`jVr>t5^sNr zC2+>!7}i=IQ)R$iSLQl}_sXcKUfe+)yyRvSRSTBv}Wn;0q zsEPHWzJF_iK1_obaZM!=uBJlVWqK&AW`vpQ5_$j{i{glq>cVQ+DT~F-#|^cuy6}kq z%dxf(VXS4B#yZP@Owi z9o(QL9=nX@2I=fdDMV#oWmI;p$Dqnlf5y0WBrQqtFF1j^PqrSST4^eII)! z(aQlJ)l3c?E-nFGFxa((I?q)pZ zBOp4|x_^A5=0dr__~r${k;F2wxp>tfMYVNqiv(TgL(DtcSp1 z&C5uFr4G|*pMu7;rDSQYHQ>0HouZoxZ?o_I|8q$ECxX)Jork_|0Wl-i{x z(j0wZV#Ulgd55M*&*tzgNp#AD!7fWf+>$`~k+r55-KkjHbLW1LW2yyDpOM2YgA|5O~b$Nc7u3lkAD(X8z^bHlgT_^B9vkStq% zXo|;6d1q`wI5$FL`GB&^l=-6jeQ=cy3zbJNit33C{UYuosbj%IBkLU7p??M6#$1R; ze@HLidV$$LS9_?ZY8z~z?sVWQl`Eut*cVjrrn0JSbXm`1RYmL@&OHwidzz1QaULJe zO$)7V_qh=Yh&z4IHm;E^h9u}S))k71)9Wgx+i8l;#}{*>27bfmWHo#Dgs`FDbF!4A zjQLES!La$gm5=G=o(nu=Grok_dwltxxBU2mI>Hwa?ithbR>;lA{4*2J!VV8#q2CAn zc*6!b+B}4{&3m9lg#2I_Vqb~Ccfq0`YUhOcz8FX_ymS@*9df^D+)k4>NP5@`UZu)@ z17fZkb|Ckhs71SppePRcCGo02U4>tYfplqpmw7*xVo-b?fIO@#rXtKIh{{ z=LT~yQwV=!j^k@NmZU0-wa;aUMo?mU3z*@a)iZn3@IopO}-(xS0P-+xvJN2;V zy~WQk3SLbo^sOy*Knh>GrAFse!y1J3uqu^c^Oo4`#v)2z@nFKg4PlJ?9*+Z*-R zAxZdlKyq81CuUnaRw|MICgvm>i^9uqcl-9xm9?F|2ieyJ=C@(DU|s|>OS@Oo6rWc_1I1qVTFq#6)m~I40(I~3at`nRcbyrY z>aF|x{C}VSAKf!&_CEWu)?RDvwfA0o?f!jFoTy^qO*UU}Y8YtES(k6xqNTIk#(vR9 zDH1*OzHME0;XrVbu!MTtO7 z)y;AHs+L0D0O!;9y(?tV2DE9PeHnY?w~l!2sy9=cnZ2%+GwKfT&?8=($lZ!O zn4MGnGw}D=WD9+oRv`~%W1snPF5*nBJKK>{y+-|%DV5pJWa}F_f9*Ox0ve})bxpQY zoTr2C!?ve0`wy_^&3=5Rc)w0SI=mg!1|j_^q?=eZtl7?4^o@f5PSK*1Aswf&YD1A; zjPyJPKO2lZi6l4YpoE<|Io^QxgxW~F@gCmTg*Oi4jZ%~$M;RaK-0+4RZ%o7+Kj4kC zc;f=zcmrkRqKuz)?s&ruZ~P8#xJ}3t?RcXDZ@hvs+OE(;2n4FOsz3yrx@Xz zCk9NwS;q;_T5$qx#ZGaeuLsfE;RdBmWxYf7aIv>7GbWLHcH-TadnC!ffRCLjLW(N~C)s{Sl-eNBT~rmrj_5{A-c_ zjPDSnE0O*<(!WLeM@avCLKN~3LH?`0-bf#U^kPK@#B_dmy+sojqB1<`rpLZqZe z?-aA}*0XqC58d@G3$GFI-fq0J74h2z8vwF z5q}l&&C$8XZPD|mO{dM=SZH&8r%fxcgM*!<>fog;LiZJJ&533O z7_}NQmy_M@>!LSGa>vSu!R`j1a`dgnH(!dK+?l7LebRVx+Qe;imN5P+4kf_XY`+q2 zQMZ*9FEgMG#c5yGc%n8|9B}|OO`#_i(P|6EJOnFgDMEGNQWYeAm9>96nlB!&DPBe} z6sH|UekwfzcOl_)4W4e4|BSVNE1EBs*PJE`UO>CP2D?`3VvL3ypdh_(@{uawLW|R$ za$}V#(7L;<3bqB7SS5aO)gMI-v3jcA?ICT;mSTsy!Bf`q`Ob!qTwVrYK>;fYdY{c_ zVb1qkr1xcb|C00lFZ$lU0{NX+N@quK!_eM5si$tQeqD@GVHZu5X#^uYc4=4(%YcQO z2InXd0uTZbLJ-slVF=*}Q3yJOi3pPsrfF#PTn{M=i$tvV^E|A*)H%RA^UF!g0sdI1 z^V@=N21|4{?saj3w9fPqR`^-C0juMP4JUdKBzmZ9JZ~A;6M~un|2?UPEd%k4QNe>O zvQ!Ko2KC44=E?9?J6^rAoO*?NWKs=b zT?HSZ#AWB)g1+dF_m+zWq_YByS*gTB!Ar4^!w(t6Lz2IdinUf?{Upmzk59}4t2 zC-u3Eehr8Jffik=>?_piMRTfiV=@OX<~%I% zPT@Hdx0W&OrqWMpJ!3wpRU!xo@)!;Li@FyoGSrEByJfB--2*zmzE*5Ui(R{g5wMEi{rySsr8=07rASA`1bbW+(LJ06}wagzJ(o*KX)k(tUTIP z;}a_2G>;I5vqW^_Y97w*YEbJmcX(^qAv)g)APWT8UNf9EpgfsJSlBCQry>q*jf+oW zo0OY^m&GLz=LDw(FXQn|@`*z%$v9`6o+p{ZOL34`(t8}w7x2Y`6bCyZDNex)1wye* zio;n#DNe@A3S`A{iUXxdarmbFI5+7%i|aiL?^fVe>@LOOtYX^mWddf3?uA^aO5G-^jgTLwEgign2ZQQLYtuR`#duW29@RM4Pd%ZSyIxdUJ%`$&osA9KYg6AecsO@yX z=6km{d~hr71+>5Kbet1IjWUg=x)bMp>GS6<`aKL!N4n_qa+fz&vm|^m;7o=9n_krs zWrA|HfPLRHu+DK>BkKLs#xw9td!|yipa*%=DX(muA?;Sud9|?Tdy_YTZCliWtwrJFNz+R&v%Mn61u86bgMa^a(TU zmK5J~U!txQnjvy#?0%2sOpC0O;cHi35V(xyT7kgn?Fn!EGH#Us%7refloz_~_Rzlk zi`yz-vFaN!pt{qc#~DmsqX#x3yXbRq7u7Tdr=T#xMR=ODRxQfpr7{&#nV~3iILcH= zW%7f{RG`eEQkelLjq9RMom8f8qVwDid)#?$hdoZS`Oj)((rKQ)SXrU0q!?p}i800_ z2(E9!G2x2(E1&R1{RP`=)sdW178V8C4q7$?ycCocxQsPh7|VxU)h6JgnW+dDLNoIZ zg=HI8s8n;HutS|nwC#sZc;eM~<7rPPeO|(EnPxJcc68F`IsE1{cjBp}lRp10rBB6E zVJCf_meS+!^in5%9>;GtO+21*JL&VF^xKH1Rh>pPdACyOI~{+;+6%z}d!q!^Y|IB% z60MP?3sD$oE>i^yyfd|KoF{y+IVn#8>hp~5bR6jFqn;t}p&T8j-}NW*2hEv{i7$bE zHiCX+pdYswMImSP!pw(n$&2O+@1fHU_uP|y51n>!$tB-q@ZiJwreQXSZ+;`c1vG{@ z4sm{v4;*-$*TB*Ra!JO?LxnJ5%eVyD?oVpn4IK_mw;X&Dnc%)jwp&qP3FFDLpur7N zD*)?aV_;=fnZmTEYZ(WLWjb9e6YxDjWh5`pr_ny&)cL8Mj$>`Z6LsYwNh1>K(qBtM zo>Zo(0zUL4DO2G$nhQ<|gf*5Wx~f+0TUG1sw+OjjZ&M{@mF-Dqp*T0FemxC+)1d{P zTRRBiZpZ3Qx5Uq}g0!mBEeWT5Bs@tpgM#CLFBNQJFN2QAhL#-(o|TNTZC$CSDF6^9 zniOQ)6%Bs_z$4+3#hj|;qrFmIf`^O(F~1D0nLi|fFXVT_*G1diO>hC4f*zb19o)m+FTUiVM%pS2Es1>3omR65lJk9+==s6|#Nq~xNW z2gJE$?kHE$TW)fDPr0hj29!HQDi^R(nR-qinJAbG4QorsNIQSGSi`Y?IbQmlVjY9$ zARBy@LPF4vo+Wy5`h6J~hhpS~@)BjW(Bd6KoF1GZ>8a_%akh>V0AUF%qh z5mG*+5&u$}AJPOVA4Uc#pF$v80ZQ{inoP>ayYle@aX6)+-B=qQu$N6^7v$iySwzPq z2Ycw)-8va^aJck29yHBP-VT46{*Z9w2gyIpf7u(qL_#X5a0`oUHwP&vTg8eMpv2EQ z>Fr38KFcEUw#D^!DBiXJ2H9?2dYhBpJ|?|ADv>8Hz&e`58Tjtw44K3km1-V(5RVa9 z2J=waZXxuRgm184rzq%fZ0=09U6}wi~AfK(eIru`km6s!c%OM61p6>KwDa5gPx_0 z?xnf!u-Y>lmVXTUq!?E@6ZqW?QXee{^(atcJ?LAksZ6%rnji~x12_D$Evq~k5n2x@Ulr5k_*eVnvRe|m9jtqp#5dd9 z-vxcY-mXI2wJvAe=Jsz9SJ?gqKJ(kp;`8P9Dtx}!z8|0K+TX%wUOSB)5`G!})cF52 zeS*%8+>v<4tXDx-{kghlq$OQWSy*WDPbo~9@4{HOgs&2Ka*vVlHO7Unw}2(RO$*)R zIpA`#gfDe3zGk+Y$|X2Mhoj%!0H@n3Phcqt@DFw2D86lb8BQ=tc$wZdxs0I9?edWz z*S3EIIMnT4hbc8xOcSz@03?9n}l>KR^YB0d#WfAUgSbYgW11g@H4zy7Cb& z47}G$lo1HZ2)qGhd=LEi0T)f3PAVvDI}G^q+lC;n))n_c8&SsEHlmE&HlhqmTNyr| zYuk&@XWDj>{HH%PV+n*O?0c$?cqMpY?EQX%r0H?W-jP-ojgc2LY3-+{Ej&(pDwCfj z9dF32A+x>Vvk^Qr7vr`c#%CJWQk*5owVIS4A-xau%YEl@)SrN@hE+_xeJ%Eif3@y{ zez}8>(_YRliCZ!5vwq_~aI=EOevJ4S`|CgmsmkhTfqKkFU; zY3xUCc)f6q{Jy~{i&8rrZ*=7)QLb~A9hl{Fnp@RdY5W&&rSV@Rxc`(WD^z9Zn=KgU zRT&~4uM<>6H%!{r5n+8yop;uX6yA;SYIU;ah0s>5-p?Xkqi z{tHO%?Mjs($+x5?rT%6}rgZ%tkUVX3!ZAjI;~{Yj;CM}fBRfhKS4V{w!$7I{2q}yFksDHIT z{@EFkbfL@%!%PVVnK%4KoG`POOf5bz?3(}lT#_N%yPXJ%g8K|qzF!eEf(^$D4&ve!)9N%;v ziq2(zgmHk*LYeVR=cT}dvy<)H^)m1=Tfv8_P1+Uk0j2%kaz^w}|Crt7c(30aZ{aZu zc%RW9JWIS#%0dre4+0|zFphDrfU|l)dWR%2X{H1_1{)GpPWw)x57A<3;n9*^I(zt^2*k_!Lz5XhEOS)OCFD$xaF<{VD4Y6$o?Gxt`Zv}2G4AG}IWPySOVJT_jM9HXI`u6*e}`x4H+sH^ z=c`g%yBzJuP(R*L96>8h@axrv@3|e0vNmcbB!?b+j5flm1*09?DCxbhWLuoUH(LzS z%Ew01cP;ardaKiy1Z-S@H=;543$?amuCGjh%!kCseX9%4%$3EQ3EW_4J@1+5IU73( z)&3?~^jn7Z^KSO|F1g*m1GyQmW*BX@;@+ga9Iv47pL5{T6;?M?_URXG?nA!tieQa% zv-Q{B(Vo|T1z$0V*}2c}IZ*XzAl7VEM+9R|sVCmJEBm{ves(2}()u7giEtQU0>UFG zi+_oW=5w*Gt+|*2OD#oEYA$?lVf{-f)4;dUnX1|$p%-(`iGcxRE5Wka*kt(W4+~sx zNWxfsS`3uIaxSpYG^3h5nlGww*65Mox#;~kFR0XiHdixQJ)Na(5ZNUr{GeNIV->g$j%$H#F z%i8ER=?knB0_w%-boS0of;hl;aaAJMv`3 z83D${8F)hhECc`xiXEo+Y7809+E)Whz!_F{oi+P}`mGOKN;oL+2wszYh+#Dv>G%u@ zh&}|Ht3bj8_R*9(#G#deORQqpM+=v%#p#iIt=JQny=7Q^EqQ;aarOaV)Wj9K+3)S7 zQyKx@^d`!R!^-zZpNb0Ivo)CEhAya~GT~V|PD3eyACtXZ_;VB+j@STCiVeR@`aMoc z+gr0DjvKof_T{|KRx1Y>H30;lIuP)k;U-W&=hx8wb@G0~OZOFmkjL-m_UB=3^%A6f zSdG$njGog)`b=x7>HfC+TgV2`a@C7;8nG&;+p+yUC;biqb#`YPdg-?n^jm&FSl&m! zO1$xlM8Dpwp7TXmh43jtGs5i%nY}d3G{H-Au}ct?N$O~Tza-i5BeUf zB1*U5DFT#!&I6cHhuGCVL>13k-9qJIptrc9^tHxIITzBY+`Zu1u<*C)Ib{#biflS> zr}{cwvIJl_`#`j(ic2*z(BCZ`4D{#cy|&tt(St7^7rvgbp&msQ_P%qn@G8 z!wyb!ASbIQpTt>^ILX%jeSti z|0i592f|vd58|!33G#gHQ12ntB-O+#lyuFvG?m{he~W&~!&XI((p42K)kwB&rU;q1zo|f;xEV416c5 zh%+sCnvI=2X6Lh=CBXxr47X;QX#}IX6CV*861aTGgNLgJj$8zp#O;stj9i*tZeQo( zc1395Qok%3Km0s`|HQ_wgyl`&2*M(sv~m%@L)-=Px&e~r`caeha%;1T!=7_-t2f{Z zUQ19dTI+*l#SpC8uvJ_Q|2KxN6W? zEaZ?b>CHIa7oig&6XA6P143gj-_b39EOhao;AcirC5Bh>8d)fIx&zW7bE{zMN7~ha z?5`myzlL_@CuvvA-eFl8M<+UjY|=sK<~%-YgQy8Rj}lN)_*qat7kw5_C#w17J-l+* z#VdMfr@%VK=n<7cORH*wLYex)L%Cu`r5||nF}2O5lfIID3Y>2PcjBz8#GR+Wo%5Co z4H=+iu4H*N@6zG)_T1jGKZ7pVSN0GpI}KLKDErd7o{|^cwB%`0$%g8jM|)a5zqjN! zq>>3^Lc?5drU_b7RqGe}DLf`ET#YliOqE4*z+795f>uMK6V}6cvuv!gqG0K{r7MgCA?YLe1Ej>S=_5o3B%!S6iLEXz ze=}?-sLrW43zCv;qHayY0`6i#Y%Flmpb;>aa_XP=5y)xlqtYAfSP79(5q;)&qI!OZYW_Zdyay z(1X(Fc5CnneSn0si!mF!_Iui z`sI6SpXE?bt`OvU2NHURQLFP&CNO-Np=XWw%=ga(A|2tOeFgm4w%dFB(Tw||zcf0XinTcelC zxda;wC+9Lv!6R>`Qa`@&%bTmtgH~{N6J$BV$_P$31m=QHe>4Yyj&eocFn9%q98rhM zV6J)wtDbmYK{M1UOAgjND`8th+JN5vt_5qip?+B7#r+!mTTccVkz0oG6;?EAg%+!e zm?ws!v@;`&FiA-x>S#mmuyB;%l4N@OgV|g}^w$vwgsf~)Juz*WX4wd%O$?h1$rA=V zPu^>Krp8(upbLH`*8IbhMKgai|L|Opkwxsn?S_DEqhKz11he2RZ~XGeD&!02w44jF zY}nUPPknFpGH$HlmQ|TIO8f|a@%x=}NN?X`L9Yg)Jqxho>+885o{w5<%p~7>Zt2J>aOwA$V~3=}T9@1(5i_nGR=xOaB6Z5-IF9V`P<)t z-E;H4%4O8M{|23*cJz{3vDNhR6@{70XAM~byrw)EnaGN~?iXRv zX2@Dj5%eObxI33qaClaj??@zQEK1y@Q^}hq_O?KH^3=mz%4rjzK^wXqQ#uB;fuPCQ zfi@_mHvAcFxB_@?l}aOwu&d9@Q=hHhW8D zu7;iyXeTpo;87`Wr zuo61OmX|rj7aKXnt$_JuJPXo1U%)*7+DR2ogelCbM0jFR=9D_&AL44U@Fe!CHK&&F zIcigE4wnzVBb{7P?2FW!u{mC4)CX!KjXJF2dz4@M8mFMOuNwshTW>xI`^mfH_BCyI zCjx7&h%+?mI4)mqhYe7XfOSubQ)wvXgPxd*zPJ;;art7Q{9M#2SZJ*4m)dB!mc_~K+i=U2>SQT=K6yR;)xqHh;0pq~@|iPX$2jvJ<{~54 zpz!_{{a+pLWyAfa!UI>?gG=IDY&c^v(HY+)-6u=FLxp-lTf&Nssu!>0#ovd?+ZAAqNN=kAtpy$sN**lJ~P)WFXVQ& zIVN>QQt377#lTav-m`A>bE#Eggdd& zM%@u%Y;Nf$<5a6`>@zm~EUzrTRfh77qGHBI(PKhI(|xc082r^iRU~`_z;_VC;ez-; z&$;i{a-nsmt+=rX9+q?kW%tRqir&@mg{h9Mn}Zt*Il*i0Bc^#}wh1 zf+rHMvS?>~6K+iHj_Kiv^SDE%LUD$_97lZ=83P zjZcjiY_V6F;uzL!>up}(5C1r?xG8?YE291Wm|48yHosA!uV;Rj`M`?c%pc9Auy%G2 z<#!d$6yWXWnw2l9t*N4U0Z*-_7H7`fSMLFy+G~^d^3;#!P2eXe=IYwboPzLkGu(u_ za1$osX7vB}xbgX4zztb*KOspS&#d_)iKF?8(#^HG3F}FN+KKZh&txdunFI2u;(#b; z-bQdYC#%oqlk2kOB$vS3u!pW}Vo{h6dF$(Z?n+5FEFlQE`R^snz&u_ zK3j6%EJfA}W|9z)UxJ4@=%E-$Bv+Z72T2D=E*|zTGGAKTPseF}=JN#3gv7xfrY9d4 zy3i~XkVHG%9`iJd+-Z%;Dx#K2rN)S|kolrQ+2;6jr_3m`-`~ZV?}8`d5;GSSP>~(B zF|5$A$VTGRV_OV7z%_yoYUm2+nkUwkh6W!&$3I6xU*%KeC6?OPByU-w;Qh z+x9hfP2z65>!No9FTWqM|EYN1hBH`_><@?`$*;`J7CDO(pYFy#!Y=^xc^#0*0Lhjb zdBa=|D`0ZS=(z!Ro96}0`sWfwRwg_|u1ER{H3i~g+~K{t`H%Z;kCwp~_@ezU)bODk z_<;vMOmB|d&u#gnW_LXg4%lIH@8y7zpnsB`V-co7Ui9z*`g(?PY+k9qafNlr*xRh0 z*ooPmT{b&3>#(R&9v1zgh_7;j7nutVo^D~O8x20j`4@tpVG(-!2Nqgy`oY50Geu>S zC#yvIpAEmmeYLk3|7;j%lwAydCI=Q|7>T3azS~^=_J1rjTq^ZB$rIJF-&AJ7-FH>9 z@p@?Vheh?s!(!+-YU_)1n<&nl>Fq_9my(8Oy_7`bYOuxW%WVCBY{M0_A+?;1{gd^# z!1UDJK{%rtZ48f-+n2x#SCIMm@Ck9@M!Ed~ky`S$$Tn~#!=K3$hfQ2~Xl;M!#U`$4|tbSgI?O!Tu*Jv#xtxu&n|yOHo2>Im2D$d~oH)EphwR5|pkIKD%-`lBt^!LJUkD zg+SxN2M7xmVk|{?;vuG%O##2mW$MAT5rs`}{%l`AjP>@#4_EjkSVZoU*@t#oFb>lg zEH|zzg^s8#Nr=oYNfHXorbyGuqs8zS$VCU66j_xANwWH9Z%&jK@R->qt0YXkfI!-QmD@VX4rXkGh@9A%fbJdl#s!&-P&d-#d zt@SB5TkD@QC4I`055;NuABwXrAHdH@w0WMJ@OiY^04tJc@*s>_(26PW`hywv1>8>& zY5LieFah-owyVkGPhHhdXW^|j>c5}O7Ogh3-poet5IK{8^MF})%u)Ro)Obq>&`Z&El-zf4I5_G^s80vs-LN4!BzK@AKsioYzU4UlMG0K~?qb+!W;q>MuGeoqE*shCMbNtKAM*Ai(Y! z`uWyurk;pBAra3c|G$PV)KbkUG~aWI9k70$?}4)>c7=kCc}apa7h@E!-Q&XaKg5K& zgli_jT8`MSL_K2#i;P~tcAMja?ytlIT_3)GZbgnvC!9O`_3dAb>Bg=24QNZ9(3a!Y z&opR=B?tSU{X$fhp4Xxsx_UF=Tez+rCt8Wpu5U-qpmyw%+Ck-&0QU;`&*i4QVmsl& z&D7`U@qD7~o*s9F>XXx*PF6#c*dFr3aKDhPQ_xBm`#{K=`pjMPh zt?23RSa{3frsvxXF1VB1V6(lQ;GTg#=M@s%;A+6V9dJuL-2?YaH>vTSK{fuV5AIK) z|ET~uqxt0)g99hOVq0^Cs;WGN>If10bIt?+wUb#SF+P zG0wG~1;3v_*o&|g;X&~Hvl12{y@&oHM0ZKy{C3?yjqZC z{J<8pI!<*H6%1~{`T;H2FSUTm{4+Ry>Ve}8oM*mmfeR0FTK@>2xaENTIR(KDy0_Fr z!pi{jU}@LtctNV~dMT6KNwh!JHM+Dqp6nJ%KsP$4ybB=dR_$-z3hMVaySpI&TfMhO zTD=UtU)Bpb=rmv0+Nph^N78}vce^C+$CAWtb9(Sl2!s}{MmywhYg^iXAydI8_dq4EeY94W)l|{l})d9;=ODdi`~OU@*=hOX4M)iyy(U1?Yn=K;pH{;{TODh=uGebX9Cvr@ z=+!S;asMVfQ>4c`^%{)|{+9e9{n~8bi2xZ&Z8^4=8~67C>x)3}T92 zi1Yb=IwisUAx_r})+yT@ueA5a{*g|CS)%QJxR-EViSf4jaf~}B>SanN)=%_~J3pIW zlX{)Ty}|Xqbi;b*4XF2Nsa_|pEi}eXFSfla@ta@|0^bVg&7qzQkoNTHgQPq5>(PZb ztYs8x>Cu>59mdYTIyKk-61L3}Y=dhYb;BC3wDqrX2x@dfHTA$T=!@HJPrIPn*Y%c> zdM*&XhC88*FZRwp|(^xxVZ`4yWc2KV9(M$ev+L(&_z7irN3=!y43eh+TJ?whn=`JfiOCba;v zb9*tkhLd`7qwRMtm?w7eB?~ZO2nw7g1I4PmB%NstXtva;I}Gg8$D~>Z&VsPUJ^;h_ zcewg=wnloqak#US*|T-R9B` z1bbB{Y_GwelM%uZ0ucZ&;PjF7L*h{WHNKd*^t>59sw67*ePOQoElDo5wBLwFPoVyu z89}S#$M#+xRR>^Hr16o)pB~Mi7UK_%jywFc;bdN<_@Uw z9jQjXL@54Rl7{5@mgZ5#HodE7XMz`qM}0~1O1;_xBP2rUz#4OJSYzUV8dp$_rPs@$ zA!d^NZ8m5>gJh6hf^uUIl!NM=b;CMU1M0j(s*^8q!sz7J5}St$!Y@1cBAU17iE+A~ z@msL}qXWL-(8pjk8oy^|8+VqjJX$qd3(xCXQ>&w-yWO#-yAo$PajyvM?B8hXa4czC zc~lGUl=@tHrwDXiU#16-71a)VJ zV+z7z1V5w)c2jFg?xwaxWSu+6A|k9?Ty1$sYD;7$)4Z0j)Uwp%gE|*>6e5-C{ll94 z1evDIk=&8*ikY+Kr33|Hj2#{h!%6t{QJ!!)a09 znAJ&pO!5XblegrhCF2~^oyKJ;`%}VG1pAmS>>Ua61{Ej2HkrLIhE051R6p^)7>*Ey zpv!t+oS4<+xT|$}()*%*vY}DU-)WS$$W3d+IHV;Y%+3<*LYKUS&6l^xQme!#vZ}-^ zgv@YiTWT`lVv2+dX8%+6gRZuHmv_R*X_fG=cmJHD#`muJz=w%aw>Sy6i?D6+N~~ME zHK^QXi^`DMxUK0ITMVpvm=3L92|slSg8lEEt&T%oZpK!}(Jmps)lt?ZY?+;uUB`H# zoo`%Q$169~`OUV}DW%`{=(FqmW^AbAyoGk}E(>lr$f+A?SRW{~!{<7Ds%@xK&E~Pv zLceF=UDXU_J#+#5GU#*aREF%jd8+>JZm2UzzomEQY^d|*gm$h0W!n?=mO7Q*6qi%y zYd}jH819D1t@Drdk4vsoJ$q|ncAf7GKT&BCHdi(Y(^kaMv-{@cI^SpM$z$^=#L|-z zasE=A&*myuod4$As<&oiJ+Pap&VWZ)7VO6mXnhbKyVP{{@+zJ$Abf;i2QTeJJne&S zMX1D@-*$wLFh*WQxQyqrr|;MnfU_Z_3!z0BRJV-U7DZ1AYL67>Nl#MT5NeMUr=lk* zE`V~}PdViD6oRK}yf4LgQBI06dDHt+oG(2|aktR>v*|7sdc%$4jegjX+x$mA%BL6X zJ_2s5<%Dgj`|r8`-2F`ZP?NhL+xAeCk5-P)VX zABvu?6i+F|OUh?LigMeKq7OwMSE>nRdKdX@Ga+XRQdgy@wm3^nL5lB|6vX@ClS&NI z`EPTk1xRJyB9)mcrH!Q7sxZ~Xr7RMAOSw#=Fg82RbiOU|GP6IJOYcMu;^a3ZP9C4t zeUOchwr(X}!D{@hT;k(TYBe!*YB(5YEkk1778MiJg(C}>nXj6)g~7Y`m`4?wO`_xJ z*8HR(NuoTQXEuut_qLZ^zaPZ!JB^|vs_g|=%2Fy$L6uQlCnCO;l`OZekip7PiME*qd;E%L23O`~R$KmEcHL6$GXu)BiHYAw#{Pmo_rNc&66&nrboUH5FMkM#DJ zbLfpw+znt*1!LB!cH=ZbY2U6Hn!jz0zj-J;fetMkTEb&zn0$=VPJKYM0UkyvSFo9l zZGg9}M-Bv=d%Tdw!q1X+631{c=;9vy|Sv@G0QTOKF-USPK57N}TxA*g+ID zazHQDTYO!;G{)6S-o3pvq_>yCs$nYuSwQ>rn@Em8c0kUmGZAPHpY|Q{5a0O|Y;zD! zJcjiegau~YPVghA7;%|X#37ybD|&jo4QJr`dc3LaU~i8fxM`1nk$rsGJFXu8EPMA& zdi;~N9q92LNjv|q^>{;9fvd+iW#72Rheaa||NiVgv>d{M6lBWD$>!p!h z;jUgfC2{Wcd>JV9QXqOMurTm{t(R`8Ci*8{ARZuo2$?sK4`~06_~r6crv6V{Tzu0$ zeXXC?q{q3#{&M)$-u`~^z{^r!PqsOGI^3w#&J|^&xX><#FLp$(A766%<+Vt8gM0IG zQeK98)feeip%(c$>I>yNG&x9rJ~ZYM6M9)0lgBB`}iQfsGX z{$bYBraMzwQ_4~W&AoT86Q`)w;eM2L;yl&7S$C$lrt3B3Pp-o{>pF3q?#^|Q=D*^G zl)Gm=4ed93LYYZun8>x#7?M(!laMGhOy6+uw9v=9?m!F4qvXo{GAZVft{7L$ z()|i4CZ$W~iZSg+Uujw$3%jCRF@M<48E;K!btHFnvTk6np}-Ii|CLYRRze|vn7Y;Rsz{&jiSUG`3CGhqkv@-#?hT9NLUD36(IVMdn4uG< z6Kx_aKo~6VpFkY>e3AT>sCnreJNc@J!Z;9uphW0EIX@$Ojc^trGK)ptVI`V*Uli1l z7Rk*{gbnBWOySrJ*@T|zJJ?7)H;rFkUZQ@36f;Ax*sj>a|X#0VPtar?s$NbFmf(s3S z7c?)JptBXWyW^gsg&mxEpVJO#JnVqRY#dW)(A%4GoR&bi6*UJwSxdRw72{+LO4;YO zovz_4roe zdxPezXyNXQkGxoB{(Zu!CN4#Q{m@zD;(QI}Q+PiIpX)W-hM&gw1`R)s$G1h}@2o3T zfQEVmZ}Y*1rGKZl@wr}e8lQLM;D)W}d*HEQKh-}h9{aO_n|^*k0nk#0zn)nj)Qo&h}Ty+@)yRRN1W1tp+2RiAqIZ5NT39h_a+ggFOz;WBbDRSIq?$@6^^pX+_M4gVhB8+`e38hl%P z1A1z6_CMZ-%Qxy#-#4j8eZQw3_0>?1`c9R4GzWIQB~p)$hwWaoV`3-S6+V$oD>^H( z9S*Cw@X)5&pJPpgjpyu-X8#kI{#?vj|4zNaYq3!u5niDWJsr>4=UeU^F(VF^i3JYF zJ0jkg%oY2`&5wuw;>n)2@`mX??iWx>IN2qi5ucb$@vCQRvo?vdbVLhZ9gHbU%NTCn zW*%y{xH0<&Yo3%^b-9CDRog+W8Y#7EI9jy^t$M7tRbiGJwaSWC#rJE~gTJv=i_xmZ zxbKr%^_^&bQ&AYKJT9`#Z-*Z}coa7;vc~o6^o^Y{+YGoL?LOA{FYM<=-Cl^(Gd0-b zj~dO}ziurNbw|q^e0*5r**sw!*O;HpY2E4tC8sULNt2(dW0SlJy&!`RwK*K4;0T8acZo>3^ zaj_DfF>=3q9GISd?Xh4AR$`};qlQ}2T_a{D73ERmblI^6|yUHz1u#+@_cZjF#8jQO%|H8cNS$uB?Z{^_o znxhCFv8FeF|88n8HXgqzHfF}_?VIu_*BTcd-Y`gX)qLtY`2Y6PoIw(5db1~X)n;86 zdw8FtCJwlI^2(@jqc5VTCzr;DSCnErvy0TP*oQ~Y;!I)Vm0U#wujI^Aq%n@hHuKt^ zvF(LCCyi{Xs0iu~BwBh0H1wVL5zY-V7sWH2>pE`PBnC}OE2HuCb1@`pxVa?wP|N1! z6Jfzmw@U_1(SSN~p>T|;ju&S%KN3TQ8C^gI4KNpp7is zE$3^fuSf<`zmZIQ2&bab5gtJ}+G1G_%51hg#6r>5C>p7;%BlVko(W*nYAQ8+Cw3gP zfO<=fr3SvsVD!R!ynSUmzS9;N|56Wcxb?6Owl8hJLy}N? zME=Lm>bGOT)KS&oHpS5lUN0IKjU|$+;;a(04ykJur z?(ptgov^r8C)n6V+eS&Y9Bm`nQjXi($>Tgpymgjqjz4rQJoKVVwlMpgwKtY6&`j3$ zNW5!~30=QU;ysCdY{)g2Y`J{E1G1$Ybn*4@ov_Ix+49&LqMNM-w-(F7qzlI+$r5kf z1-Zi6OS_9i-P&W4Tsf2@M6t%D>o~0d%MVWb7UW8Nb*edknICS(5z>VvJIqySKbou3 zWzn%%GrOyGz6(QD@BS7H?GytxEUQTx#2wqLV$GrI0J%}&%TQ>Ld#h2Id#ag6|245U? z@r7?rSb|LAi_5?y_2;YqfiLcK@r9dt)i>7uWFek#$5m~C)dnNO^n%_cE z9i=y;s2}ofMo|vOnC@F$6t#EHZ>6Yi$CYEF2FZ)*&F*BmqJ^I(^kirajn(_xojOao zYpf33-R;;_K1`DJqbv+JUj#(GbXkbfHy*br8Uz8i+$`q{Cs~I=f9TJ1v__=_-x0rE zy94&-2*iJ6Z|;ZCMsautW3<2*IhFH`##rM z8V?`DIH#P)+ub=TZ9Pik8n_mD%BsNUrPA7S;kArPn|E3&je`$`8)=Qxn=|(GX^ofK zoIOzF(i$UnJGI8q=!vv)Ui;u6T;*R$#%*SK(A3j1sxwBJpN74;#x8JAo4#H9?K--( zopiP_?{F{Zu^v{^UY&6zonl75BbX11b5-R1soOE^_&>~_h)N4M6K6n6jby*cm%w%a zIw($o`s5p=Q=qpu*32Er?Cy*prX5S1OyZfOgMK0g!p8n|%j(C4n62@w@q;{g1@VR! zI=v9*K%s?Jv+#e}jK*(j1q;LNT-)MaiGwC0RN?fDAs(v%j4uqYFc>Ax+-_&PaF*w; z3;H@Q)@;PgmO%nAZi^bXgbL$uL#b-y1^CBd`nvThoK#gnuVIO0bP~vK&uI90)kFFW z>+W`Vm1`5o+cy`7m;%l|q?;=iO7i1sYanUvItm>X=&3krWcKMO>lIaUoqF5`k*kIV zt@ylVo5)q-)=@RX4WwuZrAEDiIPV5ddAR29gfY_Bm2Vk85f#;6qYXI*A6sk)PCzB+ z8LrU@sJohKHef93(b1m8*@{@w;^Rn*mq}W@Obsnw_6T~m_GfR<9JIiw%|bcXE~W0*^Cn)6;O;5_70=q-h5m z>ujKw}UV5$x9;y z<(+)lI14tNm5LGW<`ebq3XRnr()2XW$bzP4vId#=>3H{c{)h02)5cN8VJ0r7IG%5m z!LE&Ich!6Hql~iZPoYzCG2V@`IfVxCd?iYwb2ln8()r$*>)!L+125#|IL+mpA*K#Bb~}DP*7rLAzn?ii@OMeMb3Vw~AL($;2Y0&W zgPc93V@&USP;10FAH#Oc2Sd%xgXV*0(jATu+wvs+<@x5>`+tl6((U-uahjVs`@Y@M zzwg7cd;0#d>u@>rhPdoT=l2$M)ju^#SUNBo-u8F_dV>S9;4M(jHlwn-hhIo{ zB>wqOWYt`ZJ7mc~*`TYJ7S<&~aZ%RSli^zq3#|EqOQgzoL+)6*3( z|Eo2f`stfm`8d9$B$y=F&^Gz!N=yr4;`=(Z=_QzY_KocRIW_5GXo*_vK z`n~-xNJIJQy^=J9|83|jO3Uizv3vVGd2~BM%BU|%1}cuol4Oa+CK>!CKit;k*aE*N z>=)>Qj8lk64^7a{63p7f@5Eh|+*Ej-HhG!V3Ezp^tGTIOxJPQ_EN-g537VB|1bktf z#kT-!%sjkHZw4he-wNz~YiPf>(p_))x!$6ZNVlNU?ismwq+rHKq>@Ti3nq55C>weQ zLO#NJgggWbLKeak2$=}!2pbSG5S~RaBakiWBS}Suyi&TSc`2TFt8}h;-NUm3MA=lF zv+-c|?OmbRTT$x*%&CdIYgg=mnb`CX*HlTV0=R=(IjONZ0>b|Ja$56Zv_PCctm z5ZE|_bp_B!ekNvsYsNs=+u$EtSLv^Mnz=zs6PVqk#|W%Ugr1Yu*O0;iQcuMQ_fWXQrdOPC4>n?_Q)vA{KIPm&MZ|3vRwiw|f5D;D zk$k;D;NbD0P+Z8kaE+}{T*Opmh2mn}R|k;BbM-j_ogBXAIN5=FB<(q!oH^aOS9@Ck z_8PKr4q>AQt!ck)aju;_YUL%niTkj_8nhHEy)15m^AvNO4~uf1_91)WdRxfS9E~P3 zXldYxQzEB+Q04eXGQ;+}@|xp?4s8bP_|?U0<*-e=M|Ei}ez73)g>{MM zqIGn)V4?|U0Wjx`*M-uXI$yIjLpg!YJzsNN=+a1Wp62x#%4m&IYS+wbjOedPQryqbfE6EQF11(9r$Co<8rCbrZt3@y z$@P}+e<{OAM)k-rqSGFlB{`OlK(ebxwkeWwmK7VAHFKtWlD{}Bl5~FB(dX{0x@s83 zB1Bu}O!9wEdqTr|-&Gw60#+7hSTn9UMt2QOTAY!OCrwuXR+(1Aht4eZq6~%EfcrTG z-e9_rK@x@weN()>@=fus%51DlWFDY3iEEB~+TD{bWRO4Z@B@PR zx4ZqGPFZ0z(c@Gh&g%F}XIh-+yBW@Pc;j5hF2kW2jps8@(!TYCS zMcCLF8@zYL1g(GJ;GzRuL-(}vS0D6QZxC!%S0DI{W~vRg?1Q5dg!Lr{mmLxsy6>`H zeekob*wd@Ly5Qd|s?nxS8r8PJKHNOx0FOJt6^A0qyETi_wxnH3Q=QZuP`PQppkEj9 z3pczrt>@;+6Dm%%bgw{P@Cg;%gvG9Ry|L!Y#{OcZQi97Crxi;jbi)#9QSURwQb{-i z3E4?wJNb+tn{Zizd=_uU`q~t3DsS?V@I`b+GE#RWDlvCip&_?4*&2bnxs(+Vvm$2s zVpLpt6kgiy3QxGSOrX=@)ig45F{hg<Thw6CwDC@{Vkj#Q9{Cy zU+vJi+sZ#h{EWu1$LV)4?5v$rS?jtR$){NP}&zpAy#7 zvx&B6h~AX~dRCB5V34<~Pf^c>8aJt32W#kl2fEjR)-Y)gbSvPXJp|p61rjW4AnmtV z=1I`pGkcyi!eMTG@8^K$3sE=pJUp%@;=TnHpO_9$DGf@V{5^%@w{pUas~BT&H#fxx zxZ?GEKyN&#N$qDo=dSp|WW45hreiAXiCB1=jmo~T4DhC;F2oK}fdox#+FZaVye&rD zPU)~UzEyTDE{(k=n;^RumU_)`xHU@3A1meOLW9h&uL%ungATzZycs21On)aXsH6}A+5wg@7WC{MYH`Dv(=wWkZL)cSM@z8Ja#jqSM!mg3 z^b5(H`5k<{QqJ%D)k!?r)5rfW>eC&W52;4fRARXYPs)mWX5Z6GO`xRP@5=~s=WK*G z>R+O(KHUCy^kn+{zo(wp#PJgKB>#UwJ*#SnBO2Pg&?a3O;p4R$4+&cj!(xuXu46DQ z5giddQQy-2nKD>^)_3yZ9CRhs$nGj-JP+>KzmO>h(^_M)uhYOP2f&9@6v0TCFCLevl!9te=;&d$3au5=T$B7Vw zOa4f65a)b`Mg9#|f7Bwm)P@t+$scMz$)F*#ye0i&TICRE15W+oJ5eWT7yoHq)1wEF zJSMqJ<35KSMlK<9W@QBFFTA4v?}_+LdhoN-u5Ut_zR@FguQ(c~qh>rGkg!*TJ^x8= zvJgHBephj_h7Sr*CFvWDcZI5lBnHO$i`?)~ksp&cW{=(pmUpw#*}U zB;}L_7z?56@rMdU^@4H6_siA?;_QL~9-3w6#i%n?qArkyPq3xNk31)|tTi#aXZzTs z^P&znGKa^7#vwg?dK7n&y#5x7nz$2XMN7A)a2g&bFx3J1Ge^RXS{+BJ#kg6$*6NF~ z@`|-6#TWhq*Uuc5R1KfQcq5G7*jvx!ZyQicMugE67hZvTRSTH?T-%T&t{CkJ@I`Im z0jN80uYsO*fv3(FEoI^FS$Q*yD7S7);m!Fo(VC>cT{s5Mj|8h{lGiM+#2}nWtY}Jp zwW4V`Lh`GMS&F2;oB8JIRR7q2NGYtMvxd%Y&5yl%p6*^_9L=+3tW7@H7imyJFy`FB zm~&@AyL8UEhhfg02}&ZJ^_E^uG@zd*y1=E0s(zy;$|^|D?9oGebWNf^qC=Wn_voeU z$P2Ke)u2sI?TAHAvO>#JkC;7j7Hq95&mbsFA+th~JWVXjd5bgMusnR3+ot5^>bV?d z%oYKj7&!P!$IVk*33jF8S{(5m(3(S&aSLpVV-#*h9+E&d*)5KsF0X`8x@i^9{$1VQ z3JJ1V?un$ioUI`~+l8Ho>xY5zMN7w<2RToDvXDXLtqU# z$M&Kj!1(CJgKasDh6AUekG!TI4G+Sb^dn;3+W3yq%D)~w(tfQ0e#c|o+I{bht&_)c z?e6v>gU7`m1EKp(vcRK!kwM?cviUo;al?b5*>xOhzh-x9SH;Y(6K1n^_olRX&x--| zN+pxt|HYCPuWzig?2O|lV>d#-7dC86x;tKGjcVhYSm>vncltqmKc4V00`w`QM@}KlQ(qh_;W?r54>uT8+X(z1Seb$ zXaP>R+}au7QuM%eJAW<>Z+~jh83`VJom~G~&8Y^p{3U&8o4f;8hS@ z<62|uWD3-6aBJh*O4}P9DE}JYK8z*T1;)kIu{ig3H~V&j=f#kEnamx2IxY6CmhHgv z77IT;JuVD*Uf*=Mz0uCMtKv{rENgeWwhh<{0S18Qi5A>7xXrNoB0o9p|Fn1hu}z$3 z9KSP3xKN-;Tei^f;}|7%AjIK!b`3QCfk`MCG;vTj=b(L2vkvGFBqu?w)e`=*1Gu?}HU# zuN7Z;qj84&_l;oI#?5;;l@#3lCzI=inNywpHs{9v*LPwLprOkHRmlIKckUP5{k;3g z(B(^vjkBL~K9;KX4!>vj&Pd(w#kiJ#3GImgP`t)p`VM-R2dA>ubY$r2z>j_of-~4v zi=D9f%5K*~=j96i>~5EMuDO!0N17}A_lk!~2KfW~k#a0;KOI|<#S7uZGHibEky%iB|K*73Y8_@sqP29jngfBds4zJIIpLdn@@ob%!^Ut~F zdnccB&pCwuRWBdp7fl-E9fK*G2eY1^DLalHWws4+6=x=S@mF`ek0M3oM=7i`);H~u zX&bnz*_FS%`10MQXSji$!6_TC9&hGM@tMSNAI5!nuV49o$_!mfJ%@hAl;;sntV^t$ z;@RUujW1)@FYALHkD!!|d(e-S9beVQReMf9I#`5761YW&E)^BMcH-TN;j4Rw=d9;n zyf=d1#|G{TOu{#qI;@qx>%?J~aDG8Vp80}{KQS}1CDJ@|Uf_My=$6P`M-IELoiC5H zSG?@H>&yd@c^+TIVOM$JoinqLt}QUtb6?=kDcgDcr8V$YDt(P2;~Jqr(Ft}PU*L@t zdu#bA{L^3`cVh6EaO{WP#Jb$DXZhkfY-8@Y%(iVMFPwz@6~eFLh3e@z*7JfH&yFhe zF32`WH)J0qHy+_lZtRNK$zrxEYHY#2gaymku3$XJW5Hx2C3Y-aGFd3)O3FTGNG4(} zzPD_Hux zr)x)=eURVm;{``j3pqMet!uliDUOgo7?-uMLrx@&h;Chts(P;@DnHp<8%ZSaZ8aWC z>dKCUCi<$xs2bF|LP<$7FCa;o9FnvFSsHvbqEqWku8|})rt0=;am#j9kBB~Tb-ujG zgepmG3H2c(6ViOm?{C}G>~E4|;i%&4O(-6vW8v2yXZK#fZdubE+75rPyQ4$V(w1{v zPh1V>E4Le|C8>3X%SheegSkl^=lr-rxJxc%b~<}IGQhG4=M(Hw>+0(zXvB4GOrQyv0h`){><*l844*;MT>VSx}x3Q_tlwD|tq@b;TJA zyU+z~SHpUwx?tavW2?;kS=!jKiZyK=<@zLPlzJ;R7q$~E+|gq+@R9ye)%0#TDvsGt zW}gX3QZy~D`9pFv8k9p%*jaf!H)EL+5~Gt~*wHLJDKtAW5j04z&}WB4%fGvTUgzqXRQ! z@X2FaHoLC%`gKi?CA-+mh$J;c<*s1ZtnYEN`ZC#CWHlO$?_f`MRrMNd0xU+if#_v}U-~G0S6)!SWuZ zDYyJ4#c|GPRFz{PMQn`+<1I4YJMq@3$X&I7*xKT56P=w(Oc#5W)z;N6SzK2eR(fie z*LDWwjup%5gB<7awu`OZG1fM@x+P1OEnl&6Rs9#8F1Kf`m(@e@WRB--obl3LYFouou#iNn`3^iypA(>;nYjY^MK;MK^6U>Y z5g-CYfCvzQnqGLT^Qm8_Pa}_v)_*Hg>L?ql%+5Gm>1aZ zADPha83Ccd#u%}=n5Gm+N6|!p2oM1xKm>>Y5g-CYfCvx)B0vO)01+SpM1Tko0U~f` z5MbYp*?i#B?u^@|yCeccfCvx)B0vO)01+SpM1Tko0U|&IhyW2F0z`la5P<>&*mvVs z@O$I*jKT#HQ#27E0z`la5CI}U1c(3;AOb{y2oM1xKm>>Y5g-CYfC&8G6KKG^9ude! z+$o#e;~c_Muk*rNIr>)Ui!A!WjBb2)-adMrH$?qU8@uN1vb_y9!Y8~S!eGYF)aP3C zITn4kMW1ESD=qr{89kRx9m)+rYI5SHBg|xP&!<}_xa4iMF;0vF+m7AgEOw!dyaw3~ z7Oah$`Ywz9LXQ41(kGxV&ZLhHr{@dGo&V<`&iw&$Zzi9q-(%6sGdimiR`vk$nCmB+ z`ooztroK9(vvn>5oK4sVJ>R;Qo3M^1qPU+u!-*Boa^lkvrr+w;)aGnz`j&J`ZSO1j v)6w!DfBu7}SEsArl@|qOR@OdUY5Vo65A0a?^UYa@IKJKMDVzSgqdWft5b+U3 literal 0 HcmV?d00001 diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml index 61946dff..3d15b2ee 100644 --- a/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml @@ -39,6 +39,7 @@ rmcs_executor: hero_hardware: ros__parameters: + dd: 7 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" diff --git a/rmcs_ws/src/rmcs_core/CMakeLists.txt b/rmcs_ws/src/rmcs_core/CMakeLists.txt index dd3c8579..c576aca1 100644 --- a/rmcs_ws/src/rmcs_core/CMakeLists.txt +++ b/rmcs_ws/src/rmcs_core/CMakeLists.txt @@ -17,8 +17,7 @@ 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 file:///workspaces/RMCS/develop_ws/librmcs-sdk-src-3.0.1-0.dev.4.gbaf538b.zip DOWNLOAD_EXTRACT_TIMESTAMP TRUE ) FetchContent_MakeAvailable(librmcs) 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 fbf85761..832e6e72 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 @@ -1,3 +1,7 @@ +#include +#include +#include + #include #include #include @@ -50,27 +54,27 @@ class PutterController last_preload_flag_ = false; - bullet_feeder_velocity_pid_.kp = 50.0; - bullet_feeder_velocity_pid_.ki = 10.0; + bullet_feeder_velocity_pid_.kp = 20.0; + bullet_feeder_velocity_pid_.ki = 5.0; bullet_feeder_velocity_pid_.kd = 0.0; - bullet_feeder_velocity_pid_.integral_max = 60.0; + bullet_feeder_velocity_pid_.integral_max = 40.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; - 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; + 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; - putter_velocity_pid_.integral_max = 0.03; - putter_velocity_pid_.integral_min = 0.; + 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; @@ -79,8 +83,16 @@ class PutterController 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/shoot/delay_ms", shoot_delay_ms_, nan_); register_output("/gimbal/shooter/mode", shoot_mode_, rmcs_msgs::ShootMode::SINGLE); + + initialize_shoot_delay_log(); + } + + ~PutterController() override { + if (shoot_delay_log_stream_.is_open()) + shoot_delay_log_stream_.close(); } void update() override { @@ -103,17 +115,20 @@ class PutterController if (bullet_feeder_cool_down_ > 0) { bullet_feeder_cool_down_--; - // 使用角度环反转到“后退一格”的位置以解除卡弹 - 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, Retrying..."); - set_preloading(); + // 冷却期前期：反转供弹轮以解除卡弹 + 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_) { @@ -131,23 +146,37 @@ class PutterController } } - if (shoot_stage_ == ShootStage::PRELOADING) { - // 盲拨模式：始终执行角度环定位 - if (std::isnan(bullet_feeder_control_angle_)) { - bullet_feeder_control_angle_ = - *bullet_feeder_angle_ + bullet_feeder_angle_per_bullet_; - last_preload_flag_ = true; - } + if (*photoelectric_sensor_status_) { + trigger = true; + } - const auto angle_err = bullet_feeder_control_angle_ - *bullet_feeder_angle_; - if (angle_err < 0.1) { - set_preloaded(); + 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 (trigger) { + RCLCPP_INFO(get_logger(), "Photoelectric Sensor Triggered"); + last_preload_flag_ = true; + bullet_feeder_control_angle_ = + *bullet_feeder_angle_ + bullet_feeder_angle_per_bullet_ * 0.5; + } else + *bullet_feeder_control_torque_ = + bullet_feeder_velocity_pid_.update( + low_latency_velocity_ - *bullet_feeder_velocity_) + * 0.5; } - 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 { // 其他状态：角度环保持角度不变防止弹链退弹 @@ -164,6 +193,7 @@ class PutterController if (*bullet_fired_ || *putter_angle_ - putter_startpoint >= putter_stroke_) { shooted = true; + trigger = false; } update_putter_jam_detection(); @@ -175,14 +205,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_ = - putter_return_velocity_pid_.update(60. - *putter_velocity_); + puttter_return_velocity_pid_.update(60. - *putter_velocity_); } } } else { @@ -196,10 +226,12 @@ class PutterController } } else { // 推杆未初始化：执行复位操作 - *putter_control_torque_ = putter_return_velocity_pid_.update(-80. - *putter_velocity_); + *putter_control_torque_ = puttter_return_velocity_pid_.update(-80. - *putter_velocity_); update_putter_jam_detection(); } + update_shoot_delay_measurement(*bullet_fired_); + // 保存当前状态用于下次比较 last_switch_right_ = switch_right; last_switch_left_ = switch_left; @@ -226,13 +258,15 @@ class PutterController putter_initialized = false; putter_startpoint = nan_; - putter_return_velocity_pid_.reset(); + 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; + shot_delay_pending_ = false; + last_bullet_fired_ = *bullet_fired_; bullet_feeder_faulty_count_ = 0; bullet_feeder_cool_down_ = 0; @@ -241,23 +275,67 @@ 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() { 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; + shoot_start_time_ = std::chrono::steady_clock::now(); + shot_delay_pending_ = true; + } + + void update_shoot_delay_measurement(bool bullet_fired_now) { + const bool bullet_fired_rising_edge = bullet_fired_now && !last_bullet_fired_; + + if (shot_delay_pending_ && bullet_fired_rising_edge) { + const auto now = std::chrono::steady_clock::now(); + const double delay_ms = + std::chrono::duration(now - shoot_start_time_).count(); + *shoot_delay_ms_ = delay_ms; + log_shoot_delay(delay_ms); + shot_delay_pending_ = false; + } + + // Shot ended without a bullet_fired edge: drop this sample. + if (shot_delay_pending_ && shoot_stage_ != ShootStage::SHOOTING) + shot_delay_pending_ = false; + + last_bullet_fired_ = bullet_fired_now; + } + + void initialize_shoot_delay_log() { + using namespace std::chrono; + const auto now = system_clock::now(); + const auto ms = duration_cast(now.time_since_epoch()).count(); + shoot_delay_log_path_ = "/robot_shoot/shoot_delay_" + std::to_string(ms) + ".log"; + shoot_delay_log_stream_.open(shoot_delay_log_path_); + + if (!shoot_delay_log_stream_.is_open()) { + RCLCPP_WARN( + get_logger(), "Failed to open shoot delay log file: %s", + shoot_delay_log_path_.c_str()); + return; + } + + shoot_delay_log_stream_ << "shot_index,delay_ms,timestamp_ms" << std::endl; + RCLCPP_INFO(get_logger(), "Shoot delay log file: %s", shoot_delay_log_path_.c_str()); + } + + void log_shoot_delay(double delay_ms) { + if (!shoot_delay_log_stream_.is_open()) + return; + + using namespace std::chrono; + const auto timestamp_ms = + duration_cast(system_clock::now().time_since_epoch()).count(); + shoot_delay_log_stream_ << shoot_delay_sample_count_++ << ',' << delay_ms << ',' + << timestamp_ms << std::endl; } void update_jam_detection() { @@ -272,8 +350,10 @@ class PutterController bullet_feeder_faulty_count_++; else { bullet_feeder_faulty_count_ = 0; - RCLCPP_WARN(get_logger(), "Jam Detected! Reversing 60 degrees..."); - enter_jam_protection(); + if (last_preload_flag_) + set_preloaded(); + else + enter_jam_protection(); } } @@ -302,8 +382,7 @@ class PutterController } void enter_jam_protection() { - // 设置目标角度为当前角度后退 60 度（一格） - bullet_feeder_control_angle_ = *bullet_feeder_angle_ - bullet_feeder_angle_per_bullet_; + bullet_feeder_control_angle_ = nan_; bullet_feeder_cool_down_ = 1000; bullet_feeder_angle_pid_.reset(); bullet_feeder_velocity_pid_.reset(); @@ -324,6 +403,7 @@ class PutterController InputInterface photoelectric_sensor_status_; bool last_photoelectric_sensor_status_; InputInterface bullet_fired_; + bool trigger{false}; bool shooted{false}; InputInterface friction_ready_; @@ -350,7 +430,7 @@ class PutterController bool putter_initialized = false; int putter_faulty_count_ = 0; double putter_startpoint = nan_; - pid::PidCalculator putter_return_velocity_pid_; + pid::PidCalculator puttter_return_velocity_pid_; InputInterface putter_velocity_; pid::PidCalculator putter_velocity_pid_; @@ -366,10 +446,19 @@ class PutterController InputInterface putter_angle_; pid::PidCalculator putter_return_angle_pid; OutputInterface putter_control_torque_; + OutputInterface shoot_delay_ms_; int bullet_feeder_faulty_count_ = 0; int bullet_feeder_cool_down_ = 0; + std::chrono::steady_clock::time_point shoot_start_time_{}; + bool shot_delay_pending_ = false; + bool last_bullet_fired_ = false; + + std::ofstream shoot_delay_log_stream_; + std::string shoot_delay_log_path_; + std::size_t shoot_delay_sample_count_ = 0; + OutputInterface shoot_mode_; }; @@ -377,4 +466,4 @@ class PutterController #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::shooting::PutterController, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::shooting::PutterController, rmcs_executor::Component) 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 4394bb8b..1144e734 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 @@ -85,7 +85,7 @@ class ShootingRecorder switch (log_mode_) { case LogMode::TRIGGER: // It will be triggered by shooting action - if (*shoot_timestamp_ == last_shoot_timestamp_ || v == *shoot_timestamp_) + if (*shoot_timestamp_ == last_shoot_timestamp_) return; break; case LogMode::TIMING: @@ -94,39 +94,39 @@ class ShootingRecorder return; break; } - v = *shoot_timestamp_; + // v = *shoot_timestamp_; - static constexpr size_t max_velocities_size = 1000; + // static constexpr size_t max_velocities_size = 1000; - velocities.push_back(*initial_speed_); - if (velocities.size() > max_velocities_size) { - velocities.erase(velocities.begin()); - } - - analysis3(); + // velocities.push_back(*initial_speed_); + // if (velocities.size() > max_velocities_size) { + // velocities.erase(velocities.begin()); + // } - auto log_text = std::string{}; - auto timestamp = timestamp_to_string(*shoot_timestamp_); + // analysis3(); - if (friction_wheel_count_ == 2) { - log_text = fmt::format( - "{},{},{:.3f},{:.3f},{:.3f},{:.3f},{:.3f},{:.3f},{:.3f}", *initial_speed_, - (int)velocities.size(), // - velocity_, excellence_rate_, pass_rate_, range_, range2_, velocity_max, - velocity_min); - } + // auto log_text = std::string{}; + // auto timestamp = timestamp_to_string(*shoot_timestamp_); + + // if (friction_wheel_count_ == 2) { + // log_text = fmt::format( + // "{},{},{:.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_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(); - } + // 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_; + // last_shoot_timestamp_ = *shoot_timestamp_; } private: 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 847698c2..24b5d3de 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp @@ -1,3 +1,4 @@ +#include #include #include #include @@ -165,6 +166,8 @@ class SteeringHero .set_reduction_ratio(1.) .enable_multi_turn_angle()); + dd = steering_hero.get_parameter("dd").as_int(); + steering_hero.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); steering_hero.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); @@ -262,6 +265,11 @@ class SteeringHero .can_id = 0x142, .can_data = gimbal_pitch_motor_.generate_command().as_bytes(), }); + + builder.gpio_digital_read({ + .channel = 7, + .falling_edge = true, + }); } private: @@ -296,6 +304,15 @@ class SteeringHero } } + void gpio_digital_read_result_callback( + const librmcs::data::GpioDigitalDataView& data) override { + *photoelectric_sensor_status_ = false; + if (data.channel == 7) { + *photoelectric_sensor_status_ = true; + RCLCPP_INFO(logger_, "trigger!"); + } + } + void accelerometer_receive_callback( const librmcs::data::AccelerometerDataView& data) override { imu_.store_accelerometer_status(data.x, data.y, data.z); @@ -307,6 +324,8 @@ class SteeringHero void putter_receive_callback(bool status) { *photoelectric_sensor_status_ = status; } + uint8_t dd; + rclcpp::Logger logger_; OutputInterface& tf_; @@ -324,6 +343,7 @@ class SteeringHero OutputInterface photoelectric_sensor_status_; OutputInterface camera_capturer_trigger_; OutputInterface camera_capturer_trigger_timestamp_; + std::atomic photoelectric_sensor_status_atomic{false}; }; class BottomBoard_one final : private librmcs::agent::CBoard { @@ -388,8 +408,7 @@ 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); + 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; From cf68ff78146e31926194c3840717543a01effb9b Mon Sep 17 00:00:00 2001 From: zhzy-star <2807406212@qq.com> Date: Thu, 19 Mar 2026 06:08:13 +0800 Subject: [PATCH 09/32] Fixed dual_yaw; add new putter_controller. --- .../rmcs_bringup/config/steering-hero.yaml | 5 - .../controller/gimbal/dual_yaw_controller.cpp | 3 + .../gimbal/hero_gimbal_controller.cpp | 10 +- .../controller/shooting/putter_controller.cpp | 251 ++++++++++-------- .../rmcs_core/src/hardware/steering-hero.cpp | 4 - 5 files changed, 141 insertions(+), 132 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml index 3d15b2ee..3ebcf314 100644 --- a/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml @@ -39,7 +39,6 @@ rmcs_executor: hero_hardware: ros__parameters: - dd: 7 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" @@ -52,10 +51,6 @@ hero_hardware: 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 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 20c09236..90ae2576 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 @@ -44,6 +44,9 @@ 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_angle_, nan_); + register_output( + "/gimbal/bottom_yaw/control_angle_shift", bottom_yaw_control_angle_shift_, nan_); status_component_ = create_partner_component(get_component_name() + "_status"); 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 1c6dde8b..40aba208 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,9 +65,9 @@ class HeroGimbalController else gimbal_mode_keyboard_ = GimbalMode::IMU; } - // *gimbal_mode_ = - // *switch_right_ == Switch::UP ? GimbalMode::ENCODER : gimbal_mode_keyboard_; - *gimbal_mode_ = 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(); @@ -122,7 +122,7 @@ class HeroGimbalController double yaw_shift = joystick_sensitivity * joystick_left_->y() + mouse_sensitivity * mouse_velocity_->y(); double pitch_shift = - -joystick_sensitivity * joystick_left_->x() - mouse_sensitivity * mouse_velocity_->x(); + -joystick_sensitivity * joystick_left_->x() + mouse_sensitivity * mouse_velocity_->x(); return imu_gimbal_solver.update( TwoAxisGimbalSolver::SetControlShift{yaw_shift, pitch_shift}); @@ -139,7 +139,7 @@ class HeroGimbalController double yaw_shift = joystick_sensitivity * joystick_left_->y() + mouse_yaw_sensitivity * mouse_velocity_->y(); double pitch_shift = -joystick_sensitivity * joystick_left_->x() - - mouse_pitch_sensitivity * mouse_velocity_->x(); + + mouse_pitch_sensitivity * mouse_velocity_->x(); return encoder_gimbal_solver.update( PreciseTwoAxisGimbalSolver::SetControlShift{yaw_shift, pitch_shift}); 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 832e6e72..02ab2dd8 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 @@ -1,4 +1,5 @@ #include +#include #include #include @@ -40,6 +41,7 @@ class PutterController register_input("/gimbal/friction_ready", friction_ready_); register_input("/gimbal/bullet_feeder/angle", bullet_feeder_angle_); + register_input("/gimbal/bullet_feeder/torque", bullet_feeder_torque_); register_input("/gimbal/bullet_feeder/velocity", bullet_feeder_velocity_); register_input( @@ -54,27 +56,27 @@ class PutterController last_preload_flag_ = false; - bullet_feeder_velocity_pid_.kp = 20.0; - bullet_feeder_velocity_pid_.ki = 5.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 = 40.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_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; @@ -83,14 +85,14 @@ class PutterController 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/shoot/delay_ms", shoot_delay_ms_, nan_); register_output("/gimbal/shooter/mode", shoot_mode_, rmcs_msgs::ShootMode::SINGLE); + register_output("/gimbal/shoot/delay_ms", shoot_delay_ms_, nan_); initialize_shoot_delay_log(); } - ~PutterController() override { + ~PutterController() { if (shoot_delay_log_stream_.is_open()) shoot_delay_log_stream_.close(); } @@ -100,12 +102,14 @@ class PutterController const auto switch_left = *switch_left_; const auto mouse = *mouse_; const auto keyboard = *keyboard_; + const bool bullet_fired_now = *bullet_fired_; using namespace rmcs_msgs; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { reset_all_controls(); + update_shoot_delay_measurement(bullet_fired_now); return; } @@ -115,20 +119,16 @@ 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_) { @@ -145,38 +145,27 @@ class PutterController set_shooting(); } } - - if (*photoelectric_sensor_status_) { - trigger = true; + if (shoot_stage_ == ShootStage::UPDATING) { + wait_bullet_ready(); } 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 (trigger) { - RCLCPP_INFO(get_logger(), "Photoelectric Sensor Triggered"); - last_preload_flag_ = true; - bullet_feeder_control_angle_ = - *bullet_feeder_angle_ + bullet_feeder_angle_per_bullet_ * 0.5; - } else - *bullet_feeder_control_torque_ = - bullet_feeder_velocity_pid_.update( - low_latency_velocity_ - *bullet_feeder_velocity_) - * 0.5; + // 盲拨模式：始终执行角度环定位 + 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 { // 其他状态：角度环保持角度不变防止弹链退弹 @@ -193,7 +182,6 @@ class PutterController if (*bullet_fired_ || *putter_angle_ - putter_startpoint >= putter_stroke_) { shooted = true; - trigger = false; } update_putter_jam_detection(); @@ -203,16 +191,16 @@ class PutterController const auto angle_err = putter_startpoint - *putter_angle_; if (angle_err > -0.05) { *putter_control_torque_ = 0.; - set_preloading(); + set_updating(); 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 { @@ -226,11 +214,11 @@ 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(); } - update_shoot_delay_measurement(*bullet_fired_); + update_shoot_delay_measurement(bullet_fired_now); // 保存当前状态用于下次比较 last_switch_right_ = switch_right; @@ -258,23 +246,29 @@ 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_; last_preload_flag_ = false; last_photoelectric_sensor_status_ = false; - shot_delay_pending_ = false; - last_bullet_fired_ = *bullet_fired_; bullet_feeder_faulty_count_ = 0; bullet_feeder_cool_down_ = 0; + shot_delay_pending_ = false; + last_bullet_fired_ = false; + *shoot_delay_ms_ = nan_; } 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() { @@ -290,6 +284,55 @@ class PutterController shot_delay_pending_ = true; } + void set_updating() { + RCLCPP_INFO(get_logger(), "UPDATING"); + shoot_stage_ = ShootStage::UPDATING; + } + + void update_jam_detection() { + // RCLCPP_INFO(get_logger(), "%.2f --", *bullet_feeder_control_torque_); + if (*bullet_feeder_torque_ > 100 && *bullet_feeder_velocity_ < 0.1) { + bullet_feeder_faulty_count_++; + } else { + bullet_feeder_faulty_count_ = 0; + } + if (bullet_feeder_faulty_count_ >= 1000) + 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() { + // 设置目标角度为当前角度后退 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(); + RCLCPP_INFO(get_logger(), "Jammed!"); + } + void update_shoot_delay_measurement(bool bullet_fired_now) { const bool bullet_fired_rising_edge = bullet_fired_now && !last_bullet_fired_; @@ -298,6 +341,8 @@ class PutterController const double delay_ms = std::chrono::duration(now - shoot_start_time_).count(); *shoot_delay_ms_ = delay_ms; + RCLCPP_INFO( + get_logger(), "Shoot delay[%zu]: %.3f ms", shoot_delay_sample_count_, delay_ms); log_shoot_delay(delay_ms); shot_delay_pending_ = false; } @@ -311,9 +356,18 @@ class PutterController void initialize_shoot_delay_log() { using namespace std::chrono; + const std::filesystem::path log_dir{"/robot_shoot"}; + std::error_code ec; + std::filesystem::create_directories(log_dir, ec); + if (ec) { + RCLCPP_WARN( + get_logger(), "Failed to create shoot delay log directory %s: %s", log_dir.c_str(), + ec.message().c_str()); + } + const auto now = system_clock::now(); const auto ms = duration_cast(now.time_since_epoch()).count(); - shoot_delay_log_path_ = "/robot_shoot/shoot_delay_" + std::to_string(ms) + ".log"; + shoot_delay_log_path_ = (log_dir / ("shoot_delay_" + std::to_string(ms) + ".log")).string(); shoot_delay_log_stream_.open(shoot_delay_log_path_); if (!shoot_delay_log_stream_.is_open()) { @@ -336,59 +390,18 @@ class PutterController duration_cast(system_clock::now().time_since_epoch()).count(); shoot_delay_log_stream_ << shoot_delay_sample_count_++ << ',' << delay_ms << ',' << timestamp_ms << std::endl; + shoot_delay_log_stream_.flush(); } - 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 wait_bullet_ready() { + if (bullet_ready_count_ >= 600) { + bullet_ready_count_ = 0; + set_preloading(); + } else { + bullet_ready_count_++; } } - 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(); ///< 无穷大常量 @@ -403,7 +416,6 @@ class PutterController InputInterface photoelectric_sensor_status_; bool last_photoelectric_sensor_status_; InputInterface bullet_fired_; - bool trigger{false}; bool shooted{false}; InputInterface friction_ready_; @@ -421,6 +433,7 @@ class PutterController bool overdrive_mode_ = false; InputInterface bullet_feeder_angle_; + InputInterface bullet_feeder_torque_; InputInterface bullet_feeder_velocity_; InputInterface control_bullet_allowance_limited_by_heat_; @@ -430,13 +443,13 @@ 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_; - enum class ShootStage { PRELOADING, PRELOADED, SHOOTING }; - ShootStage shoot_stage_ = ShootStage::PRELOADING; + enum class ShootStage { PRELOADING, PRELOADED, SHOOTING, UPDATING }; + ShootStage shoot_stage_ = ShootStage::PRELOADED; double bullet_feeder_control_angle_ = nan_; pid::PidCalculator bullet_feeder_velocity_pid_; @@ -446,10 +459,12 @@ class PutterController InputInterface putter_angle_; pid::PidCalculator putter_return_angle_pid; OutputInterface putter_control_torque_; - OutputInterface shoot_delay_ms_; int bullet_feeder_faulty_count_ = 0; int bullet_feeder_cool_down_ = 0; + int bullet_ready_count_ = 0; + + OutputInterface shoot_delay_ms_; std::chrono::steady_clock::time_point shoot_start_time_{}; bool shot_delay_pending_ = false; 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 24b5d3de..976be1d2 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp @@ -166,8 +166,6 @@ class SteeringHero .set_reduction_ratio(1.) .enable_multi_turn_angle()); - dd = steering_hero.get_parameter("dd").as_int(); - steering_hero.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); steering_hero.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); @@ -324,8 +322,6 @@ class SteeringHero void putter_receive_callback(bool status) { *photoelectric_sensor_status_ = status; } - uint8_t dd; - rclcpp::Logger logger_; OutputInterface& tf_; From eafda5f05f5c130bd7e3b06a8732785dc9d62111 Mon Sep 17 00:00:00 2001 From: zhzy-star <2807406212@qq.com> Date: Sat, 21 Mar 2026 18:01:18 +0800 Subject: [PATCH 10/32] dd --- .../shooting/friction_wheel_controller.cpp | 2 +- .../controller/shooting/putter_controller.cpp | 14 ++--- .../controller/shooting/shooting_recorder.cpp | 54 +++++++++---------- 3 files changed, 35 insertions(+), 35 deletions(-) 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 e38437a4..879d9e53 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 @@ -169,7 +169,7 @@ class FrictionWheelController primary_friction_velocity_decrease_integral_ += differential; else { if (primary_friction_velocity_decrease_integral_ < -14.0 - && last_primary_friction_velocity_ < friction_working_velocities_[0] - 20.0) + && last_primary_friction_velocity_ < friction_working_velocities_[0] - 25.0) fired = true; primary_friction_velocity_decrease_integral_ = 0; 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 02ab2dd8..75032695 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 @@ -127,7 +127,7 @@ class PutterController if (!bullet_feeder_cool_down_) { RCLCPP_INFO(get_logger(), "Jamming Solved, Retrying..."); - set_preloading(); + set_preloaded(); } } else { // 正常运行模式：摩擦轮就绪时才允许发射 @@ -262,7 +262,7 @@ class PutterController } void set_preloading() { - RCLCPP_INFO(get_logger(), "PRELOADING"); + // RCLCPP_INFO(get_logger(), "PRELOADING"); shoot_stage_ = ShootStage::PRELOADING; // 盲拨方案：直接增加目标角度 if (!std::isnan(bullet_feeder_control_angle_)) { @@ -272,20 +272,20 @@ class PutterController } void set_preloaded() { - RCLCPP_INFO(get_logger(), "PRELOADED"); + // RCLCPP_INFO(get_logger(), "PRELOADED"); shoot_stage_ = ShootStage::PRELOADED; last_preload_flag_ = false; } void set_shooting() { - RCLCPP_INFO(get_logger(), "SHOOTING"); + // RCLCPP_INFO(get_logger(), "SHOOTING"); shoot_stage_ = ShootStage::SHOOTING; shoot_start_time_ = std::chrono::steady_clock::now(); shot_delay_pending_ = true; } void set_updating() { - RCLCPP_INFO(get_logger(), "UPDATING"); + // RCLCPP_INFO(get_logger(), "UPDATING"); shoot_stage_ = ShootStage::UPDATING; } @@ -341,8 +341,8 @@ class PutterController const double delay_ms = std::chrono::duration(now - shoot_start_time_).count(); *shoot_delay_ms_ = delay_ms; - RCLCPP_INFO( - get_logger(), "Shoot delay[%zu]: %.3f ms", shoot_delay_sample_count_, delay_ms); + // RCLCPP_INFO( + // get_logger(), "Shoot delay[%zu]: %.3f ms", shoot_delay_sample_count_, delay_ms); log_shoot_delay(delay_ms); shot_delay_pending_ = false; } 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 1144e734..1e2039cd 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 @@ -81,7 +81,7 @@ class ShootingRecorder // if (outFile.is_open()) { // outFile << log_text << std::endl; // outFile.close(); - // } + // } 评估摩擦轮质量 switch (log_mode_) { case LogMode::TRIGGER: // It will be triggered by shooting action @@ -94,39 +94,39 @@ class ShootingRecorder return; break; } - // v = *shoot_timestamp_; + v = *shoot_timestamp_; - // static constexpr size_t max_velocities_size = 1000; + static constexpr size_t max_velocities_size = 1000; - // velocities.push_back(*initial_speed_); - // if (velocities.size() > max_velocities_size) { - // velocities.erase(velocities.begin()); - // } + velocities.push_back(*initial_speed_); + if (velocities.size() > max_velocities_size) { + velocities.erase(velocities.begin()); + } - // analysis3(); + analysis3(); - // auto log_text = std::string{}; - // auto timestamp = timestamp_to_string(*shoot_timestamp_); - - // if (friction_wheel_count_ == 2) { - // log_text = fmt::format( - // "{},{},{:.3f},{:.3f},{:.3f},{:.3f},{:.3f},{:.3f},{:.3f}", *initial_speed_, - // (int)velocities.size(), // - // velocity_, excellence_rate_, pass_rate_, range_, range2_, velocity_max, - // velocity_min); - // } + auto log_text = std::string{}; + auto timestamp = timestamp_to_string(*shoot_timestamp_); - // log_stream_ << log_text << std::endl; - // RCLCPP_INFO(get_logger(), "%s", log_text.c_str()); + if (friction_wheel_count_ == 4) { + log_text = fmt::format( + "{},{},{:.3f},{:.3f},{:.3f},{:.3f},{:.3f},{:.3f},{:.3f}", *initial_speed_, + (int)velocities.size(), // + velocity_, excellence_rate_, pass_rate_, range_, range2_, velocity_max, + velocity_min); + } - // 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(); - // } + 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_; + last_shoot_timestamp_ = *shoot_timestamp_; } private: From 8eb58c31755f93d4f3cbf3f6fd3df902d2785da6 Mon Sep 17 00:00:00 2001 From: zhzy-star <2807406212@qq.com> Date: Mon, 23 Mar 2026 20:24:24 +0800 Subject: [PATCH 11/32] fixed putter controller --- .../rmcs_bringup/config/steering-hero.yaml | 95 +- rmcs_ws/src/rmcs_bringup/config/test.yaml | 58 + rmcs_ws/src/rmcs_core/CMakeLists.txt | 3 +- rmcs_ws/src/rmcs_core/plugins.xml | 5 +- .../pid/friction_wheel_pid_recorder.cpp | 166 +++ .../shooting/friction_wheel_controller.cpp | 3 + .../controller/shooting/putter_controller.cpp | 81 +- .../controller/shooting/shooting_recorder.cpp | 20 +- .../rmcs_core/src/hardware/steering-hero.cpp | 10 +- tools/pid_tuning/README.md | 134 ++ .../friction_wheel_pid_recorder.example.yaml | 15 + tools/pid_tuning/identify_plant.py | 724 ++++++++++ tools/pid_tuning/tune_pid.py | 1280 +++++++++++++++++ 13 files changed, 2518 insertions(+), 76 deletions(-) create mode 100644 rmcs_ws/src/rmcs_bringup/config/test.yaml create mode 100644 rmcs_ws/src/rmcs_core/src/controller/pid/friction_wheel_pid_recorder.cpp create mode 100644 tools/pid_tuning/README.md create mode 100644 tools/pid_tuning/friction_wheel_pid_recorder.example.yaml create mode 100644 tools/pid_tuning/identify_plant.py create mode 100644 tools/pid_tuning/tune_pid.py diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml index 3ebcf314..d58c5745 100644 --- a/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml @@ -25,7 +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::shooting::ShootingRecorder -> shooting_recorder - rmcs_core::controller::chassis::ChassisController -> chassis_controller - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller @@ -43,14 +43,15 @@ hero_hardware: 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: 52508 - pitch_motor_zero_point: 63917 + pitch_motor_zero_point: 40651 #35701 top_yaw_motor_zero_point: 58744 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 + bullet_feeder_motor_zero_point: 15111 + left_front_zero_point: 4516 + right_front_zero_point: 7190 + left_back_zero_point: 3841 + right_back_zero_point: 5159 # left_front_zero_point: 3473 # left_back_zero_point: 6124 # right_back_zero_point: 6157 @@ -59,24 +60,35 @@ hero_hardware: 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/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 + - /gimbal/putter/velocity + - /gimbal/first_left_friction/velocity + - /gimbal/first_second_friction/velocity + - /gimbal/second_left_friction/velocity + - /gimbal/second_right_friction/velocity + - /gimbal/first_left_friction/control_torque + - /gimbal/first_second_friction/control_torque + - /gimbal/second_left_friction/control_torque + - /gimbal/second_right_friction/control_torque + # - /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/pitch/control_torque + # - /gimbal/pitch/velocity + # - /gimbal/pitch/control_velocity + # - /gimbal/pitch/control_angle_error + # - /gimbal/bullet_feeder/phase_zero + # - /gimbal/bullet_feeder/alignment_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: @@ -136,7 +148,7 @@ pitch_angle_pid_controller: ros__parameters: measurement: /gimbal/pitch/control_angle_error control: /gimbal/pitch/control_velocity - kp: 28.0 #28.00 + kp: 30.0 #28.00 ki: 0.0 kd: 0.6 #0.6 @@ -145,9 +157,9 @@ pitch_velocity_pid_controller: measurement: /gimbal/pitch/velocity_imu setpoint: /gimbal/pitch/control_velocity control: /gimbal/pitch/control_torque - kp: 45.0 #45.00 + kp: 15.0 #45.00 ki: 0.00 - kd: 1.0 #1.00 + kd: 2.0 #1.00 gimbal_player_viewer_controller: ros__parameters: @@ -170,10 +182,10 @@ friction_wheel_controller: - /gimbal/second_left_friction - /gimbal/second_right_friction friction_velocities: - - 450.00 - - 450.00 - - 535.00 - - 535.00 + - 375.00 + - 375.00 + - 520.00 + - 520.00 friction_soft_start_stop_time: 1.0 heat_controller: @@ -183,44 +195,45 @@ heat_controller: shooting_recorder: ros__parameters: - friction_wheel_count: 2 - log_mode: 2 # 1: trigger, 2: timing + friction_wheel_count: 4 + aim_velocity: 11.8 + log_mode: 1 # 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 + kp: 0.0016 ki: 0.00 - kd: 0.009373434 + kd: 0.001 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 + kp: 0.0016 ki: 0.00 - kd: 0.009373434 + kd: 0.001 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 + kp: 0.001 ki: 0.00 - kd: 0.009373434 + kd: 0.001 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 + kp: 0.001 ki: 0.00 - kd: 0.009373434 + kd: 0.00 steering_wheel_controller: ros__parameters: diff --git a/rmcs_ws/src/rmcs_bringup/config/test.yaml b/rmcs_ws/src/rmcs_bringup/config/test.yaml new file mode 100644 index 00000000..ebf295ae --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/test.yaml @@ -0,0 +1,58 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::SteeringHero -> hero_hardware + # - rmcs_core::controller::pid::PidController -> test_pid_controller + + # - 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::pid::PidController -> pitch_velocity_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::shooting::ShootingRecorder -> shooting_recorder + + # - 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: + board_serial_bottom_board_two: "D4-3674-7174-8768-879E-E44A-3931" + test: 6.0 + bottom_yaw_motor_zero_point: 52508 + pitch_motor_zero_point: 35701 + top_yaw_motor_zero_point: 58744 + viewer_motor_zero_point: 3030 + external_imu_port: /dev/ttyUSB0 + bullet_feeder_motor_zero_point: 15111 + left_front_zero_point: 4433 + right_front_zero_point: 7177 + left_back_zero_point: 6919 + right_back_zero_point: 5120 + # left_front_zero_point: 3473 + # left_back_zero_point: 6124 + # right_back_zero_point: 6157 + # right_front_zero_point: 2723 diff --git a/rmcs_ws/src/rmcs_core/CMakeLists.txt b/rmcs_ws/src/rmcs_core/CMakeLists.txt index c576aca1..579561c3 100644 --- a/rmcs_ws/src/rmcs_core/CMakeLists.txt +++ b/rmcs_ws/src/rmcs_core/CMakeLists.txt @@ -17,7 +17,8 @@ include(FetchContent) set(BUILD_STATIC_LIBRMCS ON CACHE BOOL "Build static librmcs SDK" FORCE) FetchContent_Declare( librmcs - URL file:///workspaces/RMCS/develop_ws/librmcs-sdk-src-3.0.1-0.dev.4.gbaf538b.zip + URL https://github.com/Alliance-Algorithm/librmcs/releases/download/v3.1.0b1/librmcs-sdk-src-3.1.0-beta.1.zip + URL_HASH SHA256=ec68a8b0e9441bd9e35b859139b3875c559944f7c0ecb4a1539bcdc75442fec4 DOWNLOAD_EXTRACT_TIMESTAMP TRUE ) FetchContent_MakeAvailable(librmcs) diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 9df7668b..4ab6d82b 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -62,6 +62,9 @@ Test plugin. + + Recorder for friction wheel PID tuning. + Test plugin. @@ -125,4 +128,4 @@ Test plugin. - \ No newline at end of file + diff --git a/rmcs_ws/src/rmcs_core/src/controller/pid/friction_wheel_pid_recorder.cpp b/rmcs_ws/src/rmcs_core/src/controller/pid/friction_wheel_pid_recorder.cpp new file mode 100644 index 00000000..366f66b6 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/pid/friction_wheel_pid_recorder.cpp @@ -0,0 +1,166 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace rmcs_core::controller::pid { + +class FrictionWheelPidRecorder + : public rmcs_executor::Component + , public rclcpp::Node { +public: + FrictionWheelPidRecorder() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { + const auto wheels = + declare_parameter>("wheels", std::vector{}); + if (wheels.empty()) + throw std::runtime_error("Parameter \"wheels\" must not be empty"); + + min_setpoint_abs_ = declare_parameter("min_setpoint_abs", 10.0); + + const auto flush_every_n_samples = declare_parameter("flush_every_n_samples", 100); + if (flush_every_n_samples <= 0) + throw std::runtime_error("Parameter \"flush_every_n_samples\" must be positive"); + flush_every_n_samples_ = static_cast(flush_every_n_samples); + + const auto output_dir = std::filesystem::path{ + declare_parameter("output_dir", "/tmp/friction_pid_logs")}; + const auto output_name = declare_parameter("output_name", ""); + + wheel_count_ = wheels.size(); + wheel_ids_ = std::make_unique(wheel_count_); + setpoints_ = std::make_unique[]>(wheel_count_); + measurements_ = std::make_unique[]>(wheel_count_); + control_torques_ = std::make_unique[]>(wheel_count_); + run_ids_ = std::make_unique(wheel_count_); + sample_indices_ = std::make_unique(wheel_count_); + previously_enabled_ = std::make_unique(wheel_count_); + + for (size_t i = 0; i < wheel_count_; ++i) { + wheel_ids_[i] = wheel_id_from_topic(wheels[i]); + register_input(wheels[i] + "/control_velocity", setpoints_[i]); + register_input(wheels[i] + "/velocity", measurements_[i]); + register_input(wheels[i] + "/control_torque", control_torques_[i]); + + run_ids_[i] = 0; + sample_indices_[i] = 0; + previously_enabled_[i] = false; + } + + std::filesystem::create_directories(output_dir); + log_file_path_ = output_dir / resolve_output_name(output_name); + log_stream_.open(log_file_path_); + if (!log_stream_.is_open()) + throw std::runtime_error( + "Failed to open recorder output file: " + log_file_path_.string()); + + log_stream_.setf(std::ios::fixed); + log_stream_.precision(9); + log_stream_ + << "timestamp_us,wheel_id,run_id,sample_idx,setpoint_velocity,measured_velocity," + "control_torque,enabled\n"; + + RCLCPP_INFO(get_logger(), "FrictionWheelPidRecorder writing to %s", log_file_path_.c_str()); + } + + ~FrictionWheelPidRecorder() override { + if (log_stream_.is_open()) { + log_stream_.flush(); + log_stream_.close(); + } + } + + void update() override { + const auto timestamp_us = now_timestamp_us(); + + for (size_t i = 0; i < wheel_count_; ++i) { + const bool enabled = should_record(i); + if (enabled && !previously_enabled_[i]) { + ++run_ids_[i]; + sample_indices_[i] = 0; + } + + if (enabled) { + log_stream_ << timestamp_us << ',' << wheel_ids_[i] << ',' << run_ids_[i] << ',' + << sample_indices_[i]++ << ',' << *setpoints_[i] << ',' + << *measurements_[i] << ',' << *control_torques_[i] << ",1\n"; + if (++unflushed_samples_ >= flush_every_n_samples_) { + log_stream_.flush(); + unflushed_samples_ = 0; + } + } + + previously_enabled_[i] = enabled; + } + } + +private: + static std::string wheel_id_from_topic(std::string_view topic) { + auto pos = topic.find_last_of('/'); + if (pos == std::string_view::npos || pos + 1 >= topic.size()) + return std::string(topic); + return std::string(topic.substr(pos + 1)); + } + + static uint64_t now_timestamp_us() { + const auto now = std::chrono::steady_clock::now().time_since_epoch(); + return static_cast( + std::chrono::duration_cast(now).count()); + } + + static std::string resolve_output_name(const std::string& configured_name) { + if (!configured_name.empty()) + return configured_name; + + const auto now = std::chrono::system_clock::now().time_since_epoch(); + const auto ms = std::chrono::duration_cast(now).count(); + return "friction_wheel_pid_" + std::to_string(ms) + ".csv"; + } + + bool should_record(size_t index) const { + if (!setpoints_[index].ready() || !measurements_[index].ready() + || !control_torques_[index].ready()) + return false; + + const auto setpoint = *setpoints_[index]; + const auto measurement = *measurements_[index]; + const auto control_torque = *control_torques_[index]; + + return std::isfinite(setpoint) && std::isfinite(measurement) + && std::isfinite(control_torque) && std::abs(setpoint) >= min_setpoint_abs_; + } + + size_t wheel_count_ = 0; + double min_setpoint_abs_ = 10.0; + size_t flush_every_n_samples_ = 100; + size_t unflushed_samples_ = 0; + + std::filesystem::path log_file_path_; + std::ofstream log_stream_; + + std::unique_ptr wheel_ids_; + std::unique_ptr[]> setpoints_; + std::unique_ptr[]> measurements_; + std::unique_ptr[]> control_torques_; + std::unique_ptr run_ids_; + std::unique_ptr sample_indices_; + std::unique_ptr previously_enabled_; +}; + +} // namespace rmcs_core::controller::pid + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::pid::FrictionWheelPidRecorder, rmcs_executor::Component) 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 879d9e53..5aecea6b 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 @@ -90,6 +90,9 @@ class FrictionWheelController last_switch_left_ = switch_left; last_keyboard_ = keyboard; } + if (!friction_enabled_) { + reset_all_controls(); + } } private: 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 75032695..b1b621c1 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 @@ -1,9 +1,12 @@ #include + #include #include + #include #include + #include #include #include @@ -89,12 +92,12 @@ class PutterController register_output("/gimbal/shooter/mode", shoot_mode_, rmcs_msgs::ShootMode::SINGLE); register_output("/gimbal/shoot/delay_ms", shoot_delay_ms_, nan_); - initialize_shoot_delay_log(); + // initialize_shoot_delay_log(); } ~PutterController() { - if (shoot_delay_log_stream_.is_open()) - shoot_delay_log_stream_.close(); + /* if (shoot_delay_log_stream_.is_open()) + shoot_delay_log_stream_.close(); */ } void update() override { @@ -102,19 +105,20 @@ class PutterController const auto switch_left = *switch_left_; const auto mouse = *mouse_; const auto keyboard = *keyboard_; - const bool bullet_fired_now = *bullet_fired_; + // const bool bullet_fired_now = *bullet_fired_; using namespace rmcs_msgs; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { reset_all_controls(); - update_shoot_delay_measurement(bullet_fired_now); + // update_shoot_delay_measurement(bullet_fired_now); return; } // 推杆已初始化后的正常控制流程 if (putter_initialized) { + // 供弹轮卡弹保护冷却期间的处理 if (bullet_feeder_cool_down_ > 0) { bullet_feeder_cool_down_--; @@ -126,8 +130,18 @@ class PutterController *bullet_feeder_control_torque_ = bullet_feeder_velocity_pid_.update(velocity_err); if (!bullet_feeder_cool_down_) { - RCLCPP_INFO(get_logger(), "Jamming Solved, Retrying..."); - set_preloaded(); + if (*photoelectric_sensor_status_) { + RCLCPP_INFO( + get_logger(), "Photoelectric Sensor Triggered during Jam Protection"); + *bullet_feeder_control_torque_ = 0.; + bullet_feeder_cool_down_ = 0; + // 认为是满链状态，直接设置为预装弹完成 + set_preloaded(); + } else { + RCLCPP_INFO(get_logger(), "Jamming Solved, Retrying..."); + set_preloading(); + // 认为是卡弹状态，继续保持在后退位置直到冷却结束 + } } } else { // 正常运行模式：摩擦轮就绪时才允许发射 @@ -167,6 +181,13 @@ class PutterController bullet_feeder_velocity_pid_.update(velocity_err); update_jam_detection(); + } else if (shoot_stage_ == ShootStage::SHOOTING) { + // 发射状态：供弹盘不发力 + if (!last_shoot_flag_) { + bullet_feeder_control_angle_ = *bullet_feeder_angle_; + last_shoot_flag_ = true; + } + *bullet_feeder_control_torque_ = 0; } else { // 其他状态：角度环保持角度不变防止弹链退弹 double velocity_err = @@ -179,8 +200,10 @@ class PutterController if (shoot_stage_ == ShootStage::SHOOTING) { // 发射状态：检测子弹是否发出 - if (*bullet_fired_ - || *putter_angle_ - putter_startpoint >= putter_stroke_) { + if (*putter_angle_ - putter_startpoint >= putter_stroke_) { + if (!shooted){ + RCLCPP_INFO(get_logger(),"%f", *putter_angle_); + } shooted = true; } @@ -193,6 +216,7 @@ class PutterController *putter_control_torque_ = 0.; set_updating(); shooted = false; + last_shoot_flag_ = false; } else { *putter_control_torque_ = putter_return_velocity_pid_.update(-80. - *putter_velocity_); @@ -218,7 +242,7 @@ class PutterController update_putter_jam_detection(); } - update_shoot_delay_measurement(bullet_fired_now); + // update_shoot_delay_measurement(bullet_fired_now); // 保存当前状态用于下次比较 last_switch_right_ = switch_right; @@ -233,6 +257,7 @@ class PutterController last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; last_mouse_ = rmcs_msgs::Mouse::zero(); last_keyboard_ = rmcs_msgs::Keyboard::zero(); + bullet_feeder_reset(); overdrive_mode_ = false; @@ -252,7 +277,6 @@ class PutterController *putter_control_torque_ = nan_; last_preload_flag_ = false; - last_photoelectric_sensor_status_ = false; bullet_feeder_faulty_count_ = 0; bullet_feeder_cool_down_ = 0; @@ -302,7 +326,8 @@ class PutterController void update_putter_jam_detection() { if ((*putter_control_torque_ > -0.03 && shoot_stage_ == ShootStage::PRELOADING) - || (*putter_control_torque_ < 0.05 && shoot_stage_ == ShootStage::SHOOTING) + || ((*putter_control_torque_ < 0.08) + && shoot_stage_ == ShootStage::SHOOTING) || std::isnan(*putter_control_torque_)) { putter_faulty_count_ = 0; return; @@ -319,11 +344,21 @@ class PutterController putter_startpoint = *putter_angle_; } else { // 发射状态下检测到堵转：认为子弹已发出 + RCLCPP_INFO(get_logger(),"dd"); shooted = true; } } } + void wait_bullet_ready() { + if (bullet_ready_count_ >= 600) { + bullet_ready_count_ = 0; + set_preloading(); + } else { + bullet_ready_count_++; + } + } + void enter_jam_protection() { // 设置目标角度为当前角度后退 60 度（一格） bullet_feeder_control_angle_ = *bullet_feeder_angle_ - bullet_feeder_angle_per_bullet_; @@ -333,6 +368,16 @@ class PutterController RCLCPP_INFO(get_logger(), "Jammed!"); } + void bullet_feeder_reset() {} + + static double normalize_feeder_phase(double angle) { + auto normalized = std::fmod(angle, bullet_feeder_angle_per_bullet_); + if (normalized < 0.0) + normalized += bullet_feeder_angle_per_bullet_; + return normalized; + } + + /* 记录发射延迟的测量逻辑： void update_shoot_delay_measurement(bool bullet_fired_now) { const bool bullet_fired_rising_edge = bullet_fired_now && !last_bullet_fired_; @@ -393,14 +438,7 @@ class PutterController shoot_delay_log_stream_.flush(); } - void wait_bullet_ready() { - if (bullet_ready_count_ >= 600) { - bullet_ready_count_ = 0; - set_preloading(); - } else { - bullet_ready_count_++; - } - } + */ static constexpr double nan_ = std::numeric_limits::quiet_NaN(); ///< 非数值常量 static constexpr double inf_ = std::numeric_limits::infinity(); ///< 无穷大常量 @@ -412,9 +450,9 @@ class PutterController static constexpr double max_bullet_feeder_control_torque_ = 0.1; static constexpr double bullet_feeder_angle_per_bullet_ = 2 * std::numbers::pi / 6; + static constexpr double bullet_feeder_offset = 0.1; InputInterface photoelectric_sensor_status_; - bool last_photoelectric_sensor_status_; InputInterface bullet_fired_; bool shooted{false}; @@ -439,6 +477,7 @@ class PutterController InputInterface control_bullet_allowance_limited_by_heat_; bool last_preload_flag_ = false; + bool last_shoot_flag_ = false; bool putter_initialized = false; int putter_faulty_count_ = 0; 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 1e2039cd..80358510 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 @@ -26,16 +26,16 @@ class ShootingRecorder register_input("/referee/shooter/initial_speed", initial_speed_); register_input("/referee/shooter/shoot_timestamp", shoot_timestamp_); - if (friction_wheel_count_ == 2) { - const auto topic = std::array{ - "/gimbal/first_left_friction/control_velocity", - "/gimbal/first_right_friction/control_velocity", - }; - 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]); - } + // if (friction_wheel_count_ == 4) { + // const auto topic = std::array{ + // "/gimbal/first_left_friction/control_velocity", + // "/gimbal/first_right_friction/control_velocity", + // }; + // 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(); 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 976be1d2..0f16692d 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp @@ -92,6 +92,9 @@ class SteeringHero 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 bullet feeder offset: %ld", + top_board_->gimbal_bullet_feeder_.calibrate_zero_point()); RCLCPP_INFO( get_logger(), "[chassis calibration] left front steering offset: %d", bottom_board_one_->chassis_steering_motors_[0].calibrate_zero_point()); @@ -159,6 +162,9 @@ class SteeringHero .set_reduction_ratio(1.)); gimbal_bullet_feeder_.configure( device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("bullet_feeder_motor_zero_point").as_int())) .set_reversed() .enable_multi_turn_angle()); putter_motor_.configure( @@ -265,7 +271,7 @@ class SteeringHero }); builder.gpio_digital_read({ - .channel = 7, + .channel = 1, .falling_edge = true, }); } @@ -305,7 +311,7 @@ class SteeringHero void gpio_digital_read_result_callback( const librmcs::data::GpioDigitalDataView& data) override { *photoelectric_sensor_status_ = false; - if (data.channel == 7) { + if (data.channel == 1) { *photoelectric_sensor_status_ = true; RCLCPP_INFO(logger_, "trigger!"); } diff --git a/tools/pid_tuning/README.md b/tools/pid_tuning/README.md new file mode 100644 index 00000000..eab5cd21 --- /dev/null +++ b/tools/pid_tuning/README.md @@ -0,0 +1,134 @@ +# Friction Wheel PID Tuning + +This directory contains friction wheel PID tuning utilities and configuration snippets. + +## Recorder + +The recorder plugin class is: + +```text +rmcs_core::controller::pid::FrictionWheelPidRecorder +``` + +It records only when: + +- `control_velocity` is finite +- `velocity` is finite +- `control_torque` is finite +- `abs(control_velocity) >= min_setpoint_abs` + +Raw CSV columns: + +```text +timestamp_us,wheel_id,run_id,sample_idx,setpoint_velocity,measured_velocity,control_torque,enabled +``` + +Default output directory: + +```text +/tmp/friction_pid_logs +``` + +See `friction_wheel_pid_recorder.example.yaml` for a minimal configuration snippet. + +## Identification + +`identify_plant.py` estimates a discrete first-order-plus-dead-time model from the recorder CSV: + +```text +y[k+1] = a * y[k] + b * u[k-d] + c +``` + +Where: + +- `u` is `control_torque` +- `y` is `measured_velocity` +- `d` is the pure delay in samples + +The script scans candidate delays, fits each candidate with least squares, and writes: + +- `identification_summary.csv` +- one `*_model.json` per `wheel_id/run_id` +- one `*_fit.csv` per `wheel_id/run_id` + +Example: + +```bash +python3 tools/pid_tuning/identify_plant.py /tmp/friction_pid_logs/friction_wheel_pid_xxx.csv \ + --output-dir /tmp/friction_pid_identify +``` + +Common filters: + +```bash +python3 tools/pid_tuning/identify_plant.py /tmp/friction_pid_logs/friction_wheel_pid_xxx.csv \ + --wheel-id first_left_friction \ + --run-id 1 +``` + +Key options: + +- `--sample-time-ms`: fallback sample interval when timestamps are unavailable +- `--max-delay-ms`: maximum delay search range +- `--min-samples`: minimum segment length +- `--allow-unphysical`: export the best fit even if `a` or `b` is not physically plausible + +## Tuning + +`tune_pid.py` tunes only `kp` and `kd`. `ki` is always fixed to `0` so the offline controller matches the current friction wheel constraint. + +The simulated controller uses the same discrete update rule as `PidCalculator`: + +```text +u[k] = clamp(kp * e[k] + kd * (e[k] - e[k-1])) +``` + +Where: + +- `e[k] = setpoint_velocity[k] - measured_velocity[k]` +- `ki = 0` +- plant model comes from `identify_plant.py` + +Supported optimization methods: + +- `hybrid` (default): GA first, then GP-like Bayesian optimization refinement +- `ga`: genetic algorithm only +- `bo`: GP-like Bayesian optimization only + +Trace-driven tuning with the original recorded setpoint profile: + +```bash +python3 tools/pid_tuning/tune_pid.py /tmp/friction_pid_identify/first_left_friction_run_1_model.json \ + --trace-csv /tmp/friction_pid_logs/friction_wheel_pid_xxx.csv \ + --output-dir /tmp/friction_pid_tuning +``` + +Synthetic startup step tuning: + +```bash +python3 tools/pid_tuning/tune_pid.py /tmp/friction_pid_identify/first_left_friction_run_1_model.json \ + --step-setpoint 5000 \ + --output-limit 12 \ + --method hybrid +``` + +Batch tuning from an identification summary: + +```bash +python3 tools/pid_tuning/tune_pid.py /tmp/friction_pid_identify/identification_summary.csv \ + --trace-csv /tmp/friction_pid_logs/friction_wheel_pid_xxx.csv +``` + +Outputs: + +- `tuning_summary.csv` +- one `tuning_result.json` per `wheel_id/run_id` +- one `candidates.csv` per `wheel_id/run_id` +- one `best_response.csv` per `wheel_id/run_id` + +Common options: + +- `--seed-kp/--seed-kd`: baseline gains used for comparison +- `--kp-min/--kp-max`, `--kd-min/--kd-max`: search bounds +- `--output-limit` or `--output-min/--output-max`: controller output clamp +- `--trace-scope wheel`: reuse all runs of the same wheel instead of exact `run_id` diff --git a/tools/pid_tuning/friction_wheel_pid_recorder.example.yaml b/tools/pid_tuning/friction_wheel_pid_recorder.example.yaml new file mode 100644 index 00000000..8a709112 --- /dev/null +++ b/tools/pid_tuning/friction_wheel_pid_recorder.example.yaml @@ -0,0 +1,15 @@ +rmcs_executor: + ros__parameters: + components: + - rmcs_core::controller::pid::FrictionWheelPidRecorder -> friction_wheel_pid_recorder + +friction_wheel_pid_recorder: + ros__parameters: + wheels: + - /gimbal/first_left_friction + - /gimbal/first_right_friction + - /gimbal/second_left_friction + - /gimbal/second_right_friction + output_dir: /tmp/friction_pid_logs + min_setpoint_abs: 10.0 + flush_every_n_samples: 100 diff --git a/tools/pid_tuning/identify_plant.py b/tools/pid_tuning/identify_plant.py new file mode 100644 index 00000000..2f50af2e --- /dev/null +++ b/tools/pid_tuning/identify_plant.py @@ -0,0 +1,724 @@ +#!/usr/bin/env python3 + +from __future__ import annotations + +import argparse +import csv +import json +import math +import re +import sys +from dataclasses import dataclass +from datetime import datetime, timezone +from pathlib import Path +from typing import Iterable + +import numpy as np + + +REQUIRED_COLUMNS = { + "timestamp_us", + "wheel_id", + "run_id", + "sample_idx", + "setpoint_velocity", + "measured_velocity", + "control_torque", +} + + +@dataclass(frozen=True) +class Sample: + timestamp_us: int + wheel_id: str + run_id: int + sample_idx: int + setpoint_velocity: float + measured_velocity: float + control_torque: float + + +@dataclass(frozen=True) +class Segment: + wheel_id: str + run_id: int + timestamp_us: np.ndarray + sample_idx: np.ndarray + setpoint_velocity: np.ndarray + measured_velocity: np.ndarray + control_torque: np.ndarray + dropped_rows: int + inferred_dt_s: float + sample_jitter_ratio: float + missing_samples: int + + +@dataclass(frozen=True) +class CandidateResult: + delay_samples: int + a: float + b: float + c: float + rollout_rmse: float + one_step_rmse: float + mae: float + fit_percent: float + physically_plausible: bool + predicted_velocity: np.ndarray + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description=( + "Estimate a first-order-plus-dead-time plant model for friction wheel velocity loops " + "from recorder CSV." + ) + ) + parser.add_argument("input_csv", help="CSV produced by FrictionWheelPidRecorder") + parser.add_argument( + "--output-dir", + help="Directory for summary/model files. Default: /tmp/friction_pid_identify/", + ) + parser.add_argument("--wheel-id", help="Only process one wheel_id") + parser.add_argument("--run-id", type=int, help="Only process one run_id") + parser.add_argument( + "--sample-time-ms", + type=float, + default=1.0, + help="Fallback sample time in milliseconds when timestamps are missing or invalid", + ) + parser.add_argument( + "--max-delay-ms", + type=float, + default=25.0, + help="Maximum pure delay to scan in milliseconds", + ) + parser.add_argument( + "--max-delay-samples", + type=int, + help="Maximum pure delay to scan in samples. Overrides --max-delay-ms", + ) + parser.add_argument( + "--min-samples", + type=int, + default=80, + help="Minimum samples required for one segment", + ) + parser.add_argument( + "--min-speed-span", + type=float, + default=30.0, + help="Minimum measured velocity span required to consider a segment sufficiently excited", + ) + parser.add_argument( + "--min-torque-span", + type=float, + default=0.05, + help="Minimum control torque span required to consider a segment sufficiently excited", + ) + parser.add_argument( + "--allow-unphysical", + action="store_true", + help="Allow selecting candidates outside the expected 00 range if needed", + ) + parser.add_argument( + "--quiet", + action="store_true", + help="Only print output paths and fatal errors", + ) + return parser.parse_args() + + +def parse_float(value: str) -> float: + number = float(value) + if not math.isfinite(number): + raise ValueError("non-finite number") + return number + + +def parse_int(value: str) -> int: + return int(value) + + +def default_output_dir() -> Path: + stamp = datetime.now(timezone.utc).strftime("%Y%m%dT%H%M%SZ") + return Path("/tmp/friction_pid_identify") / stamp + + +def log(message: str, quiet: bool = False) -> None: + if not quiet: + print(message) + + +def load_samples(input_csv: Path, wheel_id: str | None, run_id: int | None) -> list[Sample]: + with input_csv.open("r", newline="") as handle: + reader = csv.DictReader(handle) + if reader.fieldnames is None: + raise ValueError("input CSV has no header") + + missing = REQUIRED_COLUMNS.difference(reader.fieldnames) + if missing: + raise ValueError(f"input CSV is missing columns: {', '.join(sorted(missing))}") + + samples: list[Sample] = [] + for row_index, row in enumerate(reader, start=2): + try: + enabled_value = row.get("enabled", "1").strip() + if enabled_value not in {"", "1", "true", "True", "TRUE"}: + continue + + sample = Sample( + timestamp_us=parse_int(row["timestamp_us"]), + wheel_id=row["wheel_id"].strip(), + run_id=parse_int(row["run_id"]), + sample_idx=parse_int(row["sample_idx"]), + setpoint_velocity=parse_float(row["setpoint_velocity"]), + measured_velocity=parse_float(row["measured_velocity"]), + control_torque=parse_float(row["control_torque"]), + ) + except Exception as exc: + raise ValueError(f"failed to parse row {row_index}: {exc}") from exc + + if wheel_id is not None and sample.wheel_id != wheel_id: + continue + if run_id is not None and sample.run_id != run_id: + continue + samples.append(sample) + + return samples + + +def group_samples(samples: Iterable[Sample]) -> dict[tuple[str, int], list[Sample]]: + grouped: dict[tuple[str, int], list[Sample]] = {} + for sample in samples: + grouped.setdefault((sample.wheel_id, sample.run_id), []).append(sample) + return grouped + + +def build_segment( + wheel_id: str, + run_id: int, + rows: list[Sample], + fallback_dt_s: float, +) -> Segment: + ordered = sorted(rows, key=lambda item: (item.sample_idx, item.timestamp_us)) + + filtered: list[Sample] = [] + dropped_rows = 0 + last_sample_idx: int | None = None + last_timestamp_us: int | None = None + for row in ordered: + if last_sample_idx is not None and row.sample_idx <= last_sample_idx: + dropped_rows += 1 + continue + if last_timestamp_us is not None and row.timestamp_us <= last_timestamp_us: + dropped_rows += 1 + continue + filtered.append(row) + last_sample_idx = row.sample_idx + last_timestamp_us = row.timestamp_us + + if not filtered: + raise ValueError(f"{wheel_id}/run{run_id}: no valid rows remain after ordering checks") + + timestamp_us = np.asarray([row.timestamp_us for row in filtered], dtype=np.int64) + sample_idx = np.asarray([row.sample_idx for row in filtered], dtype=np.int64) + setpoint_velocity = np.asarray([row.setpoint_velocity for row in filtered], dtype=np.float64) + measured_velocity = np.asarray([row.measured_velocity for row in filtered], dtype=np.float64) + control_torque = np.asarray([row.control_torque for row in filtered], dtype=np.float64) + + inferred_dt_s, sample_jitter_ratio = infer_sample_time(timestamp_us, fallback_dt_s) + missing_samples = count_missing_samples(sample_idx) + + return Segment( + wheel_id=wheel_id, + run_id=run_id, + timestamp_us=timestamp_us, + sample_idx=sample_idx, + setpoint_velocity=setpoint_velocity, + measured_velocity=measured_velocity, + control_torque=control_torque, + dropped_rows=dropped_rows, + inferred_dt_s=inferred_dt_s, + sample_jitter_ratio=sample_jitter_ratio, + missing_samples=missing_samples, + ) + + +def infer_sample_time(timestamp_us: np.ndarray, fallback_dt_s: float) -> tuple[float, float]: + if timestamp_us.size < 2: + return fallback_dt_s, 0.0 + + dt_us = np.diff(timestamp_us).astype(np.float64) + dt_us = dt_us[dt_us > 0.0] + if dt_us.size == 0: + return fallback_dt_s, 0.0 + + median_dt_us = float(np.median(dt_us)) + if not math.isfinite(median_dt_us) or median_dt_us <= 0.0: + return fallback_dt_s, 0.0 + + mad_us = float(np.median(np.abs(dt_us - median_dt_us))) + jitter_ratio = 0.0 if median_dt_us == 0.0 else mad_us / median_dt_us + return median_dt_us / 1e6, jitter_ratio + + +def count_missing_samples(sample_idx: np.ndarray) -> int: + if sample_idx.size < 2: + return 0 + diffs = np.diff(sample_idx) + return int(np.sum(np.clip(diffs - 1, 0, None))) + + +def excitation_ok(segment: Segment, min_speed_span: float, min_torque_span: float) -> tuple[bool, str]: + speed_span = percentile_span(segment.measured_velocity) + torque_span = percentile_span(segment.control_torque) + if speed_span < min_speed_span: + return False, f"measured velocity span too small ({speed_span:.3f} < {min_speed_span:.3f})" + if torque_span < min_torque_span: + return False, f"control torque span too small ({torque_span:.3f} < {min_torque_span:.3f})" + return True, "" + + +def percentile_span(values: np.ndarray) -> float: + if values.size == 0: + return 0.0 + return float(np.percentile(values, 95) - np.percentile(values, 5)) + + +def estimate_segment( + segment: Segment, + max_delay_samples: int, + allow_unphysical: bool, +) -> CandidateResult: + if segment.measured_velocity.size < 3: + raise ValueError(f"{segment.wheel_id}/run{segment.run_id}: too few samples to fit") + + best_result: CandidateResult | None = None + best_unphysical: CandidateResult | None = None + + for delay_samples in range(max_delay_samples + 1): + candidate = fit_delay_candidate(segment.measured_velocity, segment.control_torque, delay_samples) + if candidate is None: + continue + + if candidate.physically_plausible: + if best_result is None or compare_candidates(candidate, best_result): + best_result = candidate + elif best_unphysical is None or compare_candidates(candidate, best_unphysical): + best_unphysical = candidate + + if best_result is not None: + return best_result + if allow_unphysical and best_unphysical is not None: + return best_unphysical + if best_unphysical is not None: + raise ValueError( + f"{segment.wheel_id}/run{segment.run_id}: only unphysical candidates found; " + "rerun with --allow-unphysical to inspect them" + ) + raise ValueError(f"{segment.wheel_id}/run{segment.run_id}: no candidate could be fitted") + + +def fit_delay_candidate(y: np.ndarray, u: np.ndarray, delay_samples: int) -> CandidateResult | None: + sample_count = y.size + if sample_count - delay_samples < 3: + return None + + k = np.arange(delay_samples, sample_count - 1, dtype=np.int64) + if k.size < 3: + return None + + x = np.column_stack((y[k], u[k - delay_samples], np.ones(k.size, dtype=np.float64))) + target = y[k + 1] + + theta, *_ = np.linalg.lstsq(x, target, rcond=None) + a, b, c = (float(theta[0]), float(theta[1]), float(theta[2])) + if not all(math.isfinite(value) for value in (a, b, c)): + return None + + one_step = x @ theta + one_step_rmse = rmse(target, one_step) + + predicted = np.zeros_like(y) + predicted[0] = y[0] + for index in range(sample_count - 1): + delayed_u = u[index - delay_samples] if index >= delay_samples else 0.0 + predicted[index + 1] = a * predicted[index] + b * delayed_u + c + + actual_tail = y[1:] + predicted_tail = predicted[1:] + if actual_tail.size == 0: + return None + + rollout_rmse = rmse(actual_tail, predicted_tail) + mae = float(np.mean(np.abs(actual_tail - predicted_tail))) + fit_percent = normalized_fit_percent(actual_tail, predicted_tail) + physically_plausible = (0.0 < a < 1.0) and (b > 0.0) + + return CandidateResult( + delay_samples=delay_samples, + a=a, + b=b, + c=c, + rollout_rmse=rollout_rmse, + one_step_rmse=one_step_rmse, + mae=mae, + fit_percent=fit_percent, + physically_plausible=physically_plausible, + predicted_velocity=predicted, + ) + + +def compare_candidates(left: CandidateResult, right: CandidateResult) -> bool: + if left.rollout_rmse < right.rollout_rmse - 1e-9: + return True + if math.isclose(left.rollout_rmse, right.rollout_rmse, rel_tol=0.0, abs_tol=1e-9): + if left.fit_percent > right.fit_percent + 1e-9: + return True + if math.isclose(left.fit_percent, right.fit_percent, rel_tol=0.0, abs_tol=1e-9): + return left.delay_samples < right.delay_samples + return False + + +def rmse(actual: np.ndarray, predicted: np.ndarray) -> float: + return float(np.sqrt(np.mean(np.square(actual - predicted)))) + + +def normalized_fit_percent(actual: np.ndarray, predicted: np.ndarray) -> float: + centered = actual - np.mean(actual) + denominator = float(np.linalg.norm(centered)) + if denominator <= 0.0: + return 0.0 + numerator = float(np.linalg.norm(actual - predicted)) + return 100.0 * max(0.0, 1.0 - numerator / denominator) + + +def continuous_params(candidate: CandidateResult, dt_s: float) -> tuple[float | None, float | None, float | None]: + a = candidate.a + if not (0.0 < a < 1.0): + return None, None, None + + one_minus_a = 1.0 - a + if one_minus_a <= 0.0: + return None, None, None + + time_constant_s = -dt_s / math.log(a) + dc_gain = candidate.b / one_minus_a + velocity_bias = candidate.c / one_minus_a + return time_constant_s, dc_gain, velocity_bias + + +def sanitize_token(value: str) -> str: + sanitized = re.sub(r"[^A-Za-z0-9_.-]+", "_", value.strip()) + return sanitized or "unknown" + + +def write_prediction_csv( + output_path: Path, + segment: Segment, + candidate: CandidateResult, +) -> None: + with output_path.open("w", newline="") as handle: + writer = csv.writer(handle) + writer.writerow( + [ + "timestamp_us", + "sample_idx", + "wheel_id", + "run_id", + "setpoint_velocity", + "measured_velocity", + "predicted_velocity", + "control_torque", + "residual", + ] + ) + residual = segment.measured_velocity - candidate.predicted_velocity + for index in range(segment.measured_velocity.size): + writer.writerow( + [ + int(segment.timestamp_us[index]), + int(segment.sample_idx[index]), + segment.wheel_id, + segment.run_id, + float(segment.setpoint_velocity[index]), + float(segment.measured_velocity[index]), + float(candidate.predicted_velocity[index]), + float(segment.control_torque[index]), + float(residual[index]), + ] + ) + + +def write_model_json( + output_path: Path, + segment: Segment, + candidate: CandidateResult, + time_constant_s: float | None, + dc_gain: float | None, + velocity_bias: float | None, + excitation_message: str, +) -> None: + payload = { + "wheel_id": segment.wheel_id, + "run_id": segment.run_id, + "sample_count": int(segment.measured_velocity.size), + "dropped_rows": segment.dropped_rows, + "missing_samples": segment.missing_samples, + "inferred_dt_s": segment.inferred_dt_s, + "sample_jitter_ratio": segment.sample_jitter_ratio, + "excitation_warning": excitation_message or None, + "model_type": "first_order_plus_dead_time_discrete", + "discrete": { + "a": candidate.a, + "b": candidate.b, + "c": candidate.c, + "delay_samples": candidate.delay_samples, + }, + "continuous": { + "time_constant_s": time_constant_s, + "dc_gain": dc_gain, + "velocity_bias": velocity_bias, + "delay_s": candidate.delay_samples * segment.inferred_dt_s, + }, + "fit": { + "rollout_rmse": candidate.rollout_rmse, + "one_step_rmse": candidate.one_step_rmse, + "mae": candidate.mae, + "fit_percent": candidate.fit_percent, + "physically_plausible": candidate.physically_plausible, + }, + } + + output_path.write_text(json.dumps(payload, indent=2, sort_keys=True) + "\n") + + +def write_summary_csv(output_path: Path, rows: list[dict[str, object]]) -> None: + fieldnames = [ + "wheel_id", + "run_id", + "status", + "message", + "sample_count", + "dropped_rows", + "missing_samples", + "inferred_dt_s", + "sample_jitter_ratio", + "speed_span", + "torque_span", + "delay_samples", + "delay_s", + "a", + "b", + "c", + "time_constant_s", + "dc_gain", + "velocity_bias", + "rollout_rmse", + "one_step_rmse", + "mae", + "fit_percent", + "model_json", + "prediction_csv", + ] + + with output_path.open("w", newline="") as handle: + writer = csv.DictWriter(handle, fieldnames=fieldnames) + writer.writeheader() + for row in rows: + writer.writerow(row) + + +def main() -> int: + args = parse_args() + + input_csv = Path(args.input_csv) + if not input_csv.is_file(): + print(f"input CSV does not exist: {input_csv}", file=sys.stderr) + return 2 + + fallback_dt_s = args.sample_time_ms / 1000.0 + if fallback_dt_s <= 0.0: + print("--sample-time-ms must be positive", file=sys.stderr) + return 2 + + output_dir = Path(args.output_dir) if args.output_dir else default_output_dir() + output_dir.mkdir(parents=True, exist_ok=True) + + try: + samples = load_samples(input_csv, args.wheel_id, args.run_id) + except Exception as exc: + print(f"failed to load CSV: {exc}", file=sys.stderr) + return 2 + + if not samples: + print("no samples matched the selected wheel/run filters", file=sys.stderr) + return 2 + + grouped = group_samples(samples) + summary_rows: list[dict[str, object]] = [] + success_count = 0 + + for (wheel_id, run_id), rows in sorted(grouped.items()): + base_summary = { + "wheel_id": wheel_id, + "run_id": run_id, + "model_json": "", + "prediction_csv": "", + } + + try: + segment = build_segment(wheel_id, run_id, rows, fallback_dt_s) + except Exception as exc: + summary_rows.append( + { + **base_summary, + "status": "error", + "message": str(exc), + } + ) + continue + + if segment.measured_velocity.size < args.min_samples: + summary_rows.append( + { + **base_summary, + "status": "skipped", + "message": ( + f"too few samples ({segment.measured_velocity.size} < {args.min_samples})" + ), + "sample_count": int(segment.measured_velocity.size), + "dropped_rows": segment.dropped_rows, + "missing_samples": segment.missing_samples, + "inferred_dt_s": segment.inferred_dt_s, + "sample_jitter_ratio": segment.sample_jitter_ratio, + "speed_span": percentile_span(segment.measured_velocity), + "torque_span": percentile_span(segment.control_torque), + } + ) + continue + + excitation_good, excitation_message = excitation_ok( + segment, + min_speed_span=args.min_speed_span, + min_torque_span=args.min_torque_span, + ) + if not excitation_good: + summary_rows.append( + { + **base_summary, + "status": "skipped", + "message": excitation_message, + "sample_count": int(segment.measured_velocity.size), + "dropped_rows": segment.dropped_rows, + "missing_samples": segment.missing_samples, + "inferred_dt_s": segment.inferred_dt_s, + "sample_jitter_ratio": segment.sample_jitter_ratio, + "speed_span": percentile_span(segment.measured_velocity), + "torque_span": percentile_span(segment.control_torque), + } + ) + continue + + if args.max_delay_samples is not None: + max_delay_samples = max(0, args.max_delay_samples) + else: + max_delay_samples = max(0, int(round((args.max_delay_ms / 1000.0) / segment.inferred_dt_s))) + + try: + candidate = estimate_segment( + segment=segment, + max_delay_samples=max_delay_samples, + allow_unphysical=args.allow_unphysical, + ) + except Exception as exc: + summary_rows.append( + { + **base_summary, + "status": "error", + "message": str(exc), + "sample_count": int(segment.measured_velocity.size), + "dropped_rows": segment.dropped_rows, + "missing_samples": segment.missing_samples, + "inferred_dt_s": segment.inferred_dt_s, + "sample_jitter_ratio": segment.sample_jitter_ratio, + "speed_span": percentile_span(segment.measured_velocity), + "torque_span": percentile_span(segment.control_torque), + } + ) + continue + + time_constant_s, dc_gain, velocity_bias = continuous_params(candidate, segment.inferred_dt_s) + + wheel_token = sanitize_token(wheel_id) + run_token = f"run_{run_id}" + model_json_path = output_dir / f"{wheel_token}_{run_token}_model.json" + prediction_csv_path = output_dir / f"{wheel_token}_{run_token}_fit.csv" + + write_model_json( + model_json_path, + segment, + candidate, + time_constant_s, + dc_gain, + velocity_bias, + excitation_message="" if excitation_good else excitation_message, + ) + write_prediction_csv(prediction_csv_path, segment, candidate) + + speed_span = percentile_span(segment.measured_velocity) + torque_span = percentile_span(segment.control_torque) + summary_rows.append( + { + **base_summary, + "status": "ok", + "message": "" if candidate.physically_plausible else "selected unphysical candidate", + "sample_count": int(segment.measured_velocity.size), + "dropped_rows": segment.dropped_rows, + "missing_samples": segment.missing_samples, + "inferred_dt_s": segment.inferred_dt_s, + "sample_jitter_ratio": segment.sample_jitter_ratio, + "speed_span": speed_span, + "torque_span": torque_span, + "delay_samples": candidate.delay_samples, + "delay_s": candidate.delay_samples * segment.inferred_dt_s, + "a": candidate.a, + "b": candidate.b, + "c": candidate.c, + "time_constant_s": time_constant_s, + "dc_gain": dc_gain, + "velocity_bias": velocity_bias, + "rollout_rmse": candidate.rollout_rmse, + "one_step_rmse": candidate.one_step_rmse, + "mae": candidate.mae, + "fit_percent": candidate.fit_percent, + "model_json": str(model_json_path), + "prediction_csv": str(prediction_csv_path), + } + ) + + success_count += 1 + log( + ( + f"[ok] {wheel_id}/run{run_id}: delay={candidate.delay_samples} samples, " + f"tau={time_constant_s if time_constant_s is not None else float('nan'):.6f}s, " + f"gain={dc_gain if dc_gain is not None else float('nan'):.6f}, " + f"fit={candidate.fit_percent:.2f}%" + ), + quiet=args.quiet, + ) + + summary_path = output_dir / "identification_summary.csv" + write_summary_csv(summary_path, summary_rows) + + if success_count == 0: + print(f"no segment was successfully identified; summary: {summary_path}", file=sys.stderr) + return 1 + + log(f"summary: {summary_path}", quiet=args.quiet) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tools/pid_tuning/tune_pid.py b/tools/pid_tuning/tune_pid.py new file mode 100644 index 00000000..6bd61a3a --- /dev/null +++ b/tools/pid_tuning/tune_pid.py @@ -0,0 +1,1280 @@ +#!/usr/bin/env python3 + +from __future__ import annotations + +import argparse +import csv +import json +import math +import random +import sys +from dataclasses import dataclass +from datetime import datetime, timezone +from pathlib import Path +from typing import Iterable + +import numpy as np + +from identify_plant import build_segment, group_samples, load_samples, sanitize_token + + +CURRENT_DEFAULT_KP = 0.003436926 +CURRENT_DEFAULT_KD = 0.009373434 + + +@dataclass(frozen=True) +class PlantModel: + wheel_id: str + run_id: int + dt_s: float + a: float + b: float + c: float + delay_samples: int + source_path: Path + fit_percent: float | None + + +@dataclass(frozen=True) +class TraceData: + wheel_id: str + run_id: int + dt_s: float + setpoint: np.ndarray + initial_velocity: float + observed_velocity: np.ndarray | None + observed_control: np.ndarray | None + source_name: str + + +@dataclass(frozen=True) +class SimulationResult: + velocity: np.ndarray + control: np.ndarray + diverged: bool + + +@dataclass(frozen=True) +class Metrics: + cost: float + mae_norm: float + rmse_norm: float + itae_norm: float + overshoot_norm: float + steady_error_norm: float + final_error_norm: float + settle_ratio: float + control_rms_norm: float + control_tv_norm: float + saturation_ratio: float + + +@dataclass(frozen=True) +class EvaluationRecord: + kp: float + kd: float + cost: float + mae_norm: float + rmse_norm: float + itae_norm: float + overshoot_norm: float + steady_error_norm: float + final_error_norm: float + settle_ratio: float + control_rms_norm: float + control_tv_norm: float + saturation_ratio: float + method: str + evaluation_index: int + + +@dataclass(frozen=True) +class TuningOutcome: + model: PlantModel + trace_count: int + output_min: float + output_max: float + best: EvaluationRecord + baseline: EvaluationRecord + best_simulation: SimulationResult + best_trace: TraceData + output_dir: Path + + +class ObjectiveEvaluator: + def __init__( + self, + model: PlantModel, + traces: list[TraceData], + output_min: float, + output_max: float, + settle_band_ratio: float, + quiet: bool, + ) -> None: + self.model = model + self.traces = traces + self.output_min = output_min + self.output_max = output_max + self.settle_band_ratio = settle_band_ratio + self.quiet = quiet + self.cache: dict[tuple[float, float], EvaluationRecord] = {} + self.records: list[EvaluationRecord] = [] + self.best_record: EvaluationRecord | None = None + + def evaluate(self, kp: float, kd: float, method: str) -> EvaluationRecord: + kp = float(kp) + kd = float(kd) + key = (round(kp, 15), round(kd, 15)) + cached = self.cache.get(key) + if cached is not None: + return cached + + trace_metrics: list[Metrics] = [] + for trace in self.traces: + simulation = simulate_closed_loop( + model=self.model, + trace=trace, + kp=kp, + kd=kd, + output_min=self.output_min, + output_max=self.output_max, + ) + metrics = compute_metrics( + trace=trace, + simulation=simulation, + settle_band_ratio=self.settle_band_ratio, + output_min=self.output_min, + output_max=self.output_max, + ) + trace_metrics.append(metrics) + + record = EvaluationRecord( + kp=kp, + kd=kd, + cost=mean_metric(trace_metrics, "cost"), + mae_norm=mean_metric(trace_metrics, "mae_norm"), + rmse_norm=mean_metric(trace_metrics, "rmse_norm"), + itae_norm=mean_metric(trace_metrics, "itae_norm"), + overshoot_norm=mean_metric(trace_metrics, "overshoot_norm"), + steady_error_norm=mean_metric(trace_metrics, "steady_error_norm"), + final_error_norm=mean_metric(trace_metrics, "final_error_norm"), + settle_ratio=mean_metric(trace_metrics, "settle_ratio"), + control_rms_norm=mean_metric(trace_metrics, "control_rms_norm"), + control_tv_norm=mean_metric(trace_metrics, "control_tv_norm"), + saturation_ratio=mean_metric(trace_metrics, "saturation_ratio"), + method=method, + evaluation_index=len(self.records), + ) + + self.records.append(record) + self.cache[key] = record + if self.best_record is None or record.cost < self.best_record.cost: + self.best_record = record + if not self.quiet: + print( + ( + f"[best] {self.model.wheel_id}/run{self.model.run_id}: " + f"kp={record.kp:.9g}, kd={record.kd:.9g}, cost={record.cost:.6f}" + ) + ) + return record + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description=( + "Tune friction wheel velocity PID gains offline from identified plant models. " + "The discrete control law matches rmcs_core::controller::pid::PidCalculator " + "with ki fixed to 0." + ) + ) + parser.add_argument( + "model_input", + help="Either one *_model.json from identify_plant.py or identification_summary.csv", + ) + parser.add_argument( + "--trace-csv", + help=( + "Recorder CSV from FrictionWheelPidRecorder. If provided, the original setpoint trace " + "is reused for closed-loop evaluation." + ), + ) + parser.add_argument("--wheel-id", help="Only tune one wheel_id") + parser.add_argument("--run-id", type=int, help="Only tune one run_id") + parser.add_argument( + "--trace-scope", + choices=("exact", "wheel"), + default="exact", + help="How to match trace segments to a model when --trace-csv is provided", + ) + parser.add_argument( + "--sample-time-ms", + type=float, + default=1.0, + help="Fallback sample interval for trace CSV parsing when timestamps are invalid", + ) + parser.add_argument( + "--method", + choices=("hybrid", "ga", "bo"), + default="hybrid", + help="Optimization method", + ) + parser.add_argument("--kp-min", type=float, default=1e-6, help="Lower bound for kp") + parser.add_argument("--kp-max", type=float, default=1.0, help="Upper bound for kp") + parser.add_argument("--kd-min", type=float, default=1e-8, help="Lower bound for kd") + parser.add_argument("--kd-max", type=float, default=1.0, help="Upper bound for kd") + parser.add_argument( + "--seed-kp", + type=float, + default=CURRENT_DEFAULT_KP, + help="Initial/reference kp to compare against", + ) + parser.add_argument( + "--seed-kd", + type=float, + default=CURRENT_DEFAULT_KD, + help="Initial/reference kd to compare against", + ) + parser.add_argument( + "--ga-population", + type=int, + default=36, + help="GA population size", + ) + parser.add_argument( + "--ga-generations", + type=int, + default=20, + help="GA generation count", + ) + parser.add_argument( + "--bo-initial", + type=int, + default=12, + help="BO random warmup samples", + ) + parser.add_argument( + "--bo-iterations", + type=int, + default=24, + help="BO surrogate iterations", + ) + parser.add_argument( + "--random-seed", + type=int, + default=42, + help="Random seed for deterministic tuning runs", + ) + parser.add_argument( + "--output-limit", + type=float, + help="Symmetric control limit. Equivalent to --output-min=-x --output-max=x", + ) + parser.add_argument("--output-min", type=float, help="Lower control clamp") + parser.add_argument("--output-max", type=float, help="Upper control clamp") + parser.add_argument( + "--infer-output-percentile", + type=float, + default=99.5, + help="Percentile used to infer |control_torque| limit from --trace-csv", + ) + parser.add_argument( + "--estimated-output-factor", + type=float, + default=2.5, + help="Fallback multiplier when output limit is estimated from target speed and plant gain", + ) + parser.add_argument( + "--step-setpoint", + type=float, + help="Target speed for synthetic step evaluation when --trace-csv is not provided", + ) + parser.add_argument( + "--step-duration-ms", + type=float, + default=1200.0, + help="Synthetic step hold duration in milliseconds", + ) + parser.add_argument( + "--pre-roll-ms", + type=float, + default=30.0, + help="Synthetic zero-input lead-in before the step", + ) + parser.add_argument( + "--settle-band-ratio", + type=float, + default=0.05, + help="Relative settling band used by the objective", + ) + parser.add_argument( + "--output-dir", + help="Directory for tuning outputs. Default: /tmp/friction_pid_tuning/", + ) + parser.add_argument( + "--quiet", + action="store_true", + help="Suppress progress output", + ) + return parser.parse_args() + + +def default_output_dir() -> Path: + stamp = datetime.now(timezone.utc).strftime("%Y%m%dT%H%M%SZ") + return Path("/tmp/friction_pid_tuning") / stamp + + +def load_models(model_input: Path, wheel_id: str | None, run_id: int | None) -> list[PlantModel]: + if not model_input.is_file(): + raise ValueError(f"model input does not exist: {model_input}") + + if model_input.suffix.lower() == ".json": + models = [load_model_json(model_input)] + else: + models = load_models_from_summary(model_input) + + filtered = [ + model + for model in models + if (wheel_id is None or model.wheel_id == wheel_id) + and (run_id is None or model.run_id == run_id) + ] + if not filtered: + raise ValueError("no model matched the selected wheel/run filters") + return filtered + + +def load_model_json(path: Path) -> PlantModel: + payload = json.loads(path.read_text()) + discrete = payload["discrete"] + fit = payload.get("fit", {}) + dt_s = float(payload["inferred_dt_s"]) + if not math.isfinite(dt_s) or dt_s <= 0.0: + raise ValueError(f"{path}: invalid inferred_dt_s") + + return PlantModel( + wheel_id=str(payload["wheel_id"]), + run_id=int(payload["run_id"]), + dt_s=dt_s, + a=float(discrete["a"]), + b=float(discrete["b"]), + c=float(discrete["c"]), + delay_samples=int(discrete["delay_samples"]), + source_path=path, + fit_percent=float(fit["fit_percent"]) if "fit_percent" in fit else None, + ) + + +def load_models_from_summary(path: Path) -> list[PlantModel]: + models: list[PlantModel] = [] + with path.open("r", newline="") as handle: + reader = csv.DictReader(handle) + for row in reader: + if row.get("status") != "ok": + continue + model_json = (row.get("model_json") or "").strip() + if not model_json: + continue + model_path = Path(model_json) + if not model_path.is_absolute(): + model_path = (path.parent / model_path).resolve() + models.append(load_model_json(model_path)) + + if not models: + raise ValueError(f"{path}: no usable model_json rows found in summary") + return models + + +def load_trace_map(trace_csv: Path, fallback_dt_s: float) -> dict[tuple[str, int], TraceData]: + samples = load_samples(trace_csv, None, None) + grouped = group_samples(samples) + trace_map: dict[tuple[str, int], TraceData] = {} + for (wheel_id, run_id), rows in grouped.items(): + segment = build_segment(wheel_id, run_id, rows, fallback_dt_s) + trace_map[(wheel_id, run_id)] = TraceData( + wheel_id=wheel_id, + run_id=run_id, + dt_s=segment.inferred_dt_s, + setpoint=segment.setpoint_velocity.copy(), + initial_velocity=float(segment.measured_velocity[0]) if segment.measured_velocity.size else 0.0, + observed_velocity=segment.measured_velocity.copy(), + observed_control=segment.control_torque.copy(), + source_name=f"{trace_csv}:{wheel_id}/run{run_id}", + ) + return trace_map + + +def select_traces( + model: PlantModel, + trace_map: dict[tuple[str, int], TraceData] | None, + trace_scope: str, + step_setpoint: float | None, + step_duration_ms: float, + pre_roll_ms: float, +) -> list[TraceData]: + if trace_map: + exact = trace_map.get((model.wheel_id, model.run_id)) + if exact is not None: + return [exact] + if trace_scope == "wheel": + same_wheel = [ + trace + for trace in trace_map.values() + if trace.wheel_id == model.wheel_id + ] + if same_wheel: + return sorted(same_wheel, key=lambda trace: trace.run_id) + raise ValueError( + f"no trace matched {model.wheel_id}/run{model.run_id}; " + "provide the corresponding recorder CSV or switch to synthetic step mode" + ) + + if step_setpoint is None: + raise ValueError( + f"{model.wheel_id}/run{model.run_id}: --step-setpoint is required when --trace-csv is absent" + ) + return [make_step_trace(model, step_setpoint, step_duration_ms, pre_roll_ms)] + + +def make_step_trace( + model: PlantModel, + step_setpoint: float, + step_duration_ms: float, + pre_roll_ms: float, +) -> TraceData: + pre_samples = max(1, int(round((pre_roll_ms / 1000.0) / model.dt_s))) + hold_samples = max(10, int(round((step_duration_ms / 1000.0) / model.dt_s))) + total = pre_samples + hold_samples + setpoint = np.zeros(total, dtype=np.float64) + setpoint[pre_samples:] = step_setpoint + + return TraceData( + wheel_id=model.wheel_id, + run_id=model.run_id, + dt_s=model.dt_s, + setpoint=setpoint, + initial_velocity=0.0, + observed_velocity=None, + observed_control=None, + source_name=f"synthetic_step:{model.wheel_id}/run{model.run_id}", + ) + + +def infer_output_bounds( + model: PlantModel, + traces: list[TraceData], + output_limit: float | None, + output_min: float | None, + output_max: float | None, + infer_output_percentile: float, + estimated_output_factor: float, +) -> tuple[float, float]: + if output_limit is not None: + limit = abs(output_limit) + if limit <= 0.0: + raise ValueError("--output-limit must be positive") + return -limit, limit + + if output_min is not None or output_max is not None: + if output_min is None or output_max is None: + raise ValueError("--output-min and --output-max must be set together") + if output_min >= output_max: + raise ValueError("--output-min must be smaller than --output-max") + return float(output_min), float(output_max) + + observed_controls = [ + trace.observed_control + for trace in traces + if trace.observed_control is not None and trace.observed_control.size > 0 + ] + if observed_controls: + merged = np.concatenate(observed_controls) + limit = float(np.percentile(np.abs(merged), infer_output_percentile)) + limit = max(limit, 1e-6) + return -limit, limit + + dc_gain = plant_dc_gain(model) + target_mag = max(trace_target_magnitude(trace) for trace in traces) + if dc_gain is None or abs(dc_gain) < 1e-9: + estimated_limit = 10.0 + else: + estimated_hold = target_mag / abs(dc_gain) + estimated_limit = max(estimated_output_factor * estimated_hold, 1.0) + return -estimated_limit, estimated_limit + + +def plant_dc_gain(model: PlantModel) -> float | None: + denominator = 1.0 - model.a + if abs(denominator) < 1e-12: + return None + return model.b / denominator + + +def trace_target_magnitude(trace: TraceData) -> float: + if trace.setpoint.size == 0: + return 1.0 + return max(float(np.percentile(np.abs(trace.setpoint), 95)), 1.0) + + +def simulate_closed_loop( + model: PlantModel, + trace: TraceData, + kp: float, + kd: float, + output_min: float, + output_max: float, +) -> SimulationResult: + sample_count = trace.setpoint.size + velocity = np.zeros(sample_count, dtype=np.float64) + control = np.zeros(sample_count, dtype=np.float64) + velocity[0] = trace.initial_velocity + + diverged = False + last_err = math.nan + divergence_limit = max(50.0 * trace_target_magnitude(trace), 1e6) + + for index in range(sample_count): + err = float(trace.setpoint[index] - velocity[index]) + command = kp * err + if not math.isnan(last_err): + command += kd * (err - last_err) + last_err = err + command = min(max(command, output_min), output_max) + control[index] = command + + if index + 1 >= sample_count: + continue + + delayed_u = control[index - model.delay_samples] if index >= model.delay_samples else 0.0 + next_velocity = model.a * velocity[index] + model.b * delayed_u + model.c + if not math.isfinite(next_velocity) or abs(next_velocity) > divergence_limit: + diverged = True + velocity[index + 1 :] = divergence_limit * np.sign(next_velocity if math.isfinite(next_velocity) else 1.0) + break + velocity[index + 1] = next_velocity + + return SimulationResult(velocity=velocity, control=control, diverged=diverged) + + +def compute_metrics( + trace: TraceData, + simulation: SimulationResult, + settle_band_ratio: float, + output_min: float, + output_max: float, +) -> Metrics: + setpoint = trace.setpoint + velocity = simulation.velocity + control = simulation.control + scale = trace_target_magnitude(trace) + error = setpoint - velocity + normalized_error = error / scale + + mae_norm = float(np.mean(np.abs(normalized_error))) + rmse_norm = float(np.sqrt(np.mean(np.square(normalized_error)))) + + time_vector = np.linspace(0.0, 1.0, setpoint.size, endpoint=True) + itae_norm = float(np.mean(time_vector * np.abs(normalized_error))) + + active_mask = np.abs(setpoint) >= 0.1 * scale + if not np.any(active_mask): + active_mask = np.ones(setpoint.size, dtype=bool) + active_indices = np.where(active_mask)[0] + active_start = int(active_indices[0]) + + plateau_mask = active_mask & ( + np.abs(np.diff(setpoint, prepend=setpoint[0])) <= 0.005 * scale + ) + if np.count_nonzero(plateau_mask) < max(5, setpoint.size // 20): + plateau_mask = np.zeros(setpoint.size, dtype=bool) + plateau_mask[max(active_start, int(setpoint.size * 0.8)) :] = True + plateau_indices = np.where(plateau_mask)[0] + + if plateau_indices.size > 0: + steady_error_norm = float(np.mean(np.abs(normalized_error[plateau_indices]))) + overshoot_norm = ( + float(np.max(np.maximum(0.0, np.abs(velocity[plateau_indices]) - np.abs(setpoint[plateau_indices])))) + / scale + ) + else: + steady_error_norm = float(np.mean(np.abs(normalized_error[-max(1, setpoint.size // 10) :]))) + overshoot_norm = ( + float(np.max(np.maximum(0.0, np.abs(velocity[active_mask]) - np.abs(setpoint[active_mask])))) + / scale + ) + + band = np.maximum(settle_band_ratio * np.abs(setpoint), settle_band_ratio * scale) + outside = np.where(active_mask & (np.abs(error) > band))[0] + if outside.size == 0: + settle_ratio = 0.0 + else: + denominator = max(1, (setpoint.size - 1) - active_start) + settle_ratio = float((outside[-1] - active_start) / denominator) + + final_error_norm = float(abs(error[-1]) / scale) + + finite_limits = math.isfinite(output_min) and math.isfinite(output_max) + if finite_limits: + control_scale = max(abs(output_min), abs(output_max), 1.0) + saturation_margin = 0.01 * control_scale + saturation_ratio = float( + np.mean((control <= output_min + saturation_margin) | (control >= output_max - saturation_margin)) + ) + else: + control_scale = max(float(np.percentile(np.abs(control), 95)), 1.0) + saturation_ratio = 0.0 + + control_rms_norm = float(np.sqrt(np.mean(np.square(control / control_scale)))) + if control.size >= 2: + control_tv_norm = float(np.mean(np.abs(np.diff(control))) / control_scale) + else: + control_tv_norm = 0.0 + + divergence_penalty = 50.0 if simulation.diverged else 0.0 + cost = ( + 4.0 * mae_norm + + 2.5 * rmse_norm + + 2.0 * itae_norm + + 3.5 * overshoot_norm + + 2.5 * steady_error_norm + + 2.0 * final_error_norm + + 1.2 * settle_ratio + + 0.25 * control_rms_norm + + 0.6 * control_tv_norm + + 1.5 * saturation_ratio + + divergence_penalty + ) + + return Metrics( + cost=cost, + mae_norm=mae_norm, + rmse_norm=rmse_norm, + itae_norm=itae_norm, + overshoot_norm=overshoot_norm, + steady_error_norm=steady_error_norm, + final_error_norm=final_error_norm, + settle_ratio=settle_ratio, + control_rms_norm=control_rms_norm, + control_tv_norm=control_tv_norm, + saturation_ratio=saturation_ratio, + ) + + +def mean_metric(metrics: Iterable[Metrics], field_name: str) -> float: + values = [getattr(metric, field_name) for metric in metrics] + return float(sum(values) / len(values)) + + +def encode_params(kp: float, kd: float) -> np.ndarray: + return np.asarray([math.log10(kp), math.log10(kd)], dtype=np.float64) + + +def decode_params(encoded: np.ndarray) -> tuple[float, float]: + return 10.0 ** float(encoded[0]), 10.0 ** float(encoded[1]) + + +def sample_encoded(bounds: tuple[np.ndarray, np.ndarray], rng: random.Random) -> np.ndarray: + lower, upper = bounds + return np.asarray( + [rng.uniform(float(lower[0]), float(upper[0])), rng.uniform(float(lower[1]), float(upper[1]))], + dtype=np.float64, + ) + + +def clamp_encoded(encoded: np.ndarray, bounds: tuple[np.ndarray, np.ndarray]) -> np.ndarray: + lower, upper = bounds + return np.minimum(np.maximum(encoded, lower), upper) + + +def build_seed_points( + bounds: tuple[np.ndarray, np.ndarray], + seed_kp: float, + seed_kd: float, + model: PlantModel, + traces: list[TraceData], + output_min: float, + output_max: float, +) -> list[np.ndarray]: + lower, upper = bounds + seeds: list[np.ndarray] = [] + + def append_point(kp: float, kd: float) -> None: + kp = min(max(kp, 10.0 ** float(lower[0])), 10.0 ** float(upper[0])) + kd = min(max(kd, 10.0 ** float(lower[1])), 10.0 ** float(upper[1])) + encoded = encode_params(kp, kd) + for existing in seeds: + if np.allclose(existing, encoded, atol=1e-12, rtol=0.0): + return + seeds.append(encoded) + + append_point(seed_kp, seed_kd) + append_point(seed_kp * 0.5, seed_kd * 0.5) + append_point(seed_kp * 2.0, seed_kd * 2.0) + append_point(seed_kp * 0.5, seed_kd * 2.0) + append_point(seed_kp * 2.0, max(seed_kd * 0.5, 1e-12)) + + dc_gain = plant_dc_gain(model) + target_mag = max(trace_target_magnitude(trace) for trace in traces) + output_limit = max(abs(output_min), abs(output_max), 1.0) + if dc_gain is not None and abs(dc_gain) > 1e-9: + proportional_guess = min(max(0.35 * output_limit / target_mag, 1e-12), 10.0 ** float(upper[0])) + derivative_guess = min(max(proportional_guess * max(1.0, model.delay_samples), 1e-12), 10.0 ** float(upper[1])) + append_point(proportional_guess, derivative_guess) + append_point(0.75 * proportional_guess, 0.5 * derivative_guess) + + append_point(10.0 ** float(lower[0]), 10.0 ** float(lower[1])) + return seeds + + +def run_ga( + evaluator: ObjectiveEvaluator, + bounds: tuple[np.ndarray, np.ndarray], + population_size: int, + generations: int, + seed_points: list[np.ndarray], + rng: random.Random, + quiet: bool, +) -> None: + lower, upper = bounds + population: list[np.ndarray] = [point.copy() for point in seed_points[:population_size]] + while len(population) < population_size: + population.append(sample_encoded(bounds, rng)) + + for generation in range(generations): + scored: list[tuple[EvaluationRecord, np.ndarray]] = [] + for encoded in population: + kp, kd = decode_params(encoded) + scored.append((evaluator.evaluate(kp, kd, method="ga"), encoded)) + scored.sort(key=lambda item: item[0].cost) + + if not quiet: + best_record = scored[0][0] + print( + ( + f"[ga] generation={generation + 1}/{generations} " + f"best_cost={best_record.cost:.6f} " + f"kp={best_record.kp:.9g} kd={best_record.kd:.9g}" + ) + ) + + elite_count = max(2, population_size // 5) + survivors = [encoded.copy() for _, encoded in scored[:elite_count]] + + next_population: list[np.ndarray] = survivors[:] + mutation_sigma = max(0.03, 0.18 * (1.0 - generation / max(1, generations))) + tournament_pool = [encoded for _, encoded in scored[: max(elite_count * 2, 4)]] + + while len(next_population) < population_size: + parent_a = rng.choice(tournament_pool) + parent_b = rng.choice(tournament_pool) + alpha = rng.uniform(0.2, 0.8) + child = alpha * parent_a + (1.0 - alpha) * parent_b + if rng.random() < 0.85: + child = child + np.asarray( + [rng.gauss(0.0, mutation_sigma), rng.gauss(0.0, mutation_sigma)], + dtype=np.float64, + ) + if rng.random() < 0.12: + child = sample_encoded(bounds, rng) + child = clamp_encoded(child, bounds) + next_population.append(child) + + population = next_population + + # Evaluate the final population once more so the final offspring enter the archive. + for encoded in population: + kp, kd = decode_params(encoded) + evaluator.evaluate(kp, kd, method="ga") + + +def kernel_rbf(x1: np.ndarray, x2: np.ndarray, length_scale: float) -> np.ndarray: + diff = x1[:, None, :] - x2[None, :, :] + dist_sq = np.sum(np.square(diff), axis=2) + return np.exp(-0.5 * dist_sq / (length_scale * length_scale)) + + +def normalize_encoded(encoded: np.ndarray, bounds: tuple[np.ndarray, np.ndarray]) -> np.ndarray: + lower, upper = bounds + return (encoded - lower) / np.maximum(upper - lower, 1e-12) + + +def run_bo( + evaluator: ObjectiveEvaluator, + bounds: tuple[np.ndarray, np.ndarray], + initial_samples: int, + iterations: int, + seed_points: list[np.ndarray], + rng: random.Random, + quiet: bool, +) -> None: + archive_encoded: list[np.ndarray] = [] + archive_costs: list[float] = [] + + def register_point(encoded: np.ndarray, method: str) -> None: + kp, kd = decode_params(encoded) + record = evaluator.evaluate(kp, kd, method=method) + archive_encoded.append(encoded.copy()) + archive_costs.append(record.cost) + + seen: set[tuple[float, float]] = set() + + def add_if_new(encoded: np.ndarray, method: str) -> None: + key = (round(float(encoded[0]), 12), round(float(encoded[1]), 12)) + if key in seen: + return + seen.add(key) + register_point(encoded, method) + + for encoded in seed_points[:initial_samples]: + add_if_new(clamp_encoded(encoded, bounds), "bo") + + while len(archive_encoded) < initial_samples: + add_if_new(sample_encoded(bounds, rng), "bo") + + for iteration in range(iterations): + x_train = np.asarray(archive_encoded, dtype=np.float64) + y_train = np.asarray(archive_costs, dtype=np.float64) + x_norm = normalize_encoded(x_train, bounds) + + length_scale = 0.22 + jitter = 1e-6 + k_xx = kernel_rbf(x_norm, x_norm, length_scale) + jitter * np.eye(x_norm.shape[0]) + centered = y_train - np.mean(y_train) + alpha = np.linalg.solve(k_xx, centered) + inverse_k = np.linalg.inv(k_xx) + + candidate_pool = [sample_encoded(bounds, rng) for _ in range(2048)] + if evaluator.best_record is not None: + best_encoded = encode_params(evaluator.best_record.kp, evaluator.best_record.kd) + for _ in range(256): + local = best_encoded + np.asarray( + [rng.gauss(0.0, 0.06), rng.gauss(0.0, 0.06)], + dtype=np.float64, + ) + candidate_pool.append(clamp_encoded(local, bounds)) + + candidate_matrix = np.asarray(candidate_pool, dtype=np.float64) + candidate_norm = normalize_encoded(candidate_matrix, bounds) + + k_star = kernel_rbf(x_norm, candidate_norm, length_scale) + mean = np.mean(y_train) + k_star.T @ alpha + variance = np.maximum(0.0, 1.0 - np.sum((k_star.T @ inverse_k) * k_star.T, axis=1)) + sigma = np.sqrt(variance) + beta = max(0.35, 1.75 - 1.2 * (iteration / max(1, iterations))) + acquisition = mean - beta * sigma + + ordering = np.argsort(acquisition) + chosen: np.ndarray | None = None + for idx in ordering: + encoded = candidate_matrix[int(idx)] + key = (round(float(encoded[0]), 12), round(float(encoded[1]), 12)) + if key not in seen: + chosen = encoded + break + if chosen is None: + chosen = sample_encoded(bounds, rng) + + add_if_new(chosen, "bo") + + if not quiet and evaluator.best_record is not None: + best = evaluator.best_record + print( + ( + f"[bo] iteration={iteration + 1}/{iterations} " + f"best_cost={best.cost:.6f} " + f"kp={best.kp:.9g} kd={best.kd:.9g}" + ) + ) + + +def tune_one_model( + model: PlantModel, + traces: list[TraceData], + args: argparse.Namespace, + rng: random.Random, + output_dir: Path, +) -> TuningOutcome: + output_min, output_max = infer_output_bounds( + model=model, + traces=traces, + output_limit=args.output_limit, + output_min=args.output_min, + output_max=args.output_max, + infer_output_percentile=args.infer_output_percentile, + estimated_output_factor=args.estimated_output_factor, + ) + + if args.kp_min <= 0.0 or args.kd_min <= 0.0: + raise ValueError("--kp-min and --kd-min must be strictly positive for log-scale search") + if args.kp_min >= args.kp_max or args.kd_min >= args.kd_max: + raise ValueError("kp/kd bounds must satisfy min < max") + + bounds = ( + np.asarray([math.log10(args.kp_min), math.log10(args.kd_min)], dtype=np.float64), + np.asarray([math.log10(args.kp_max), math.log10(args.kd_max)], dtype=np.float64), + ) + + evaluator = ObjectiveEvaluator( + model=model, + traces=traces, + output_min=output_min, + output_max=output_max, + settle_band_ratio=args.settle_band_ratio, + quiet=args.quiet, + ) + + seeds = build_seed_points( + bounds=bounds, + seed_kp=args.seed_kp, + seed_kd=args.seed_kd, + model=model, + traces=traces, + output_min=output_min, + output_max=output_max, + ) + + baseline = evaluator.evaluate(args.seed_kp, args.seed_kd, method="baseline") + + if args.method in {"ga", "hybrid"}: + run_ga( + evaluator=evaluator, + bounds=bounds, + population_size=args.ga_population, + generations=args.ga_generations, + seed_points=seeds, + rng=rng, + quiet=args.quiet, + ) + + if args.method in {"bo", "hybrid"}: + if args.method == "bo": + bo_seeds = seeds + else: + sorted_records = sorted(evaluator.records, key=lambda record: record.cost) + bo_seeds = [encode_params(record.kp, record.kd) for record in sorted_records[: max(8, args.bo_initial)]] + bo_seeds.extend(seeds) + + run_bo( + evaluator=evaluator, + bounds=bounds, + initial_samples=args.bo_initial, + iterations=args.bo_iterations, + seed_points=bo_seeds, + rng=rng, + quiet=args.quiet, + ) + + if evaluator.best_record is None: + raise RuntimeError(f"{model.wheel_id}/run{model.run_id}: no candidate was evaluated") + + best = evaluator.best_record + best_trace = traces[0] + best_simulation = simulate_closed_loop( + model=model, + trace=best_trace, + kp=best.kp, + kd=best.kd, + output_min=output_min, + output_max=output_max, + ) + + tune_output_dir = output_dir / f"{sanitize_token(model.wheel_id)}_run_{model.run_id}" + tune_output_dir.mkdir(parents=True, exist_ok=True) + + write_candidates_csv(tune_output_dir / "candidates.csv", evaluator.records) + write_response_csv( + tune_output_dir / "best_response.csv", + trace=best_trace, + simulation=best_simulation, + output_min=output_min, + output_max=output_max, + ) + write_tuning_json( + tune_output_dir / "tuning_result.json", + model=model, + trace_count=len(traces), + output_min=output_min, + output_max=output_max, + baseline=baseline, + best=best, + method=args.method, + ) + + return TuningOutcome( + model=model, + trace_count=len(traces), + output_min=output_min, + output_max=output_max, + best=best, + baseline=baseline, + best_simulation=best_simulation, + best_trace=best_trace, + output_dir=tune_output_dir, + ) + + +def write_candidates_csv(output_path: Path, records: list[EvaluationRecord]) -> None: + fieldnames = [ + "evaluation_index", + "method", + "kp", + "kd", + "cost", + "mae_norm", + "rmse_norm", + "itae_norm", + "overshoot_norm", + "steady_error_norm", + "final_error_norm", + "settle_ratio", + "control_rms_norm", + "control_tv_norm", + "saturation_ratio", + ] + ordered = sorted(records, key=lambda record: (record.cost, record.evaluation_index)) + with output_path.open("w", newline="") as handle: + writer = csv.DictWriter(handle, fieldnames=fieldnames) + writer.writeheader() + for record in ordered: + writer.writerow( + { + "evaluation_index": record.evaluation_index, + "method": record.method, + "kp": record.kp, + "kd": record.kd, + "cost": record.cost, + "mae_norm": record.mae_norm, + "rmse_norm": record.rmse_norm, + "itae_norm": record.itae_norm, + "overshoot_norm": record.overshoot_norm, + "steady_error_norm": record.steady_error_norm, + "final_error_norm": record.final_error_norm, + "settle_ratio": record.settle_ratio, + "control_rms_norm": record.control_rms_norm, + "control_tv_norm": record.control_tv_norm, + "saturation_ratio": record.saturation_ratio, + } + ) + + +def write_response_csv( + output_path: Path, + trace: TraceData, + simulation: SimulationResult, + output_min: float, + output_max: float, +) -> None: + saturation_margin = 0.01 * max(abs(output_min), abs(output_max), 1.0) + with output_path.open("w", newline="") as handle: + writer = csv.writer(handle) + writer.writerow( + [ + "sample_idx", + "time_s", + "setpoint_velocity", + "simulated_velocity", + "simulated_control", + "saturated", + "observed_velocity", + "observed_control", + ] + ) + for index in range(trace.setpoint.size): + simulated_control = float(simulation.control[index]) + saturated = int( + (simulated_control <= output_min + saturation_margin) + or (simulated_control >= output_max - saturation_margin) + ) + writer.writerow( + [ + index, + index * trace.dt_s, + float(trace.setpoint[index]), + float(simulation.velocity[index]), + simulated_control, + saturated, + "" if trace.observed_velocity is None else float(trace.observed_velocity[index]), + "" if trace.observed_control is None else float(trace.observed_control[index]), + ] + ) + + +def write_tuning_json( + output_path: Path, + model: PlantModel, + trace_count: int, + output_min: float, + output_max: float, + baseline: EvaluationRecord, + best: EvaluationRecord, + method: str, +) -> None: + payload = { + "wheel_id": model.wheel_id, + "run_id": model.run_id, + "trace_count": trace_count, + "method": method, + "ki": 0.0, + "output_min": output_min, + "output_max": output_max, + "plant_model": { + "source_path": str(model.source_path), + "dt_s": model.dt_s, + "a": model.a, + "b": model.b, + "c": model.c, + "delay_samples": model.delay_samples, + "fit_percent": model.fit_percent, + }, + "baseline": record_to_dict(baseline), + "best": record_to_dict(best), + "improvement": { + "absolute_cost_reduction": baseline.cost - best.cost, + "relative_cost_reduction": 0.0 if baseline.cost == 0.0 else (baseline.cost - best.cost) / baseline.cost, + }, + } + output_path.write_text(json.dumps(payload, indent=2, sort_keys=True) + "\n") + + +def record_to_dict(record: EvaluationRecord) -> dict[str, float | str | int]: + return { + "kp": record.kp, + "ki": 0.0, + "kd": record.kd, + "cost": record.cost, + "mae_norm": record.mae_norm, + "rmse_norm": record.rmse_norm, + "itae_norm": record.itae_norm, + "overshoot_norm": record.overshoot_norm, + "steady_error_norm": record.steady_error_norm, + "final_error_norm": record.final_error_norm, + "settle_ratio": record.settle_ratio, + "control_rms_norm": record.control_rms_norm, + "control_tv_norm": record.control_tv_norm, + "saturation_ratio": record.saturation_ratio, + "method": record.method, + "evaluation_index": record.evaluation_index, + } + + +def write_summary_csv(output_path: Path, outcomes: list[TuningOutcome]) -> None: + fieldnames = [ + "wheel_id", + "run_id", + "trace_count", + "method", + "seed_kp", + "seed_kd", + "best_kp", + "best_kd", + "baseline_cost", + "best_cost", + "absolute_cost_reduction", + "relative_cost_reduction", + "output_min", + "output_max", + "result_dir", + ] + with output_path.open("w", newline="") as handle: + writer = csv.DictWriter(handle, fieldnames=fieldnames) + writer.writeheader() + for outcome in outcomes: + baseline = outcome.baseline + best = outcome.best + writer.writerow( + { + "wheel_id": outcome.model.wheel_id, + "run_id": outcome.model.run_id, + "trace_count": outcome.trace_count, + "method": best.method, + "seed_kp": baseline.kp, + "seed_kd": baseline.kd, + "best_kp": best.kp, + "best_kd": best.kd, + "baseline_cost": baseline.cost, + "best_cost": best.cost, + "absolute_cost_reduction": baseline.cost - best.cost, + "relative_cost_reduction": 0.0 if baseline.cost == 0.0 else (baseline.cost - best.cost) / baseline.cost, + "output_min": outcome.output_min, + "output_max": outcome.output_max, + "result_dir": str(outcome.output_dir), + } + ) + + +def main() -> int: + args = parse_args() + model_input = Path(args.model_input) + output_dir = Path(args.output_dir) if args.output_dir else default_output_dir() + output_dir.mkdir(parents=True, exist_ok=True) + + if args.sample_time_ms <= 0.0: + print("--sample-time-ms must be positive", file=sys.stderr) + return 2 + if args.step_duration_ms <= 0.0 or args.pre_roll_ms < 0.0: + print("--step-duration-ms must be positive and --pre-roll-ms must be non-negative", file=sys.stderr) + return 2 + + try: + models = load_models(model_input, args.wheel_id, args.run_id) + except Exception as exc: + print(f"failed to load model input: {exc}", file=sys.stderr) + return 2 + + trace_map = None + if args.trace_csv: + try: + trace_map = load_trace_map(Path(args.trace_csv), args.sample_time_ms / 1000.0) + except Exception as exc: + print(f"failed to load trace CSV: {exc}", file=sys.stderr) + return 2 + + rng = random.Random(args.random_seed) + outcomes: list[TuningOutcome] = [] + + for model in models: + try: + traces = select_traces( + model=model, + trace_map=trace_map, + trace_scope=args.trace_scope, + step_setpoint=args.step_setpoint, + step_duration_ms=args.step_duration_ms, + pre_roll_ms=args.pre_roll_ms, + ) + outcome = tune_one_model( + model=model, + traces=traces, + args=args, + rng=rng, + output_dir=output_dir, + ) + except Exception as exc: + print(f"[error] {model.wheel_id}/run{model.run_id}: {exc}", file=sys.stderr) + continue + + outcomes.append(outcome) + if not args.quiet: + print( + ( + f"[ok] {model.wheel_id}/run{model.run_id}: " + f"kp={outcome.best.kp:.9g}, kd={outcome.best.kd:.9g}, " + f"cost={outcome.best.cost:.6f}, " + f"baseline={outcome.baseline.cost:.6f}, " + f"dir={outcome.output_dir}" + ) + ) + + if not outcomes: + print("no model was successfully tuned", file=sys.stderr) + return 1 + + summary_path = output_dir / "tuning_summary.csv" + write_summary_csv(summary_path, outcomes) + if not args.quiet: + print(f"summary: {summary_path}") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) From 65e7038de9b47ddad060b6c104a840cab40bdbe0 Mon Sep 17 00:00:00 2001 From: zhzy-star <2807406212@qq.com> Date: Mon, 23 Mar 2026 23:07:37 +0800 Subject: [PATCH 12/32] Update putter_controller: demo --- .../controller/shooting/putter_controller.cpp | 55 ++++++++++--------- 1 file changed, 28 insertions(+), 27 deletions(-) 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 b1b621c1..fd645577 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 @@ -130,18 +130,8 @@ class PutterController *bullet_feeder_control_torque_ = bullet_feeder_velocity_pid_.update(velocity_err); if (!bullet_feeder_cool_down_) { - if (*photoelectric_sensor_status_) { - RCLCPP_INFO( - get_logger(), "Photoelectric Sensor Triggered during Jam Protection"); - *bullet_feeder_control_torque_ = 0.; - bullet_feeder_cool_down_ = 0; - // 认为是满链状态，直接设置为预装弹完成 - set_preloaded(); - } else { - RCLCPP_INFO(get_logger(), "Jamming Solved, Retrying..."); - set_preloading(); - // 认为是卡弹状态，继续保持在后退位置直到冷却结束 - } + RCLCPP_INFO(get_logger(), "Jamming Solved, Retrying..."); + set_preloading(); } } else { // 正常运行模式：摩擦轮就绪时才允许发射 @@ -156,7 +146,14 @@ class PutterController if ( // *control_bullet_allowance_limited_by_heat_ > 0 && shoot_stage_ == ShootStage::PRELOADED) - set_shooting(); + if (last_photoelectric_sensor_status_ + && *photoelectric_sensor_status_) { + RCLCPP_INFO( + get_logger(), "Photoelectric sensor triggered, shooting!"); + set_shooting(); + } else { + set_preloading(); + } } } if (shoot_stage_ == ShootStage::UPDATING) { @@ -182,12 +179,10 @@ class PutterController update_jam_detection(); } else if (shoot_stage_ == ShootStage::SHOOTING) { - // 发射状态：供弹盘不发力 - if (!last_shoot_flag_) { - bullet_feeder_control_angle_ = *bullet_feeder_angle_; - last_shoot_flag_ = true; - } *bullet_feeder_control_torque_ = 0; + RCLCPP_INFO( + get_logger(), "Shooting! Angle: %.2f, Velocity: %.2f", + *bullet_feeder_angle_, *bullet_feeder_velocity_); } else { // 其他状态：角度环保持角度不变防止弹链退弹 double velocity_err = @@ -201,9 +196,10 @@ class PutterController if (shoot_stage_ == ShootStage::SHOOTING) { // 发射状态：检测子弹是否发出 if (*putter_angle_ - putter_startpoint >= putter_stroke_) { - if (!shooted){ - RCLCPP_INFO(get_logger(),"%f", *putter_angle_); + if (!shooted) { + RCLCPP_INFO(get_logger(), "%f", *putter_angle_); } + last_photoelectric_sensor_status_ = false; shooted = true; } @@ -216,7 +212,6 @@ class PutterController *putter_control_torque_ = 0.; set_updating(); shooted = false; - last_shoot_flag_ = false; } else { *putter_control_torque_ = putter_return_velocity_pid_.update(-80. - *putter_velocity_); @@ -257,6 +252,7 @@ class PutterController last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; last_mouse_ = rmcs_msgs::Mouse::zero(); last_keyboard_ = rmcs_msgs::Keyboard::zero(); + last_photoelectric_sensor_status_ = false; bullet_feeder_reset(); overdrive_mode_ = false; @@ -287,6 +283,9 @@ class PutterController void set_preloading() { // RCLCPP_INFO(get_logger(), "PRELOADING"); + if (*photoelectric_sensor_status_) { + last_photoelectric_sensor_status_ = true; + } shoot_stage_ = ShootStage::PRELOADING; // 盲拨方案：直接增加目标角度 if (!std::isnan(bullet_feeder_control_angle_)) { @@ -297,6 +296,9 @@ class PutterController void set_preloaded() { // RCLCPP_INFO(get_logger(), "PRELOADED"); + if (*photoelectric_sensor_status_) { + last_photoelectric_sensor_status_ = true; + } shoot_stage_ = ShootStage::PRELOADED; last_preload_flag_ = false; } @@ -320,14 +322,13 @@ class PutterController } else { bullet_feeder_faulty_count_ = 0; } - if (bullet_feeder_faulty_count_ >= 1000) + if (bullet_feeder_faulty_count_ >= 500) enter_jam_protection(); } void update_putter_jam_detection() { if ((*putter_control_torque_ > -0.03 && shoot_stage_ == ShootStage::PRELOADING) - || ((*putter_control_torque_ < 0.08) - && shoot_stage_ == ShootStage::SHOOTING) + || ((*putter_control_torque_ < 0.08) && shoot_stage_ == ShootStage::SHOOTING) || std::isnan(*putter_control_torque_)) { putter_faulty_count_ = 0; return; @@ -344,7 +345,7 @@ class PutterController putter_startpoint = *putter_angle_; } else { // 发射状态下检测到堵转：认为子弹已发出 - RCLCPP_INFO(get_logger(),"dd"); + RCLCPP_INFO(get_logger(), "dd"); shooted = true; } } @@ -455,6 +456,7 @@ class PutterController InputInterface photoelectric_sensor_status_; InputInterface bullet_fired_; bool shooted{false}; + bool last_photoelectric_sensor_status_ = false; InputInterface friction_ready_; @@ -477,7 +479,6 @@ class PutterController InputInterface control_bullet_allowance_limited_by_heat_; bool last_preload_flag_ = false; - bool last_shoot_flag_ = false; bool putter_initialized = false; int putter_faulty_count_ = 0; @@ -488,7 +489,7 @@ class PutterController pid::PidCalculator putter_velocity_pid_; enum class ShootStage { PRELOADING, PRELOADED, SHOOTING, UPDATING }; - ShootStage shoot_stage_ = ShootStage::PRELOADED; + ShootStage shoot_stage_ = ShootStage::PRELOADING; double bullet_feeder_control_angle_ = nan_; pid::PidCalculator bullet_feeder_velocity_pid_; From e38d60e3e615577f83da2a3dc14f7d3348c5f348 Mon Sep 17 00:00:00 2001 From: Fin_Resect Date: Wed, 1 Apr 2026 21:08:34 +0800 Subject: [PATCH 13/32] add climbable-infantry chassis control --- .../config/climbable-infantry.yaml | 283 +++++++++++++ rmcs_ws/src/rmcs_bringup/config/test.yaml | 58 --- rmcs_ws/src/rmcs_core/librmcs | 1 + rmcs_ws/src/rmcs_core/plugins.xml | 3 + .../src/hardware/climbable-infantry.cpp | 384 ++++++++++++++++++ 5 files changed, 671 insertions(+), 58 deletions(-) create mode 100644 rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml delete mode 100644 rmcs_ws/src/rmcs_bringup/config/test.yaml create mode 160000 rmcs_ws/src/rmcs_core/librmcs create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml new file mode 100644 index 00000000..a6094346 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml @@ -0,0 +1,283 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::ClimbableInfantry -> Infantry_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::pid::PidController -> pitch_velocity_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::shooting::ShootingRecorder -> shooting_recorder + + - 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 + +Infantry_hardware: + ros__parameters: + board_serial_bottom_board: "D4-6A83-030D-F3D0-9103-F8A5-1246" + # bottom_yaw_motor_zero_point: 52508 + # pitch_motor_zero_point: 40651 #35701 + # top_yaw_motor_zero_point: 58744 + # viewer_motor_zero_point: 3030 + # external_imu_port: /dev/ttyUSB0 + # bullet_feeder_motor_zero_point: 15111 + left_front_zero_point: 4516 + left_back_zero_point: 3841 + right_back_zero_point: 5159 + right_front_zero_point: 7190 + # 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: + - /gimbal/putter/velocity + - /gimbal/first_left_friction/velocity + - /gimbal/first_second_friction/velocity + - /gimbal/second_left_friction/velocity + - /gimbal/second_right_friction/velocity + - /gimbal/first_left_friction/control_torque + - /gimbal/first_second_friction/control_torque + - /gimbal/second_left_friction/control_torque + - /gimbal/second_right_friction/control_torque + # - /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/pitch/control_torque + # - /gimbal/pitch/velocity + # - /gimbal/pitch/control_velocity + # - /gimbal/pitch/control_angle_error + # - /gimbal/bullet_feeder/phase_zero + # - /gimbal/bullet_feeder/alignment_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: 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: 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 + front_kd: 0.5 + back_kp: 1.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: 30.0 #28.00 + ki: 0.0 + 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: 15.0 #45.00 + ki: 0.00 + kd: 2.0 #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: + - 375.00 + - 375.00 + - 520.00 + - 520.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: 4 + aim_velocity: 11.8 + log_mode: 1 # 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.0016 + ki: 0.00 + kd: 0.001 + +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.0016 + ki: 0.00 + kd: 0.001 + +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.001 + ki: 0.00 + kd: 0.001 + +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.001 + ki: 0.00 + kd: 0.00 + +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_bringup/config/test.yaml b/rmcs_ws/src/rmcs_bringup/config/test.yaml deleted file mode 100644 index ebf295ae..00000000 --- a/rmcs_ws/src/rmcs_bringup/config/test.yaml +++ /dev/null @@ -1,58 +0,0 @@ -rmcs_executor: - ros__parameters: - update_rate: 1000.0 - components: - - rmcs_core::hardware::SteeringHero -> hero_hardware - # - rmcs_core::controller::pid::PidController -> test_pid_controller - - # - 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::pid::PidController -> pitch_velocity_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::shooting::ShootingRecorder -> shooting_recorder - - # - 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: - board_serial_bottom_board_two: "D4-3674-7174-8768-879E-E44A-3931" - test: 6.0 - bottom_yaw_motor_zero_point: 52508 - pitch_motor_zero_point: 35701 - top_yaw_motor_zero_point: 58744 - viewer_motor_zero_point: 3030 - external_imu_port: /dev/ttyUSB0 - bullet_feeder_motor_zero_point: 15111 - left_front_zero_point: 4433 - right_front_zero_point: 7177 - left_back_zero_point: 6919 - right_back_zero_point: 5120 - # left_front_zero_point: 3473 - # left_back_zero_point: 6124 - # right_back_zero_point: 6157 - # right_front_zero_point: 2723 diff --git a/rmcs_ws/src/rmcs_core/librmcs b/rmcs_ws/src/rmcs_core/librmcs new file mode 160000 index 00000000..eff14b71 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/librmcs @@ -0,0 +1 @@ +Subproject commit eff14b71e334baaaa462a83c7e6d834886b10168 diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 4ab6d82b..27ebbc06 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -14,6 +14,9 @@ Test plugin. + + Test plugin. + Test plugin. diff --git a/rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp new file mode 100644 index 00000000..1ce45740 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp @@ -0,0 +1,384 @@ +#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" +#include "hardware/device/supercap.hpp" + +namespace rmcs_core::hardware { + +class ClimbableInfantry + : public rmcs_executor::Component + , public rclcpp::Node { +public: + ClimbableInfantry() + : Node( + get_component_name(), + rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) + , 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)); + }); + + bottom_board_ = std::make_unique( + *this, *command_component_, get_parameter("board_serial_bottom_board").as_string()); + + tf_->set_transform( + Eigen::Translation3d{0.06603, 0.0, 0.082}); + } + + ClimbableInfantry(const ClimbableInfantry&) = delete; + ClimbableInfantry& operator=(const ClimbableInfantry&) = delete; + ClimbableInfantry(ClimbableInfantry&&) = delete; + ClimbableInfantry& operator=(ClimbableInfantry&&) = delete; + + ~ClimbableInfantry() override = default; + + void update() override { bottom_board_->update(); } + + void command_update() { bottom_board_->command_update(); } + +private: + void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { + RCLCPP_INFO( + get_logger(), "[chassis calibration] left front steering offset: %d", + bottom_board_->chassis_steering_motors_[0].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[chassis calibration] left back steering offset: %d", + bottom_board_->chassis_steering_motors_[1].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()); + } + + class ClimbableInfantryCommand : public rmcs_executor::Component { + public: + explicit ClimbableInfantryCommand(ClimbableInfantry& infantry_) + : infantry_(infantry_) {} + + void update() override { infantry_.command_update(); } + + ClimbableInfantry& infantry_; + }; + std::shared_ptr command_component_; + + class BottomBoard final : private librmcs::agent::CBoard { + public: + friend class ClimbableInfantry; + explicit BottomBoard( + ClimbableInfantry& climbable_infantry, + ClimbableInfantryCommand& climbable_infantry_command, + std::string_view board_serial = {}) + : librmcs::agent::CBoard(board_serial) + , logger_(climbable_infantry.get_logger()) + , imu_(1000, 0.2, 0.0) + , dr16_(climbable_infantry) + , supercap_(climbable_infantry, climbable_infantry_command) + + , chassis_front_climber_motor_( + {climbable_infantry, climbable_infantry_command, + "/chassis/climber/left_front_motor"}, + {climbable_infantry, climbable_infantry_command, + "/chassis/climber/right_front_motor"}) + , chassis_back_climber_motor_( + {climbable_infantry, climbable_infantry_command, + "/chassis/climber/left_back_motor"}, + {climbable_infantry, climbable_infantry_command, + "/chassis/climber/right_back_motor"}) + , chassis_steering_motors_( + {climbable_infantry, climbable_infantry_command, "/chassis/left_front_steering"}, + {climbable_infantry, climbable_infantry_command, "/chassis/left_back_steering"}, + {climbable_infantry, climbable_infantry_command, "/chassis/right_back_steering"}, + {climbable_infantry, climbable_infantry_command, "/chassis/right_front_steering"}) + , chassis_wheel_motors_( + {climbable_infantry, climbable_infantry_command, "/chassis/left_front_wheel"}, + {climbable_infantry, climbable_infantry_command, "/chassis/left_back_wheel"}, + {climbable_infantry, climbable_infantry_command, "/chassis/right_back_wheel"}, + {climbable_infantry, climbable_infantry_command, "/chassis/right_front_wheel"}) { + + /* 舵 */ + chassis_steering_motors_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_encoder_zero_point( + static_cast( + climbable_infantry.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( + climbable_infantry.get_parameter("left_back_zero_point").as_int())) + .set_reversed()); + chassis_steering_motors_[2].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_encoder_zero_point( + static_cast( + climbable_infantry.get_parameter("right_back_zero_point").as_int())) + .set_reversed()); + chassis_steering_motors_[3].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_encoder_zero_point( + static_cast( + climbable_infantry.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.)); + chassis_wheel_motors_[2].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(2232. / 169.)); + chassis_wheel_motors_[3].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .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.)); + climbable_infantry.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; + }; + + climbable_infantry.register_output( + "/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); + climbable_infantry.register_output("/chassis/pitch_imu", chassis_pitch_imu_, 0.0); + } + BottomBoard(const BottomBoard&) = delete; + BottomBoard& operator=(const BottomBoard&) = delete; + BottomBoard(BottomBoard&&) = delete; + BottomBoard& operator=(BottomBoard&&) = delete; + + ~BottomBoard() final = default; + + void update() { + imu_.update_status(); + dr16_.update_status(); + supercap_.update_status(); + + *chassis_yaw_velocity_imu_ = imu_.gz(); + *chassis_pitch_imu_ = -std::asin(2.0 * (imu_.q0() * imu_.q2() - imu_.q3() * imu_.q1())); + + RCLCPP_INFO( + logger_, "[chassis calibration] left front steering offset: %d", + chassis_steering_motors_[0].calibrate_zero_point()); + RCLCPP_INFO( + logger_, "[chassis calibration] left back steering offset: %d", + chassis_steering_motors_[1].calibrate_zero_point()); + RCLCPP_INFO( + logger_, "[chassis calibration] right back steering offset: %d", + chassis_steering_motors_[2].calibrate_zero_point()); + RCLCPP_INFO( + logger_, "[chassis calibration] right front steering offset: %d", + chassis_steering_motors_[3].calibrate_zero_point()); + + // 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() { + 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.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 = 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(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_[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); + } else if (can_id == 0x204) { + chassis_wheel_motors_[3].store_status(data.can_data); + } + 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 == 0x300) { + // supercap_.store_status(data.can_data); + // } + } + + 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(data.can_data); + } else if (can_id == 0x204) { + chassis_front_climber_motor_[1].store_status(data.can_data); + } else if (can_id == 0x201) { + chassis_back_climber_motor_[0].store_status(data.can_data); + } else if (can_id == 0x202) { + chassis_back_climber_motor_[1].store_status(data.can_data); + } + } + + 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 librmcs::data::UartDataView& data) override { + dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + } + + void accelerometer_receive_callback( + const librmcs::data::AccelerometerDataView& data) override { + imu_.store_accelerometer_status(data.x, data.y, data.z); + } + + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + imu_.store_gyroscope_status(data.x, data.y, data.z); + } + + rclcpp::Logger logger_; + + short enable = 0; + + device::Bmi088 imu_; + + device::Dr16 dr16_; + device::Supercap supercap_; + + device::DjiMotor chassis_front_climber_motor_[2]; + device::DjiMotor chassis_back_climber_motor_[2]; + device::DjiMotor chassis_steering_motors_[4]; + device::DjiMotor chassis_wheel_motors_[4]; + + rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; + OutputInterface referee_serial_; + + OutputInterface chassis_yaw_velocity_imu_; + OutputInterface chassis_pitch_imu_; + OutputInterface gimbal_yaw_velocity_imu_; + OutputInterface gimbal_pitch_velocity_imu_; + }; + + OutputInterface tf_; + + rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; + + std::shared_ptr bottom_board_; +}; + +} // namespace rmcs_core::hardware + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::ClimbableInfantry, rmcs_executor::Component) From d96fc662735da218fb9dd5c66d5070f0902d241f Mon Sep 17 00:00:00 2001 From: Fin_Resect Date: Sat, 4 Apr 2026 09:28:40 +0800 Subject: [PATCH 14/32] Reduced control frequency --- .../src/hardware/climbable-infantry.cpp | 143 ++++++++++-------- 1 file changed, 84 insertions(+), 59 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp index 1ce45740..db5da90b 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp @@ -231,10 +231,10 @@ class ClimbableInfantry logger_, "[chassis calibration] right front steering offset: %d", chassis_steering_motors_[3].calibrate_zero_point()); - // 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(); + 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(); @@ -243,43 +243,59 @@ class ClimbableInfantry } 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.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(), - }); + if (can1_transmission_mode_) { + 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 = 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(), - // }); + 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(), + }); + } else { + auto builder = start_transmit(); + + builder.can1_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(), + }); + } + can1_transmission_mode_ = !can1_transmission_mode_; } private: @@ -287,24 +303,32 @@ class ClimbableInfantry 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_[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); - } else if (can_id == 0x204) { - chassis_wheel_motors_[3].store_status(data.can_data); - } - 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); + + if (!can1_transmission_mode_) { + 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); + } else if (can_id == 0x204) { + chassis_wheel_motors_[3].store_status(data.can_data); + } else if (can_id == 0x300) { + supercap_.store_status(data.can_data); + } + + } else { + 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); + } } + can1_receive_callback_mode_ = !can1_receive_callback_mode_; // else if (can_id == 0x300) { // supercap_.store_status(data.can_data); @@ -347,9 +371,10 @@ class ClimbableInfantry imu_.store_gyroscope_status(data.x, data.y, data.z); } - rclcpp::Logger logger_; + bool can1_transmission_mode_ = true; + bool can1_receive_callback_mode_ = true; - short enable = 0; + rclcpp::Logger logger_; device::Bmi088 imu_; From 3a1b7af0d4d4ac432303cff8e165cb4be4e44bf0 Mon Sep 17 00:00:00 2001 From: Palejoker <2797572751@qq.com> Date: Wed, 8 Apr 2026 02:15:55 +0800 Subject: [PATCH 15/32] fixed climber,add a bottom board --- docs/zh-cn/steering_infantry_can_id_audit.md | 81 ++++ .../config/climbable-infantry.yaml | 34 +- .../chassis/chassis_climber_controller.cpp | 10 +- .../src/hardware/climbable-infantry.cpp | 360 +++++++++++------- 4 files changed, 318 insertions(+), 167 deletions(-) create mode 100644 docs/zh-cn/steering_infantry_can_id_audit.md diff --git a/docs/zh-cn/steering_infantry_can_id_audit.md b/docs/zh-cn/steering_infantry_can_id_audit.md new file mode 100644 index 00000000..2bed724f --- /dev/null +++ b/docs/zh-cn/steering_infantry_can_id_audit.md @@ -0,0 +1,81 @@ +# steering-infantry 电机 CAN ID 对照审计 + +## 范围 +- 文件: `rmcs_ws/src/rmcs_core/src/hardware/steering-infantry.cpp` +- 目标: 收集所有电机的发送报文 ID 与回调报文 ID，反推出电机 ID，并检查是否有偏差。 + +## 反推规则 +- `DjiMotor` 采用 4 路合帧发送（`device::CanPacket8`）。 +- 对于 `can_id = 0x200` 的发送帧: + - `q0 -> 0x201` + - `q1 -> 0x202` + - `q2 -> 0x203` + - `q3 -> 0x204` +- 对于 `can_id = 0x1FF / 0x1FE` 的发送帧（本项目代码中的使用方式）: + - `q0 -> 0x205` + - `q1 -> 0x206` + - `q2 -> 0x207` + - `q3 -> 0x208` +- `LkMotor` 直接使用单独 `can_id` 发送与接收（非 4 路合帧电机映射）。 + +## TopBoard 电机映射 +代码位置: +- 构造/电机定义: `121-137` +- 发送: `212-253` +- 回调: `256-279` + +| 电机 | 发送总线/ID/槽位 | 回调 ID | 反推电机 ID | 结果 | +|---|---|---|---|---| +| `/gimbal/left_friction` | CAN1, `0x200`, `q2` | `0x203` | `3` | 一致 | +| `/gimbal/right_friction` | CAN1, `0x200`, `q3` | `0x204` | `4` | 一致 | +| `/gimbal/bullet_feeder` | CAN1, `0x1FF`, `q0` | `0x205` | `5` | 一致 | +| `/gimbal/yaw` (LK) | CAN2, `0x141` | `0x141` | `0x141` | 一致 | +| `/gimbal/pitch` (LK) | CAN2, `0x142` | `0x142` | `0x142` | 一致 | + +## BottomBoard_one 电机映射 +代码位置: +- 构造/电机定义: `308-328` +- 发送: `396-434` +- 回调: `437-466` + +| 电机 | 发送总线/ID/槽位 | 回调 ID | 反推电机 ID | 结果 | +|---|---|---|---|---| +| `/chassis/right_front_wheel` (`chassis_wheel_motors_[1]`) | CAN1, `0x200`, `q0` | `0x201` | `1` | 一致 | +| `/chassis/left_front_wheel` (`chassis_wheel_motors_[0]`) | CAN1, `0x200`, `q1` | `0x202` | `2` | 一致 | +| `/chassis/left_front_steering` (`chassis_steering_motors_[0]`) | CAN1, `0x1FE`, `q1` | `0x206` | `6` | 一致 | +| `/chassis/right_front_steering` (`chassis_steering_motors_[1]`) | CAN1, `0x1FE`, `q3` | `0x208` | `8` | 一致 | +| `/chassis/climber/left_back_motor` (`chassis_back_climber_motor_[0]`) | CAN2, `0x200`, `q0` | `0x201` | `1` | 一致 | +| `/chassis/climber/right_back_motor` (`chassis_back_climber_motor_[1]`) | CAN2, `0x200`, `q1` | `0x202` | `2` | 一致 | +| `/chassis/climber/left_front_motor` (`chassis_front_climber_motor_[0]`) | CAN2, `0x200`, `q2` | `0x203` | `3` | 一致 | +| `/chassis/climber/right_front_motor` (`chassis_front_climber_motor_[1]`) | CAN2, `0x200`, `q3` | `0x204` | `4` | 一致 | + +## BottomBoard_two 电机映射 +代码位置: +- 构造/电机定义: `491-507` +- 发送: `563-591` +- 回调: `594-610` + +| 电机 | 发送总线/ID/槽位 | 回调 ID | 反推电机 ID | 结果 | +|---|---|---|---|---| +| `/chassis/right_back_wheel` (`chassis_wheel_motors2_[1]`) | CAN1, `0x200`, `q2` | `0x203` | `3` | 一致 | +| `/chassis/left_back_wheel` (`chassis_wheel_motors2_[0]`) | CAN1, `0x200`, `q3` | `0x204` | `4` | 一致 | +| `/chassis/left_back_steering` (`chassis_steering_motors2_[0]`) | CAN1, `0x1FE`, `q0` | `0x205` | `5` | 一致 | +| `/chassis/right_back_steering` (`chassis_steering_motors2_[1]`) | CAN1, `0x1FE`, `q2` | `0x207` | `7` | 一致 | + +## 总结 +- 结论 1: 所有电机都能在代码中找到“发送 -> 回调”的闭环映射，未发现直接冲突或漏接收。 +- 结论 2: 从代码反推，当前电机 ID 规划为: + - TopBoard: 摩擦轮 `3/4`，拨弹 `5`，LK 云台 `0x141/0x142` + - BottomBoard_one: 轮 `1/2`，前转向 `6/8`，攀爬 `1/2/3/4`（CAN2） + - BottomBoard_two: 后轮 `3/4`，后转向 `5/7` + +## 偏差/问题反馈 +- 问题 1（注释已提示）: 文件头标注“电机ID没更改”，说明该映射可能为占位或沿用旧车配置，不能等同于当前实车真值。 +- 问题 2（可维护性）: 多处左右轮顺序与 ID 顺序是“交叉映射”（例如右前轮是 ID1、左前轮是 ID2），逻辑上可行，但后续维护容易误改。 +- 问题 3（非 ID 但会影响状态）: `TopBoard::update()` 中调用了 `gimbal_pitch_motor_.update_status()`，但未调用 `gimbal_yaw_motor_.update_status()`，可能导致 yaw 角度状态不更新。 + +## 建议 +- 用实车实测确认最终 ID: + 1. 在各 `can*_receive_callback` 临时打印所有 `can_id`。 + 2. 单电机动作，建立“物理电机 <-> can_id”表。 + 3. 以实测表覆盖本文件映射，再同步更新发送槽位与回调分发。 diff --git a/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml index a6094346..729fd292 100644 --- a/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml @@ -39,17 +39,18 @@ rmcs_executor: Infantry_hardware: ros__parameters: - board_serial_bottom_board: "D4-6A83-030D-F3D0-9103-F8A5-1246" + board_serial_bottom_board_one: "D4-6A83-030D-F3D0-9103-F8A5-1246" + board_serial_bottom_board_two: "D4-6654-1C0F-7353-AA15-A77B-313D" # bottom_yaw_motor_zero_point: 52508 # pitch_motor_zero_point: 40651 #35701 # top_yaw_motor_zero_point: 58744 # viewer_motor_zero_point: 3030 # external_imu_port: /dev/ttyUSB0 # bullet_feeder_motor_zero_point: 15111 - left_front_zero_point: 4516 - left_back_zero_point: 3841 - right_back_zero_point: 5159 - right_front_zero_point: 7190 + left_front_zero_point: 5816 + left_back_zero_point: 3776 + right_back_zero_point: 3084 + right_front_zero_point: 5077 # left_front_zero_point: 3473 # left_back_zero_point: 6124 # right_back_zero_point: 6157 @@ -58,15 +59,16 @@ Infantry_hardware: value_broadcaster: ros__parameters: forward_list: - - /gimbal/putter/velocity - - /gimbal/first_left_friction/velocity - - /gimbal/first_second_friction/velocity - - /gimbal/second_left_friction/velocity - - /gimbal/second_right_friction/velocity - - /gimbal/first_left_friction/control_torque - - /gimbal/first_second_friction/control_torque - - /gimbal/second_left_friction/control_torque - - /gimbal/second_right_friction/control_torque + - /chassis/pitch_imu + # - /gimbal/putter/velocity + # - /gimbal/first_left_friction/velocity + # - /gimbal/first_second_friction/velocity + # - /gimbal/second_left_friction/velocity + # - /gimbal/second_right_friction/velocity + # - /gimbal/first_left_friction/control_torque + # - /gimbal/first_second_friction/control_torque + # - /gimbal/second_left_friction/control_torque + # - /gimbal/second_right_friction/control_torque # - /chassis/left_front_steering/angle # - /chassis/left_back_steering/angle # - /chassis/right_back_steering/angle @@ -96,8 +98,8 @@ climber_controller: auto_climb_support_retract_velocity: 30.0 sync_coefficient: 0.3 climb_timeout_limit: 1000 - first_stair_approach_pitch: 0.485 - second_stair_approach_pitch: 0.35 + first_stair_approach_pitch: 0.560 + second_stair_approach_pitch: 0.370 back_climber_burst_velocity: 15.0 back_climber_burst_duration: 300 front_kp: 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 76d0ffd0..e426292a 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 @@ -87,7 +87,7 @@ class ChassisClimberController auto keyboard = *keyboard_; auto rotary_knob_switch = *rotary_knob_switch_; - // RCLCPP_INFO(logger_, "%lf", *chassis_pitch_imu_); + // RCLCPP_INFO(logger_, "%lf", *climber_front_left_control_torque_); bool rotary_knob_to_up = (last_rotary_knob_switch_ != Switch::UP && rotary_knob_switch == Switch::UP); @@ -192,9 +192,9 @@ class ChassisClimberController AutoClimbControl update_auto_climb_support_deploy() { AutoClimbControl control{ - .front_track_velocity = 0.0, + .front_track_velocity = track_velocity_max_, .back_climber_velocity = climber_back_control_velocity_abs_, - .override_chassis_vx = 0.2, + .override_chassis_vx = 2.5, }; if (is_back_climber_blocked()) { @@ -303,7 +303,7 @@ class ChassisClimberController // reset_back_safety_counters(); // } - double track_control_velocity = + double track_control_velocity = front_climber_enable_ ? joystick_right_->x() * track_velocity_max_ : nan_; dual_motor_sync_control( @@ -412,7 +412,7 @@ class ChassisClimberController rclcpp::Logger logger_; static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - static constexpr double kAutoClimbApproachVelocity = 1.0; + static constexpr double kAutoClimbApproachVelocity = 2.5; static constexpr double kAutoClimbLeveledPitchThreshold = 0.1; static constexpr double kBackClimberBlockedTorqueThreshold = 0.1; static constexpr double kBackClimberBlockedVelocityThreshold = 0.1; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp index db5da90b..ff41bccc 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp @@ -49,8 +49,11 @@ class ClimbableInfantry gimbal_calibrate_subscription_callback(std::move(msg)); }); - bottom_board_ = std::make_unique( - *this, *command_component_, get_parameter("board_serial_bottom_board").as_string()); + bottom_board_one_ = std::make_unique( + *this, *command_component_, get_parameter("board_serial_bottom_board_one").as_string()); + + bottom_board_two_ = std::make_unique( + *this, *command_component_, get_parameter("board_serial_bottom_board_two").as_string()); tf_->set_transform( Eigen::Translation3d{0.06603, 0.0, 0.082}); @@ -63,24 +66,30 @@ class ClimbableInfantry ~ClimbableInfantry() override = default; - void update() override { bottom_board_->update(); } + void update() override { + bottom_board_one_->update(); + bottom_board_two_->update(); + } - void command_update() { bottom_board_->command_update(); } + void 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(), "[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] left back steering offset: %d", - bottom_board_->chassis_steering_motors_[1].calibrate_zero_point()); + bottom_board_two_->chassis_steering_motors_[0].calibrate_zero_point()); RCLCPP_INFO( get_logger(), "[chassis calibration] right back steering offset: %d", - bottom_board_->chassis_steering_motors_[2].calibrate_zero_point()); + bottom_board_two_->chassis_steering_motors_[1].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_one_->chassis_steering_motors_[1].calibrate_zero_point()); } class ClimbableInfantryCommand : public rmcs_executor::Component { @@ -94,10 +103,10 @@ class ClimbableInfantry }; std::shared_ptr command_component_; - class BottomBoard final : private librmcs::agent::CBoard { + class BottomBoard_one final : private librmcs::agent::CBoard { public: friend class ClimbableInfantry; - explicit BottomBoard( + explicit BottomBoard_one( ClimbableInfantry& climbable_infantry, ClimbableInfantryCommand& climbable_infantry_command, std::string_view board_serial = {}) @@ -106,7 +115,6 @@ class ClimbableInfantry , imu_(1000, 0.2, 0.0) , dr16_(climbable_infantry) , supercap_(climbable_infantry, climbable_infantry_command) - , chassis_front_climber_motor_( {climbable_infantry, climbable_infantry_command, "/chassis/climber/left_front_motor"}, @@ -119,16 +127,11 @@ class ClimbableInfantry "/chassis/climber/right_back_motor"}) , chassis_steering_motors_( {climbable_infantry, climbable_infantry_command, "/chassis/left_front_steering"}, - {climbable_infantry, climbable_infantry_command, "/chassis/left_back_steering"}, - {climbable_infantry, climbable_infantry_command, "/chassis/right_back_steering"}, {climbable_infantry, climbable_infantry_command, "/chassis/right_front_steering"}) , chassis_wheel_motors_( {climbable_infantry, climbable_infantry_command, "/chassis/left_front_wheel"}, - {climbable_infantry, climbable_infantry_command, "/chassis/left_back_wheel"}, - {climbable_infantry, climbable_infantry_command, "/chassis/right_back_wheel"}, {climbable_infantry, climbable_infantry_command, "/chassis/right_front_wheel"}) { - /* 舵 */ chassis_steering_motors_[0].configure( device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} .set_encoder_zero_point( @@ -136,24 +139,12 @@ class ClimbableInfantry climbable_infantry.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( - climbable_infantry.get_parameter("left_back_zero_point").as_int())) - .set_reversed()); - chassis_steering_motors_[2].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_encoder_zero_point( - static_cast( - climbable_infantry.get_parameter("right_back_zero_point").as_int())) - .set_reversed()); - chassis_steering_motors_[3].configure( device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} .set_encoder_zero_point( static_cast( climbable_infantry.get_parameter("right_front_zero_point").as_int())) .set_reversed()); - /* 轮 */ + chassis_wheel_motors_[0].configure( device::DjiMotor::Config{device::DjiMotor::Type::kM3508} .set_reversed() @@ -162,15 +153,6 @@ class ClimbableInfantry device::DjiMotor::Config{device::DjiMotor::Type::kM3508} .set_reversed() .set_reduction_ratio(2232. / 169.)); - chassis_wheel_motors_[2].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)); - chassis_wheel_motors_[3].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)); - /* 爬 */ chassis_front_climber_motor_[0].configure( device::DjiMotor::Config{device::DjiMotor::Type::kM3508} @@ -181,12 +163,13 @@ class ClimbableInfantry chassis_back_climber_motor_[0].configure( device::DjiMotor::Config{device::DjiMotor::Type::kM3508} .enable_multi_turn_angle() - .set_reduction_ratio(19.)); + .set_reduction_ratio(19.) + .set_reversed()); chassis_back_climber_motor_[1].configure( device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() .enable_multi_turn_angle() .set_reduction_ratio(19.)); + climbable_infantry.register_output("/referee/serial", referee_serial_); referee_serial_->read = [this](std::byte* buffer, size_t size) { return referee_ring_buffer_receive_.pop_front_n( @@ -203,12 +186,13 @@ class ClimbableInfantry "/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); climbable_infantry.register_output("/chassis/pitch_imu", chassis_pitch_imu_, 0.0); } - BottomBoard(const BottomBoard&) = delete; - BottomBoard& operator=(const BottomBoard&) = delete; - BottomBoard(BottomBoard&&) = delete; - BottomBoard& operator=(BottomBoard&&) = delete; - ~BottomBoard() final = default; + 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 = default; void update() { imu_.update_status(); @@ -217,19 +201,14 @@ class ClimbableInfantry *chassis_yaw_velocity_imu_ = imu_.gz(); *chassis_pitch_imu_ = -std::asin(2.0 * (imu_.q0() * imu_.q2() - imu_.q3() * imu_.q1())); + - RCLCPP_INFO( - logger_, "[chassis calibration] left front steering offset: %d", - chassis_steering_motors_[0].calibrate_zero_point()); - RCLCPP_INFO( - logger_, "[chassis calibration] left back steering offset: %d", - chassis_steering_motors_[1].calibrate_zero_point()); - RCLCPP_INFO( - logger_, "[chassis calibration] right back steering offset: %d", - chassis_steering_motors_[2].calibrate_zero_point()); - RCLCPP_INFO( - logger_, "[chassis calibration] right front steering offset: %d", - chassis_steering_motors_[3].calibrate_zero_point()); + // RCLCPP_INFO( + // logger_, "[chassis calibration] left front steering offset: %lf", + // *chassis_pitch_imu_); + // RCLCPP_INFO( + // logger_, "[chassis calibration] right front steering offset: %d", + // chassis_steering_motors_[1].calibrate_zero_point()); chassis_front_climber_motor_[0].update_status(); chassis_front_climber_motor_[1].update_status(); @@ -243,59 +222,55 @@ class ClimbableInfantry } void command_update() { - if (can1_transmission_mode_) { - 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(), - }); + auto builder = start_transmit(); + + builder.can1_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[0].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + chassis_wheel_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(), - }); - } else { - auto builder = start_transmit(); - - builder.can1_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(), - }); - } - can1_transmission_mode_ = !can1_transmission_mode_; + builder.can1_transmit({ + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + chassis_steering_motors_[0].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + chassis_steering_motors_[1].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 = 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: @@ -304,35 +279,17 @@ class ClimbableInfantry return; auto can_id = data.can_id; - if (!can1_transmission_mode_) { - 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); - } else if (can_id == 0x204) { - chassis_wheel_motors_[3].store_status(data.can_data); - } else if (can_id == 0x300) { - supercap_.store_status(data.can_data); - } - - } else { - 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); - } + if (can_id == 0x201) { + chassis_wheel_motors_[0].store_status(data.can_data); + } else if (can_id == 0x204) { + chassis_wheel_motors_[1].store_status(data.can_data); + } else if (can_id == 0x205) { + chassis_steering_motors_[0].store_status(data.can_data); + } else if (can_id == 0x208) { + chassis_steering_motors_[1].store_status(data.can_data); + } else if (can_id == 0x300) { + supercap_.store_status(data.can_data); } - can1_receive_callback_mode_ = !can1_receive_callback_mode_; - - // else if (can_id == 0x300) { - // supercap_.store_status(data.can_data); - // } } void can2_receive_callback(const librmcs::data::CanDataView& data) override { @@ -371,35 +328,146 @@ class ClimbableInfantry imu_.store_gyroscope_status(data.x, data.y, data.z); } - bool can1_transmission_mode_ = true; - bool can1_receive_callback_mode_ = true; - rclcpp::Logger logger_; device::Bmi088 imu_; - device::Dr16 dr16_; device::Supercap supercap_; device::DjiMotor chassis_front_climber_motor_[2]; device::DjiMotor chassis_back_climber_motor_[2]; - device::DjiMotor chassis_steering_motors_[4]; - device::DjiMotor chassis_wheel_motors_[4]; + device::DjiMotor chassis_steering_motors_[2]; + device::DjiMotor chassis_wheel_motors_[2]; rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; OutputInterface referee_serial_; OutputInterface chassis_yaw_velocity_imu_; OutputInterface chassis_pitch_imu_; - OutputInterface gimbal_yaw_velocity_imu_; - OutputInterface gimbal_pitch_velocity_imu_; + }; + + class BottomBoard_two final : private librmcs::agent::CBoard { + public: + friend class ClimbableInfantry; + explicit BottomBoard_two( + ClimbableInfantry& climbable_infantry, + ClimbableInfantryCommand& climbable_infantry_command, + std::string_view board_serial = {}) + : librmcs::agent::CBoard(board_serial) + , logger_(climbable_infantry.get_logger()) + , chassis_steering_motors_( + {climbable_infantry, climbable_infantry_command, "/chassis/left_back_steering"}, + {climbable_infantry, climbable_infantry_command, "/chassis/right_back_steering"}) + , chassis_wheel_motors_( + {climbable_infantry, climbable_infantry_command, "/chassis/left_back_wheel"}, + {climbable_infantry, climbable_infantry_command, "/chassis/right_back_wheel"}) { + + chassis_steering_motors_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_encoder_zero_point( + static_cast( + climbable_infantry.get_parameter("left_back_zero_point").as_int())) + .set_reversed()); + chassis_steering_motors_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_encoder_zero_point( + static_cast( + climbable_infantry.get_parameter("right_back_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.)); + } + + 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; + + void update() { + // RCLCPP_INFO( + // logger_, "[chassis calibration] left back steering offset: %d", + // chassis_steering_motors_[0].calibrate_zero_point()); + // RCLCPP_INFO( + // logger_, "[chassis calibration] right back steering offset: %d", + // chassis_steering_motors_[1].calibrate_zero_point()); + + for (auto& motor : chassis_wheel_motors_) + motor.update_status(); + for (auto& motor : chassis_steering_motors_) + motor.update_status(); + } + + void command_update() { + auto builder = start_transmit(); + + builder.can1_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + chassis_wheel_motors_[0].generate_command(), + chassis_wheel_motors_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + + builder.can1_transmit({ + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + chassis_steering_motors_[0].generate_command(), + chassis_steering_motors_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + } + + private: + 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 == 0x202) { + chassis_wheel_motors_[0].store_status(data.can_data); + } else if (can_id == 0x203) { + chassis_wheel_motors_[1].store_status(data.can_data); + } else if (can_id == 0x206) { + chassis_steering_motors_[0].store_status(data.can_data); + } else if (can_id == 0x207) { + chassis_steering_motors_[1].store_status(data.can_data); + } + } + + void can2_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + } + + rclcpp::Logger logger_; + + device::DjiMotor chassis_steering_motors_[2]; + device::DjiMotor chassis_wheel_motors_[2]; }; OutputInterface tf_; rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; - std::shared_ptr bottom_board_; + std::shared_ptr bottom_board_one_; + std::shared_ptr bottom_board_two_; }; } // namespace rmcs_core::hardware From 1bc3d9b5ead8c4e3f842c823a540513e9886105a Mon Sep 17 00:00:00 2001 From: Palejoker <2797572751@qq.com> Date: Wed, 15 Apr 2026 23:49:16 +0800 Subject: [PATCH 16/32] gimbal control test successful --- rmcs_ws/src/hikcamera | 2 +- .../config/climbable-infantry.yaml | 75 +++---- rmcs_ws/src/rmcs_core/CMakeLists.txt | 4 +- rmcs_ws/src/rmcs_core/librmcs | 2 +- .../chassis/chassis_climber_controller.cpp | 2 +- .../src/hardware/climbable-infantry.cpp | 190 +++++++++++++++++- 6 files changed, 220 insertions(+), 55 deletions(-) diff --git a/rmcs_ws/src/hikcamera b/rmcs_ws/src/hikcamera index 815952fd..f0077f03 160000 --- a/rmcs_ws/src/hikcamera +++ b/rmcs_ws/src/hikcamera @@ -1 +1 @@ -Subproject commit 815952fd1388e121d2014956932f8796a1c8cdd1 +Subproject commit f0077f034800bcd0dde4fffeff270b733772a57e diff --git a/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml index 729fd292..b1d044c4 100644 --- a/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml @@ -10,10 +10,11 @@ rmcs_executor: # - 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::pid::PidController -> pitch_velocity_pid_controller + - rmcs_core::controller::gimbal::SimpleGimbalController -> gimbal_controller + - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller + - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller + - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_controller + - rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller # - rmcs_core::controller::gimbal::PlayerViewer -> gimbal_player_viewer_controller # - rmcs_core::controller::pid::ErrorPidController -> viewer_angle_pid_controller @@ -23,8 +24,6 @@ rmcs_executor: # - 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::shooting::ShootingRecorder -> shooting_recorder - rmcs_core::controller::chassis::ChassisController -> chassis_controller @@ -39,10 +38,11 @@ rmcs_executor: Infantry_hardware: ros__parameters: + board_serial_top_board: "todo" board_serial_bottom_board_one: "D4-6A83-030D-F3D0-9103-F8A5-1246" board_serial_bottom_board_two: "D4-6654-1C0F-7353-AA15-A77B-313D" # bottom_yaw_motor_zero_point: 52508 - # pitch_motor_zero_point: 40651 #35701 + # pitch_motor_zero_point: 40651 # top_yaw_motor_zero_point: 58744 # viewer_motor_zero_point: 3030 # external_imu_port: /dev/ttyUSB0 @@ -51,10 +51,6 @@ Infantry_hardware: left_back_zero_point: 3776 right_back_zero_point: 3084 right_front_zero_point: 5077 - # left_front_zero_point: 3473 - # left_back_zero_point: 6124 - # right_back_zero_point: 6157 - # right_front_zero_point: 2723 value_broadcaster: ros__parameters: @@ -93,9 +89,9 @@ value_broadcaster: climber_controller: ros__parameters: front_climber_velocity: 60.0 - back_climber_velocity: 20.0 + back_climber_velocity: 30.0 auto_climb_dash_velocity: 3.0 - auto_climb_support_retract_velocity: 30.0 + auto_climb_support_retract_velocity: 60.0 sync_coefficient: 0.3 climb_timeout_limit: 1000 first_stair_approach_pitch: 0.560 @@ -114,36 +110,6 @@ gimbal_controller: 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 @@ -161,6 +127,23 @@ pitch_velocity_pid_controller: ki: 0.00 kd: 2.0 #1.00 +yaw_angle_pid_controller: + ros__parameters: + measurement: /gimbal/yaw/control_angle_error + control: /gimbal/yaw/control_velocity + kp: 15.0 #28.00 + ki: 0.0 + kd: 0.6 #0.6 + +yaw_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/yaw/velocity_imu + setpoint: /gimbal/yaw/control_velocity + control: /gimbal/yaw/control_torque + kp: 40.0 #45.00 + ki: 0.00 + kd: 0.01 #1.00 + gimbal_player_viewer_controller: ros__parameters: upper_limit: 0.0101 @@ -177,10 +160,8 @@ viewer_angle_pid_controller: friction_wheel_controller: ros__parameters: friction_wheels: - - /gimbal/first_left_friction - - /gimbal/first_right_friction - - /gimbal/second_left_friction - - /gimbal/second_right_friction + - /gimbal/left_friction + - /gimbal/right_friction friction_velocities: - 375.00 - 375.00 diff --git a/rmcs_ws/src/rmcs_core/CMakeLists.txt b/rmcs_ws/src/rmcs_core/CMakeLists.txt index 579561c3..36e7e570 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.0b1/librmcs-sdk-src-3.1.0-beta.1.zip - URL_HASH SHA256=ec68a8b0e9441bd9e35b859139b3875c559944f7c0ecb4a1539bcdc75442fec4 + URL https://github.com/Alliance-Algorithm/librmcs/releases/download/v3.1.0/librmcs-sdk-src-3.1.0.zip + URL_HASH SHA256=07107e251745ddb23f7b3e39edec5d6910be1a197025d167ec9849c5c80dd954 DOWNLOAD_EXTRACT_TIMESTAMP TRUE ) FetchContent_MakeAvailable(librmcs) diff --git a/rmcs_ws/src/rmcs_core/librmcs b/rmcs_ws/src/rmcs_core/librmcs index eff14b71..1347b210 160000 --- a/rmcs_ws/src/rmcs_core/librmcs +++ b/rmcs_ws/src/rmcs_core/librmcs @@ -1 +1 @@ -Subproject commit eff14b71e334baaaa462a83c7e6d834886b10168 +Subproject commit 1347b2109d0c93dbc3cfe5daaa6acd7754987f81 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 e426292a..18802f2c 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 @@ -412,7 +412,7 @@ class ChassisClimberController rclcpp::Logger logger_; static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - static constexpr double kAutoClimbApproachVelocity = 2.5; + static constexpr double kAutoClimbApproachVelocity = 1.8; static constexpr double kAutoClimbLeveledPitchThreshold = 0.1; static constexpr double kBackClimberBlockedTorqueThreshold = 0.1; static constexpr double kBackClimberBlockedVelocityThreshold = 0.1; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp index ff41bccc..8eeb27cf 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp @@ -49,8 +49,11 @@ class ClimbableInfantry gimbal_calibrate_subscription_callback(std::move(msg)); }); + top_board_ = std::make_unique( + *this, *command_component_, get_parameter("board_serial_top_board").as_string()); bottom_board_one_ = std::make_unique( - *this, *command_component_, get_parameter("board_serial_bottom_board_one").as_string()); + *this, *command_component_, *top_board_, + get_parameter("board_serial_bottom_board_one").as_string()); bottom_board_two_ = std::make_unique( *this, *command_component_, get_parameter("board_serial_bottom_board_two").as_string()); @@ -67,17 +70,28 @@ class ClimbableInfantry ~ClimbableInfantry() override = default; void update() override { + top_board_->update(); bottom_board_one_->update(); bottom_board_two_->update(); } void command_update() { + top_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 yaw offset: %ld", + bottom_board_one_->gimbal_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 bullet feeder offset: %ld", + static_cast(bottom_board_two_->gimbal_bullet_feeder_.calibrate_zero_point())); RCLCPP_INFO( get_logger(), "[chassis calibration] left front steering offset: %d", bottom_board_one_->chassis_steering_motors_[0].calibrate_zero_point()); @@ -103,18 +117,143 @@ class ClimbableInfantry }; std::shared_ptr command_component_; + class TopBoard final : private librmcs::agent::CBoard { + public: + friend class ClimbableInfantry; + explicit TopBoard( + ClimbableInfantry& climbable_infantry, + ClimbableInfantryCommand& climbable_infantry_command, + std::string_view board_serial = {}) + : librmcs::agent::CBoard(board_serial) + , tf_(climbable_infantry.tf_) + , imu_(1000, 0.2, 0.0) + , gimbal_pitch_motor_(climbable_infantry, climbable_infantry_command, "/gimbal/pitch") + , gimbal_left_friction_( + climbable_infantry, climbable_infantry_command, "/gimbal/left_friction") + , gimbal_right_friction_( + climbable_infantry, climbable_infantry_command, "/gimbal/right_friction") { + + gimbal_pitch_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( + static_cast( + climbable_infantry.get_parameter("pitch_motor_zero_point").as_int()))); + gimbal_left_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); + gimbal_right_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reduction_ratio(1.) + .set_reversed()); + + climbable_infantry.register_output( + "/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); + climbable_infantry.register_output( + "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); + + imu_.set_coordinate_mapping( + [](double x, double y, double z) { return std::make_tuple(-x, -y, z); }); + } + + TopBoard(const TopBoard&) = delete; + TopBoard& operator=(const TopBoard&) = delete; + TopBoard(TopBoard&&) = delete; + TopBoard& operator=(TopBoard&&) = delete; + + ~TopBoard() final = default; + + double yaw_imu_velocity() const { return imu_.gz(); } + + void update() { + imu_.update_status(); + const Eigen::Quaterniond gimbal_imu_pose{imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()}; + + tf_->set_transform( + gimbal_imu_pose.conjugate()); + + *gimbal_yaw_velocity_imu_ = imu_.gz(); + *gimbal_pitch_velocity_imu_ = imu_.gy(); + + gimbal_pitch_motor_.update_status(); + gimbal_left_friction_.update_status(); + gimbal_right_friction_.update_status(); + + tf_->set_state( + gimbal_pitch_motor_.angle()); + } + + void command_update() { + auto builder = start_transmit(); + + builder.can1_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + gimbal_left_friction_.generate_command(), + gimbal_right_friction_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + + builder.can1_transmit({ + .can_id = 0x142, + .can_data = gimbal_pitch_motor_.generate_command().as_bytes(), + }); + } + + private: + 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) { + gimbal_left_friction_.store_status(data.can_data); + } else if (can_id == 0x202) { + gimbal_right_friction_.store_status(data.can_data); + } else if (can_id == 0x142) { + gimbal_pitch_motor_.store_status(data.can_data); + } + } + + void can2_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + } + + void accelerometer_receive_callback( + const librmcs::data::AccelerometerDataView& data) override { + imu_.store_accelerometer_status(data.x, data.y, data.z); + } + + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + imu_.store_gyroscope_status(data.x, data.y, data.z); + } + + OutputInterface& tf_; + + device::Bmi088 imu_; + device::LkMotor gimbal_pitch_motor_; + device::DjiMotor gimbal_left_friction_; + device::DjiMotor gimbal_right_friction_; + + OutputInterface gimbal_yaw_velocity_imu_; + OutputInterface gimbal_pitch_velocity_imu_; + }; + class BottomBoard_one final : private librmcs::agent::CBoard { public: friend class ClimbableInfantry; explicit BottomBoard_one( ClimbableInfantry& climbable_infantry, - ClimbableInfantryCommand& climbable_infantry_command, + ClimbableInfantryCommand& climbable_infantry_command, TopBoard& top_board, std::string_view board_serial = {}) : librmcs::agent::CBoard(board_serial) + , top_board_(top_board) , logger_(climbable_infantry.get_logger()) , imu_(1000, 0.2, 0.0) , dr16_(climbable_infantry) , supercap_(climbable_infantry, climbable_infantry_command) + , gimbal_yaw_motor_(climbable_infantry, climbable_infantry_command, "/gimbal/yaw") , chassis_front_climber_motor_( {climbable_infantry, climbable_infantry_command, "/chassis/climber/left_front_motor"}, @@ -132,6 +271,11 @@ class ClimbableInfantry {climbable_infantry, climbable_infantry_command, "/chassis/left_front_wheel"}, {climbable_infantry, climbable_infantry_command, "/chassis/right_front_wheel"}) { + gimbal_yaw_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( + static_cast( + climbable_infantry.get_parameter("yaw_motor_zero_point").as_int()))); + chassis_steering_motors_[0].configure( device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} .set_encoder_zero_point( @@ -201,7 +345,6 @@ class ClimbableInfantry *chassis_yaw_velocity_imu_ = imu_.gz(); *chassis_pitch_imu_ = -std::asin(2.0 * (imu_.q0() * imu_.q2() - imu_.q3() * imu_.q1())); - // RCLCPP_INFO( // logger_, "[chassis calibration] left front steering offset: %lf", @@ -214,6 +357,7 @@ class ClimbableInfantry chassis_front_climber_motor_[1].update_status(); chassis_back_climber_motor_[0].update_status(); chassis_back_climber_motor_[1].update_status(); + gimbal_yaw_motor_.update_status(); for (auto& motor : chassis_wheel_motors_) motor.update_status(); @@ -248,6 +392,11 @@ class ClimbableInfantry .as_bytes(), }); + builder.can2_transmit({ + .can_id = 0x141, + .can_data = gimbal_yaw_motor_.generate_command().as_bytes(), + }); + // builder.can1_transmit({ // .can_id = 0x1FE, // .can_data = @@ -305,6 +454,8 @@ class ClimbableInfantry chassis_back_climber_motor_[0].store_status(data.can_data); } else if (can_id == 0x202) { chassis_back_climber_motor_[1].store_status(data.can_data); + } else if (can_id == 0x141) { + gimbal_yaw_motor_.store_status(data.can_data); } } @@ -328,11 +479,13 @@ class ClimbableInfantry imu_.store_gyroscope_status(data.x, data.y, data.z); } + TopBoard& top_board_; rclcpp::Logger logger_; device::Bmi088 imu_; device::Dr16 dr16_; device::Supercap supercap_; + device::LkMotor gimbal_yaw_motor_; device::DjiMotor chassis_front_climber_motor_[2]; device::DjiMotor chassis_back_climber_motor_[2]; @@ -355,6 +508,8 @@ class ClimbableInfantry std::string_view board_serial = {}) : librmcs::agent::CBoard(board_serial) , logger_(climbable_infantry.get_logger()) + , gimbal_bullet_feeder_( + climbable_infantry, climbable_infantry_command, "/gimbal/bullet_feeder") , chassis_steering_motors_( {climbable_infantry, climbable_infantry_command, "/chassis/left_back_steering"}, {climbable_infantry, climbable_infantry_command, "/chassis/right_back_steering"}) @@ -362,6 +517,15 @@ class ClimbableInfantry {climbable_infantry, climbable_infantry_command, "/chassis/left_back_wheel"}, {climbable_infantry, climbable_infantry_command, "/chassis/right_back_wheel"}) { + gimbal_bullet_feeder_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_encoder_zero_point( + static_cast( + climbable_infantry.get_parameter("bullet_feeder_motor_zero_point") + .as_int())) + .set_reversed() + .enable_multi_turn_angle()); + chassis_steering_motors_[0].configure( device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} .set_encoder_zero_point( @@ -400,6 +564,7 @@ class ClimbableInfantry // logger_, "[chassis calibration] right back steering offset: %d", // chassis_steering_motors_[1].calibrate_zero_point()); + gimbal_bullet_feeder_.update_status(); for (auto& motor : chassis_wheel_motors_) motor.update_status(); for (auto& motor : chassis_steering_motors_) @@ -432,6 +597,18 @@ class ClimbableInfantry } .as_bytes(), }); + + builder.can2_transmit({ + .can_id = 0x1FF, + .can_data = + device::CanPacket8{ + gimbal_bullet_feeder_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); } private: @@ -454,10 +631,16 @@ class ClimbableInfantry 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 == 0x205) { + gimbal_bullet_feeder_.store_status(data.can_data); + } } rclcpp::Logger logger_; + device::DjiMotor gimbal_bullet_feeder_; device::DjiMotor chassis_steering_motors_[2]; device::DjiMotor chassis_wheel_motors_[2]; }; @@ -466,6 +649,7 @@ class ClimbableInfantry rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; + std::shared_ptr top_board_; std::shared_ptr bottom_board_one_; std::shared_ptr bottom_board_two_; }; From c5b46d4b9e021f56eb286e4cd76a228a9556b03c Mon Sep 17 00:00:00 2001 From: Palejoker <2797572751@qq.com> Date: Wed, 15 Apr 2026 23:51:34 +0800 Subject: [PATCH 17/32] fine-tune parameters --- .../rmcs_bringup/config/climbable-infantry.yaml | 16 ++++++++-------- .../chassis/chassis_climber_controller.cpp | 2 +- .../src/hardware/climbable-infantry.cpp | 9 +++------ 3 files changed, 12 insertions(+), 15 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml index b1d044c4..16277201 100644 --- a/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml @@ -12,7 +12,7 @@ rmcs_executor: - rmcs_core::controller::gimbal::SimpleGimbalController -> gimbal_controller - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller - - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller + # - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_controller - rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller @@ -38,11 +38,11 @@ rmcs_executor: Infantry_hardware: ros__parameters: - board_serial_top_board: "todo" + board_serial_top_board: "D4-632C-37BC-6A17-ACA2-E3FE-91E9" board_serial_bottom_board_one: "D4-6A83-030D-F3D0-9103-F8A5-1246" board_serial_bottom_board_two: "D4-6654-1C0F-7353-AA15-A77B-313D" - # bottom_yaw_motor_zero_point: 52508 - # pitch_motor_zero_point: 40651 + yaw_motor_zero_point: 45399 + pitch_motor_zero_point: 23003 # top_yaw_motor_zero_point: 58744 # viewer_motor_zero_point: 3030 # external_imu_port: /dev/ttyUSB0 @@ -113,8 +113,8 @@ gimbal_controller: pitch_angle_pid_controller: ros__parameters: measurement: /gimbal/pitch/control_angle_error - control: /gimbal/pitch/control_velocity - kp: 30.0 #28.00 + control: /gimbal/pitch/control_torque + kp: 1.00 ki: 0.0 kd: 0.6 #0.6 @@ -123,9 +123,9 @@ pitch_velocity_pid_controller: measurement: /gimbal/pitch/velocity_imu setpoint: /gimbal/pitch/control_velocity control: /gimbal/pitch/control_torque - kp: 15.0 #45.00 + kp: 15.0 ki: 0.00 - kd: 2.0 #1.00 + kd: 2.0 #2.00 yaw_angle_pid_controller: ros__parameters: 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 18802f2c..5801ca4c 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 @@ -169,7 +169,7 @@ class ChassisClimberController AutoClimbControl control{ .front_track_velocity = track_velocity_max_, .back_climber_velocity = 0.0, - .override_chassis_vx = kAutoClimbApproachVelocity, + .override_chassis_vx = 1.0, }; double pitch = *chassis_pitch_imu_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp index 8eeb27cf..11661fde 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp @@ -136,7 +136,8 @@ class ClimbableInfantry gimbal_pitch_motor_.configure( device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( static_cast( - climbable_infantry.get_parameter("pitch_motor_zero_point").as_int()))); + climbable_infantry.get_parameter("pitch_motor_zero_point").as_int())) + ); gimbal_left_friction_.configure( device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); gimbal_right_friction_.configure( @@ -150,7 +151,7 @@ class ClimbableInfantry "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); imu_.set_coordinate_mapping( - [](double x, double y, double z) { return std::make_tuple(-x, -y, z); }); + [](double x, double y, double z) { return std::make_tuple(x, y, z); }); } TopBoard(const TopBoard&) = delete; @@ -519,10 +520,6 @@ class ClimbableInfantry gimbal_bullet_feeder_.configure( device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_encoder_zero_point( - static_cast( - climbable_infantry.get_parameter("bullet_feeder_motor_zero_point") - .as_int())) .set_reversed() .enable_multi_turn_angle()); From a8e656fea55fc454fde81b0d9abaf5e6d8e15ddc Mon Sep 17 00:00:00 2001 From: Fin_Resect Date: Sat, 18 Apr 2026 23:44:33 +0800 Subject: [PATCH 18/32] fixed gimbal pr --- rmcs_ws/src/hikcamera | 2 +- .../config/climbable-infantry.yaml | 129 +++++----- rmcs_ws/src/rmcs_core/librmcs | 2 +- .../chassis/chassis_climber_controller.cpp | 4 +- .../controller/chassis/chassis_controller.cpp | 2 +- .../controller/shooting/shooting_recorder.cpp | 232 +++--------------- .../src/hardware/climbable-infantry.cpp | 27 +- 7 files changed, 120 insertions(+), 278 deletions(-) diff --git a/rmcs_ws/src/hikcamera b/rmcs_ws/src/hikcamera index f0077f03..815952fd 160000 --- a/rmcs_ws/src/hikcamera +++ b/rmcs_ws/src/hikcamera @@ -1 +1 @@ -Subproject commit f0077f034800bcd0dde4fffeff270b733772a57e +Subproject commit 815952fd1388e121d2014956932f8796a1c8cdd1 diff --git a/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml index 16277201..3581c4b9 100644 --- a/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml @@ -12,19 +12,22 @@ rmcs_executor: - rmcs_core::controller::gimbal::SimpleGimbalController -> gimbal_controller - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller - # - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_controller - rmcs_core::controller::pid::PidController -> yaw_velocity_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::shooting::FrictionWheelController -> friction_wheel_controller + - rmcs_core::controller::shooting::HeatController -> heat_controller + - rmcs_core::controller::shooting::BulletFeederController17mm -> bullet_feeder_controller # - rmcs_core::controller::shooting::ShootingRecorder -> shooting_recorder + - rmcs_core::controller::pid::PidController -> left_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> right_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller + + - rmcs_core::controller::chassis::ChassisController -> chassis_controller - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller @@ -34,15 +37,15 @@ rmcs_executor: # - rmcs_auto_aim::AutoAimInitializer -> auto_aim_initializer # - rmcs_auto_aim::AutoAimController -> auto_aim_controller - # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster Infantry_hardware: ros__parameters: board_serial_top_board: "D4-632C-37BC-6A17-ACA2-E3FE-91E9" board_serial_bottom_board_one: "D4-6A83-030D-F3D0-9103-F8A5-1246" board_serial_bottom_board_two: "D4-6654-1C0F-7353-AA15-A77B-313D" - yaw_motor_zero_point: 45399 - pitch_motor_zero_point: 23003 + yaw_motor_zero_point: 12580 + pitch_motor_zero_point: 21931 # top_yaw_motor_zero_point: 58744 # viewer_motor_zero_point: 3030 # external_imu_port: /dev/ttyUSB0 @@ -55,7 +58,14 @@ Infantry_hardware: value_broadcaster: ros__parameters: forward_list: - - /chassis/pitch_imu + - /gimbal/left_friction/velocity + - /gimbal/right_friction/velocity + # - /chassis/pitch_imu + # - /gimbal/pitch/velocity_imu + # - /gimbal/pitch/angle + # - /gimbal/yaw/angle + # - /chassis/climber/right_back_motor/control_torque + # - /chassis/climber/left_back_motor/control_torque # - /gimbal/putter/velocity # - /gimbal/first_left_friction/velocity # - /gimbal/first_second_friction/velocity @@ -89,13 +99,13 @@ value_broadcaster: climber_controller: ros__parameters: front_climber_velocity: 60.0 - back_climber_velocity: 30.0 - auto_climb_dash_velocity: 3.0 + back_climber_velocity: 15.0 + auto_climb_dash_velocity: 1.0 auto_climb_support_retract_velocity: 60.0 - sync_coefficient: 0.3 + sync_coefficient: 0.6 climb_timeout_limit: 1000 first_stair_approach_pitch: 0.560 - second_stair_approach_pitch: 0.370 + second_stair_approach_pitch: 0.320 back_climber_burst_velocity: 15.0 back_climber_burst_duration: 300 front_kp: 1.0 @@ -107,42 +117,42 @@ climber_controller: gimbal_controller: ros__parameters: - upper_limit: -0.6266 - lower_limit: 0.4629 + upper_limit: -0.62 + lower_limit: 0.48 pitch_angle_pid_controller: ros__parameters: measurement: /gimbal/pitch/control_angle_error - control: /gimbal/pitch/control_torque - kp: 1.00 + control: /gimbal/pitch/control_velocity + kp: 15.0 #5.0 ki: 0.0 - kd: 0.6 #0.6 + kd: 0.0 #1.5 pitch_velocity_pid_controller: ros__parameters: measurement: /gimbal/pitch/velocity_imu setpoint: /gimbal/pitch/control_velocity control: /gimbal/pitch/control_torque - kp: 15.0 + kp: 40.0 # ki: 0.00 - kd: 2.0 #2.00 + kd: 0.8 # yaw_angle_pid_controller: ros__parameters: measurement: /gimbal/yaw/control_angle_error control: /gimbal/yaw/control_velocity - kp: 15.0 #28.00 + kp: 20.0 #20.00 ki: 0.0 - kd: 0.6 #0.6 + kd: 0.8 #0.8 yaw_velocity_pid_controller: ros__parameters: measurement: /gimbal/yaw/velocity_imu setpoint: /gimbal/yaw/control_velocity control: /gimbal/yaw/control_torque - kp: 40.0 #45.00 + kp: 50.0 #50.00 ki: 0.00 - kd: 0.01 #1.00 + kd: 0.0 # gimbal_player_viewer_controller: ros__parameters: @@ -163,58 +173,57 @@ friction_wheel_controller: - /gimbal/left_friction - /gimbal/right_friction friction_velocities: - - 375.00 - - 375.00 - - 520.00 - - 520.00 + - 700.0 + - 700.0 friction_soft_start_stop_time: 1.0 heat_controller: ros__parameters: - heat_per_shot: 100000 + heat_per_shot: 10 reserved_heat: 0 -shooting_recorder: +bullet_feeder_controller: ros__parameters: - friction_wheel_count: 4 - aim_velocity: 11.8 - log_mode: 1 # 1: trigger, 2: timing + bullets_per_feeder_turn: 9.0 + shot_frequency: 30.0 + safe_shot_frequency: 10.0 + eject_frequency: 10.0 + eject_time: 0.05 + deep_eject_frequency: 5.0 + deep_eject_time: 0.2 + single_shot_max_stop_delay: 2.0 -first_left_friction_velocity_pid_controller: +shooting_recorder: ros__parameters: - measurement: /gimbal/first_left_friction/velocity - setpoint: /gimbal/first_left_friction/control_velocity - control: /gimbal/first_left_friction/control_torque - kp: 0.0016 - ki: 0.00 - kd: 0.001 + friction_wheel_count: 2 + log_mode: 1 # 1: trigger, 2: timing -first_right_friction_velocity_pid_controller: +left_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.0016 + measurement: /gimbal/left_friction/velocity + setpoint: /gimbal/left_friction/control_velocity + control: /gimbal/left_friction/control_torque + kp: 0.003436926 #0.003436926 #0.0016 ki: 0.00 - kd: 0.001 + kd: 0.009373434 #0.009373434 #0.001 -second_left_friction_velocity_pid_controller: +right_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.001 + measurement: /gimbal/right_friction/velocity + setpoint: /gimbal/right_friction/control_velocity + control: /gimbal/right_friction/control_torque + kp: 0.003436926 #0.0016 ki: 0.00 - kd: 0.001 + kd: 0.009373434 #0.009373434 #0.001 -second_right_friction_velocity_pid_controller: +bullet_feeder_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.001 - ki: 0.00 - kd: 0.00 + measurement: /gimbal/bullet_feeder/velocity + setpoint: /gimbal/bullet_feeder/control_velocity + control: /gimbal/bullet_feeder/control_torque + kp: 0.7 + ki: 0.0 + kd: 0.0 steering_wheel_controller: ros__parameters: diff --git a/rmcs_ws/src/rmcs_core/librmcs b/rmcs_ws/src/rmcs_core/librmcs index 1347b210..eff14b71 160000 --- a/rmcs_ws/src/rmcs_core/librmcs +++ b/rmcs_ws/src/rmcs_core/librmcs @@ -1 +1 @@ -Subproject commit 1347b2109d0c93dbc3cfe5daaa6acd7754987f81 +Subproject commit eff14b71e334baaaa462a83c7e6d834886b10168 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 5801ca4c..c66f67c2 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 @@ -194,7 +194,7 @@ class ChassisClimberController AutoClimbControl control{ .front_track_velocity = track_velocity_max_, .back_climber_velocity = climber_back_control_velocity_abs_, - .override_chassis_vx = 2.5, + .override_chassis_vx = auto_climb_dash_velocity_, }; if (is_back_climber_blocked()) { @@ -303,7 +303,7 @@ class ChassisClimberController // reset_back_safety_counters(); // } - double track_control_velocity = + double track_control_velocity = front_climber_enable_ ? joystick_right_->x() * track_velocity_max_ : nan_; dual_motor_sync_control( 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 af18ba43..877f5a0e 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 @@ -151,7 +151,7 @@ class ChassisController case rmcs_msgs::ChassisMode::AUTO: break; case rmcs_msgs::ChassisMode::SPIN: { angular_velocity = - 0.6 * (spinning_forward_ ? angular_velocity_max : -angular_velocity_max); + 0.4 * (spinning_forward_ ? angular_velocity_max : -angular_velocity_max); } break; case rmcs_msgs::ChassisMode::STEP_DOWN: { double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); 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 80358510..1cb9de96 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,10 +1,7 @@ -#include -#include #include #include #include #include -#include namespace rmcs_core::controller::shooting { @@ -21,21 +18,27 @@ 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_); - - // if (friction_wheel_count_ == 4) { - // const auto topic = std::array{ - // "/gimbal/first_left_friction/control_velocity", - // "/gimbal/first_right_friction/control_velocity", - // }; - // 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]); - // } + 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++) + register_input(topic[i], friction_wheels_velocity_[i]); + } using namespace std::chrono; auto now = high_resolution_clock::now(); @@ -43,45 +46,12 @@ class ShootingRecorder 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 @@ -94,50 +64,38 @@ class ShootingRecorder return; break; } - 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(); auto log_text = std::string{}; auto timestamp = timestamp_to_string(*shoot_timestamp_); - if (friction_wheel_count_ == 4) { + 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( - "{},{},{:.3f},{:.3f},{:.3f},{:.3f},{:.3f},{:.3f},{:.3f}", *initial_speed_, - (int)velocities.size(), // - velocity_, excellence_rate_, pass_rate_, range_, range2_, velocity_max, - velocity_min); + "{},{},{},{},{},{},{}", timestamp, *initial_speed_, // + *friction_wheels_velocity_[0], *friction_wheels_velocity_[1], + *friction_wheels_velocity_[2], *friction_wheels_velocity_[3], + *fractional_temperature_); } 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(); - } + RCLCPP_INFO(get_logger(), "%s", log_text.c_str()); 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, 2> friction_wheels_velocity_; + std::array, 4> friction_wheels_velocity_; /// @brief For log enum class LogMode { TRIGGER = 1, TIMING = 2 }; @@ -145,26 +103,9 @@ 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; - private: static std::string timestamp_to_string(double timestamp) { auto time = static_cast(timestamp); @@ -181,113 +122,6 @@ class ShootingRecorder 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(); - - if (velocities.size() >= 3) { - range2_ = velocities[int(velocities.size() - 2)] - velocities[1]; - } else { - range2_ = range_; - } - - 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/climbable-infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp index 11661fde..e5308776 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp @@ -52,8 +52,7 @@ class ClimbableInfantry top_board_ = std::make_unique( *this, *command_component_, get_parameter("board_serial_top_board").as_string()); bottom_board_one_ = std::make_unique( - *this, *command_component_, *top_board_, - get_parameter("board_serial_bottom_board_one").as_string()); + *this, *command_component_, get_parameter("board_serial_bottom_board_one").as_string()); bottom_board_two_ = std::make_unique( *this, *command_component_, get_parameter("board_serial_bottom_board_two").as_string()); @@ -134,10 +133,11 @@ class ClimbableInfantry climbable_infantry, climbable_infantry_command, "/gimbal/right_friction") { gimbal_pitch_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( - static_cast( - climbable_infantry.get_parameter("pitch_motor_zero_point").as_int())) - ); + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} + .set_encoder_zero_point( + static_cast( + climbable_infantry.get_parameter("pitch_motor_zero_point").as_int())) + .enable_multi_turn_angle()); gimbal_left_friction_.configure( device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); gimbal_right_friction_.configure( @@ -188,8 +188,8 @@ class ClimbableInfantry .can_id = 0x200, .can_data = device::CanPacket8{ - gimbal_left_friction_.generate_command(), gimbal_right_friction_.generate_command(), + gimbal_left_friction_.generate_command(), device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}, } @@ -208,9 +208,9 @@ class ClimbableInfantry return; auto can_id = data.can_id; if (can_id == 0x201) { - gimbal_left_friction_.store_status(data.can_data); - } else if (can_id == 0x202) { gimbal_right_friction_.store_status(data.can_data); + } else if (can_id == 0x202) { + gimbal_left_friction_.store_status(data.can_data); } else if (can_id == 0x142) { gimbal_pitch_motor_.store_status(data.can_data); } @@ -246,10 +246,10 @@ class ClimbableInfantry friend class ClimbableInfantry; explicit BottomBoard_one( ClimbableInfantry& climbable_infantry, - ClimbableInfantryCommand& climbable_infantry_command, TopBoard& top_board, + ClimbableInfantryCommand& climbable_infantry_command, std::string_view board_serial = {}) : librmcs::agent::CBoard(board_serial) - , top_board_(top_board) + , logger_(climbable_infantry.get_logger()) , imu_(1000, 0.2, 0.0) , dr16_(climbable_infantry) @@ -480,7 +480,6 @@ class ClimbableInfantry imu_.store_gyroscope_status(data.x, data.y, data.z); } - TopBoard& top_board_; rclcpp::Logger logger_; device::Bmi088 imu_; @@ -596,7 +595,7 @@ class ClimbableInfantry }); builder.can2_transmit({ - .can_id = 0x1FF, + .can_id = 0x200, .can_data = device::CanPacket8{ gimbal_bullet_feeder_.generate_command(), @@ -630,7 +629,7 @@ class ClimbableInfantry return; auto can_id = data.can_id; - if (can_id == 0x205) { + if (can_id == 0x201) { gimbal_bullet_feeder_.store_status(data.can_data); } } From 70340d84c0eb31e1da5d235f476396bc79e081a8 Mon Sep 17 00:00:00 2001 From: Fin_Resect Date: Tue, 21 Apr 2026 23:40:53 +0800 Subject: [PATCH 19/32] Refactor and update parameters for gimbal and climber controllers .Enabled shooting --- .../config/climbable-infantry.yaml | 35 +- .../chassis_climber_controller copy.cpp | 573 ++++++++++++++++++ .../chassis/chassis_climber_controller.cpp | 47 +- .../controller/chassis/chassis_controller.cpp | 4 +- ...mbable_infantry_gimbal_controller copy.cpp | 97 +++ .../shooting/friction_wheel_controller.cpp | 5 +- .../controller/shooting/heat_controller.cpp | 10 +- .../controller/shooting/shooting_recorder.cpp | 8 +- 8 files changed, 736 insertions(+), 43 deletions(-) create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller copy.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/gimbal/climbable_infantry_gimbal_controller copy.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml index 3581c4b9..1ebe088c 100644 --- a/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml @@ -6,8 +6,8 @@ rmcs_executor: - 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::interaction::Ui -> referee_ui + - rmcs_core::referee::app::ui::Infantry -> referee_ui_hero - rmcs_core::referee::Command -> referee_command - rmcs_core::controller::gimbal::SimpleGimbalController -> gimbal_controller @@ -44,7 +44,7 @@ Infantry_hardware: board_serial_top_board: "D4-632C-37BC-6A17-ACA2-E3FE-91E9" board_serial_bottom_board_one: "D4-6A83-030D-F3D0-9103-F8A5-1246" board_serial_bottom_board_two: "D4-6654-1C0F-7353-AA15-A77B-313D" - yaw_motor_zero_point: 12580 + yaw_motor_zero_point: 12279 pitch_motor_zero_point: 21931 # top_yaw_motor_zero_point: 58744 # viewer_motor_zero_point: 3030 @@ -58,6 +58,7 @@ Infantry_hardware: value_broadcaster: ros__parameters: forward_list: + # - /gimbal/yaw/control_angle_error - /gimbal/left_friction/velocity - /gimbal/right_friction/velocity # - /chassis/pitch_imu @@ -100,12 +101,12 @@ climber_controller: ros__parameters: front_climber_velocity: 60.0 back_climber_velocity: 15.0 - auto_climb_dash_velocity: 1.0 - auto_climb_support_retract_velocity: 60.0 - sync_coefficient: 0.6 + auto_climb_dash_velocity: 3.0 + auto_climb_support_retract_velocity: 45.0 + sync_coefficient: 0.8 climb_timeout_limit: 1000 - first_stair_approach_pitch: 0.560 - second_stair_approach_pitch: 0.320 + first_stair_approach_pitch: 0.590 + second_stair_approach_pitch: 0.345 back_climber_burst_velocity: 15.0 back_climber_burst_duration: 300 front_kp: 1.0 @@ -117,8 +118,8 @@ climber_controller: gimbal_controller: ros__parameters: - upper_limit: -0.62 - lower_limit: 0.48 + upper_limit: -0.54 + lower_limit: 0.274 pitch_angle_pid_controller: ros__parameters: @@ -141,18 +142,18 @@ yaw_angle_pid_controller: ros__parameters: measurement: /gimbal/yaw/control_angle_error control: /gimbal/yaw/control_velocity - kp: 20.0 #20.00 + kp: 15.0 #20.00 ki: 0.0 - kd: 0.8 #0.8 + kd: 0.0 #0.8 yaw_velocity_pid_controller: ros__parameters: measurement: /gimbal/yaw/velocity_imu setpoint: /gimbal/yaw/control_velocity control: /gimbal/yaw/control_torque - kp: 50.0 #50.00 - ki: 0.00 - kd: 0.0 # + kp: 40.0 #50.00 + ki: 0.001 + kd: 0.01 # gimbal_player_viewer_controller: ros__parameters: @@ -173,8 +174,8 @@ friction_wheel_controller: - /gimbal/left_friction - /gimbal/right_friction friction_velocities: - - 700.0 - - 700.0 + - 615.0 + - 615.0 friction_soft_start_stop_time: 1.0 heat_controller: diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller copy.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller copy.cpp new file mode 100644 index 00000000..3b2f6380 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller copy.cpp @@ -0,0 +1,573 @@ +// #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, +// ALIGN, +// APPROACH, +// SUPPORT_DEPLOY, +// DASH, +// SUPPORT_RETRACT, +// FORERAKED +// }; + +// class ChassisClimberController +// : public rmcs_executor::Component +// , public rclcpp::Node { +// public: +// ChassisClimberController() +// : Node( +// get_component_name(), +// rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) +// , logger_(get_logger()) +// , front_velocity_pid_calculator_( +// get_parameter("front_kp").as_double(), get_parameter("front_ki").as_double(), +// get_parameter("front_kd").as_double()) +// , back_velocity_pid_calculator_( +// get_parameter("back_kp").as_double(), get_parameter("back_ki").as_double(), +// get_parameter("back_kd").as_double()) { + +// track_velocity_max_ = get_parameter("front_climber_velocity").as_double(); +// climber_back_control_velocity_abs_ = get_parameter("back_climber_velocity").as_double(); +// auto_climb_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(); +// first_stair_approach_pitch_ = get_parameter("first_stair_approach_pitch").as_double(); +// second_stair_approach_pitch_ = get_parameter("second_stair_approach_pitch").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_output("/chassis/climbing_forward_velocity", climbing_forward_velocity_, 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/keyboard", keyboard_); +// register_input("/remote/rotary_knob_switch", rotary_knob_switch_); +// register_input("/chassis/pitch_imu", chassis_pitch_imu_); + +// register_input("/gimbal/yaw/angle", gimbal_yaw_angle_); +// register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_); +// register_input("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); +// } + +// void update() override { +// using namespace rmcs_msgs; + +// auto switch_right = *switch_right_; +// auto switch_left = *switch_left_; +// auto keyboard = *keyboard_; +// auto rotary_knob_switch = *rotary_knob_switch_; + +// // RCLCPP_INFO(logger_, "%lf", *chassis_pitch_imu_); + +// bool rotary_knob_to_down = +// (last_rotary_knob_switch_ != Switch::DOWN && rotary_knob_switch == Switch::DOWN); +// bool rotary_knob_from_down = +// (last_rotary_knob_switch_ == Switch::DOWN && rotary_knob_switch != Switch::DOWN); + +// if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) +// || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { +// reset_all_controls(); +// } else { +// handle_auto_climb_requests( +// (!last_keyboard_.g && keyboard.g) || rotary_knob_to_down, rotary_knob_from_down, +// rotary_knob_switch); + +// if (auto_climb_state_ != AutoClimbState::IDLE) { +// stop_manual_support(); +// apply_auto_climb_control(update_auto_climb_control()); +// } else { +// apply_auto_climb_control(update_manual_support_control(keyboard)); +// apply_manual_support_zero_output(); +// } +// } + +// 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 = nan_; +// double back_climber_velocity = nan_; +// 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) { +// start_auto_climb( +// rotary_knob_switch == rmcs_msgs::Switch::UP ? "Rotary Knob" : "Keyboard G"); +// } else { +// abort_auto_climb("toggled again"); +// } +// } else if (abort_by_rotary && auto_climb_state_ != AutoClimbState::IDLE) { +// abort_auto_climb("rotary knob left UP"); +// } +// } + +// void start_auto_climb(const char* source) { +// stop_manual_support(); +// auto_climb_continue_ = true; +// auto_climb_stair_index_ = 0; +// auto_climb_align_stable_count_ = 0; +// auto_climb_support_block_count_ = 0; +// enter_auto_climb_state(AutoClimbState::ALIGN); + +// RCLCPP_INFO(logger_, "Auto climb started by %s. Entering ALIGN.", source); +// } + +// void abort_auto_climb(const char* reason) { +// reset_all_controls(); +// RCLCPP_INFO(logger_, "Auto climb aborted (%s).", reason); +// } + +// AutoClimbControl update_auto_climb_control() { + +// if (auto_climb_state_ == AutoClimbState::IDLE) { +// detect_is_foreraked(); +// if (auto_climb_state_ == AutoClimbState::IDLE) { +// return {}; +// } +// } + +// auto_climb_timer_++; + +// switch (auto_climb_state_) { +// case AutoClimbState::IDLE: return {}; +// case AutoClimbState::ALIGN: return update_auto_climb_align(); +// case AutoClimbState::APPROACH: return update_auto_climb_approach(); +// case AutoClimbState::SUPPORT_DEPLOY: return update_auto_climb_support_deploy(); +// case AutoClimbState::DASH: return update_auto_climb_dash(); +// case AutoClimbState::SUPPORT_RETRACT: return update_auto_climb_support_retract(); +// case AutoClimbState::FORERAKED: return update_forerake_defend(); +// } + +// return {}; +// } + +// AutoClimbControl update_manual_support_control(const rmcs_msgs::Keyboard& keyboard) { +// AutoClimbControl control; + +// if (keyboard.b) { +// stop_manual_support(); +// control.back_climber_velocity = climber_back_control_velocity_abs_; +// return control; +// } + +// if (last_keyboard_.b) { +// manual_support_retracting_ = true; +// manual_support_retract_block_count_ = 0; +// manual_support_zero_output_ = false; +// RCLCPP_INFO(logger_, "Manual support retract started."); +// } + +// if (!manual_support_retracting_) { +// return control; +// } + +// control.back_climber_velocity = -auto_climb_support_retract_velocity_abs_; + +// if (is_back_climber_blocked()) { +// manual_support_retract_block_count_++; +// } else { +// manual_support_retract_block_count_ = 0; +// } + +// RCLCPP_INFO_THROTTLE( +// logger_, *get_clock(), 500, "MANUAL_SUPPORT_RETRACT: blocked_ticks=%d", +// manual_support_retract_block_count_); + +// if (manual_support_retract_block_count_ >= kManualSupportRetractConfirmTicks) { +// stop_manual_support(); +// manual_support_zero_output_ = true; +// RCLCPP_INFO(logger_, "Manual support retract completed."); +// return {}; +// } + +// return control; +// } + +// AutoClimbControl update_auto_climb_align() { +// AutoClimbControl control{ +// .front_track_velocity = 0.0, +// .back_climber_velocity = 0.0, +// .override_chassis_vx = 0.0, +// }; + +// double gimbal_yaw_angle_error = *gimbal_yaw_angle_error_; +// if (gimbal_yaw_angle_error < 0) +// gimbal_yaw_angle_error += 2 * std::numbers::pi; + +// double err = gimbal_yaw_angle_error + *gimbal_yaw_angle_; +// while (err >= std::numbers::pi) +// err -= 2 * std::numbers::pi; +// while (err < -std::numbers::pi) +// err += 2 * std::numbers::pi; + +// double yaw_velocity = *gimbal_yaw_velocity_imu_; +// bool is_aligned = std::abs(err) < kAutoClimbAlignThreshold; +// bool is_stable = std::abs(yaw_velocity) < kAutoClimbAlignVelocityThreshold; + +// if (is_aligned && is_stable) { +// auto_climb_align_stable_count_++; +// } else { +// auto_climb_align_stable_count_ = 0; +// } + +// RCLCPP_INFO_THROTTLE( +// logger_, *get_clock(), 500, "ALIGN: err=%.3f, yaw_velocity=%.3f, stable_ticks=%d", +// err, yaw_velocity, auto_climb_align_stable_count_); + +// if (auto_climb_align_stable_count_ >= kAutoClimbAlignConfirmTicks) { +// enter_auto_climb_state(AutoClimbState::APPROACH); +// RCLCPP_INFO(logger_, "Chassis aligned. Entering APPROACH."); +// } + +// return control; +// } + +// AutoClimbControl update_auto_climb_approach() { +// AutoClimbControl control{ +// .front_track_velocity = track_velocity_max_, +// .back_climber_velocity = 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.5, +// }; + +// 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); +// } +// } + +// return control; +// } + +// 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_, +// }; + +// RCLCPP_INFO_THROTTLE( +// logger_, *get_clock(), 500, "SUPPORT_RETRACT (step %d): timer=%d", +// auto_climb_stair_index_ + 1, auto_climb_timer_); + +// if (auto_climb_timer_ > kAutoClimbSupportRetractTicks) { +// bool has_next_stair = +// auto_climb_continue_ && (auto_climb_stair_index_ + 1 < kAutoClimbMaxStairs); + +// if (has_next_stair) { +// auto_climb_stair_index_++; +// enter_auto_climb_state(AutoClimbState::APPROACH); +// RCLCPP_INFO( +// logger_, "Auto climb continuing to step %d.", auto_climb_stair_index_ + 1); +// } else { +// int finished_steps = auto_climb_stair_index_ + 1; +// stop_auto_climb(); +// RCLCPP_INFO(logger_, "Auto climb completed (finished %d steps).", +// finished_steps); +// } +// } + +// return control; +// } + +// AutoClimbControl update_forerake_defend() { +// AutoClimbControl control{ +// .front_track_velocity = track_velocity_max_, +// .back_climber_velocity = 0.0, +// .override_chassis_vx = nan_, +// }; + +// double pitch = *chassis_pitch_imu_; +// bool is_leveled = std::abs(pitch) < kAutoClimbLeveledPitchThreshold +// && auto_climb_timer_ > kAutoClimbDashMinTicks; +// bool timeout = auto_climb_timer_ > kAutoClimbForerakedTimeoutTicks; + +// if (is_leveled || timeout) { +// enter_auto_climb_state(AutoClimbState::IDLE); +// } + +// return control; +// } + +// 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 reset_all_controls() { +// *climber_front_left_control_torque_ = nan_; +// *climber_front_right_control_torque_ = nan_; +// *climber_back_left_control_torque_ = nan_; +// *climber_back_right_control_torque_ = nan_; +// *climbing_forward_velocity_ = nan_; +// stop_manual_support(); +// stop_auto_climb(); +// } + +// void apply_manual_support_zero_output() { +// if (!manual_support_zero_output_) { +// return; +// } + +// *climber_back_left_control_torque_ = 0.0; +// *climber_back_right_control_torque_ = 0.0; +// } + +// void stop_manual_support() { +// manual_support_retracting_ = false; +// manual_support_retract_block_count_ = 0; +// manual_support_zero_output_ = false; +// } + +// void stop_auto_climb() { +// auto_climb_state_ = AutoClimbState::IDLE; +// auto_climb_timer_ = 0; +// auto_climb_stair_index_ = 0; +// auto_climb_continue_ = false; +// auto_climb_align_stable_count_ = 0; +// auto_climb_support_block_count_ = 0; +// } + +// void enter_auto_climb_state(AutoClimbState state) { +// if (state == auto_climb_state_) { +// return; +// } +// auto_climb_state_ = state; +// auto_climb_timer_ = 0; +// auto_climb_align_stable_count_ = 0; +// auto_climb_support_block_count_ = 0; +// } + +// bool is_back_climber_blocked() const { +// return (std::abs(*climber_back_left_torque_) > kBackClimberBlockedTorqueThreshold +// && std::abs(*climber_back_left_velocity_) < kBackClimberBlockedVelocityThreshold) +// || (std::abs(*climber_back_right_torque_) > kBackClimberBlockedTorqueThreshold +// && std::abs(*climber_back_right_velocity_) < +// kBackClimberBlockedVelocityThreshold); +// } + +// void dual_motor_sync_control( +// double setpoint, double left_velocity, double right_velocity, +// pid::MatrixPidCalculator<2>& pid_calculator, double& left_torque_out, +// double& right_torque_out) { + +// if (std::isnan(setpoint)) { +// left_torque_out = nan_; +// right_torque_out = nan_; +// return; +// } + +// Eigen::Vector2d setpoint_error{setpoint - left_velocity, setpoint - right_velocity}; +// Eigen::Vector2d relative_velocity{ +// left_velocity - right_velocity, right_velocity - left_velocity}; + +// Eigen::Vector2d control_error = setpoint_error - sync_coefficient_ * relative_velocity; +// auto control_torques = pid_calculator.update(control_error); + +// left_torque_out = control_torques[0]; +// right_torque_out = control_torques[1]; +// } + +// void detect_is_foreraked() { +// if (auto_climb_state_ != AutoClimbState::IDLE) { +// return; +// } + +// double pitch = *chassis_pitch_imu_; +// bool is_foreraked = pitch < kForerakedetectPitchThreshold; +// if (is_foreraked) { +// RCLCPP_INFO(get_logger(), "Enter foreraked"); +// enter_auto_climb_state(AutoClimbState::FORERAKED); +// } +// } + +// rclcpp::Logger logger_; +// static constexpr double nan_ = std::numeric_limits::quiet_NaN(); +// static constexpr double kAutoClimbApproachVelocity = 1.0; +// static constexpr double kAutoClimbAlignThreshold = 0.10; +// static constexpr double kAutoClimbAlignVelocityThreshold = 0.2; +// static constexpr double kAutoClimbLeveledPitchThreshold = 0.1; +// static constexpr double kForerakedetectPitchThreshold = -0.05; +// static constexpr double kBackClimberBlockedTorqueThreshold = 0.1; +// static constexpr double kBackClimberBlockedVelocityThreshold = 0.1; +// static constexpr int kAutoClimbAlignConfirmTicks = 50; +// static constexpr int kAutoClimbSupportConfirmTicks = 50; +// static constexpr int kAutoClimbDashMinTicks = 500; +// static constexpr int kAutoClimbDashTimeoutTicks = 3000; +// static constexpr int kAutoClimbForerakedTimeoutTicks = 2000; +// static constexpr int kAutoClimbSupportRetractTicks = 1000; +// static constexpr int kAutoClimbMaxStairs = 2; +// static constexpr int kManualSupportRetractConfirmTicks = 50; + +// double sync_coefficient_; +// double first_stair_approach_pitch_; +// double second_stair_approach_pitch_; + +// double track_velocity_max_; +// double climber_back_control_velocity_abs_; +// double auto_climb_dash_velocity_; +// double auto_climb_support_retract_velocity_abs_; + +// AutoClimbState auto_climb_state_ = AutoClimbState::IDLE; +// int auto_climb_timer_ = 0; +// int auto_climb_stair_index_ = 0; +// int auto_climb_align_stable_count_ = 0; +// int auto_climb_support_block_count_ = 0; +// bool auto_climb_continue_ = false; +// bool manual_support_retracting_ = false; +// int manual_support_retract_block_count_ = 0; +// bool manual_support_zero_output_ = 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_; +// InputInterface climber_back_left_velocity_; +// InputInterface climber_back_right_velocity_; + +// InputInterface climber_back_left_torque_; +// InputInterface climber_back_right_torque_; + +// InputInterface switch_right_; +// InputInterface switch_left_; +// InputInterface keyboard_; +// InputInterface rotary_knob_switch_; + +// InputInterface chassis_pitch_imu_; +// InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_, gimbal_yaw_velocity_imu_; + +// rmcs_msgs::Switch last_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_; +// }; +// } // 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/chassis/chassis_climber_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp index c66f67c2..ec72864b 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 @@ -169,7 +169,7 @@ class ChassisClimberController AutoClimbControl control{ .front_track_velocity = track_velocity_max_, .back_climber_velocity = 0.0, - .override_chassis_vx = 1.0, + .override_chassis_vx = kAutoClimbApproachVelocity, }; double pitch = *chassis_pitch_imu_; @@ -194,7 +194,7 @@ class ChassisClimberController AutoClimbControl control{ .front_track_velocity = track_velocity_max_, .back_climber_velocity = climber_back_control_velocity_abs_, - .override_chassis_vx = auto_climb_dash_velocity_, + .override_chassis_vx = 0.3, }; if (is_back_climber_blocked()) { @@ -231,18 +231,33 @@ class ChassisClimberController 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); + if (auto_climb_stair_index_ == 0) { + 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); + } + } + } else { + if (is_leveled || timeout) { + enter_auto_climb_state(AutoClimbState::SUPPORT_RETRACT); + + if (pitch < 0 || 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); + } } } @@ -412,13 +427,13 @@ class ChassisClimberController rclcpp::Logger logger_; static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - static constexpr double kAutoClimbApproachVelocity = 1.8; + 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 kAutoClimbDashTimeoutTicks = 3000; // 3000; static constexpr int kAutoClimbSupportRetractTicks = 1000; static constexpr int kAutoClimbMaxStairs = 2; 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 877f5a0e..f600caf6 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_(10.0, 0.0, 1.2) { + , following_velocity_controller_(5.0, 0.0, 0.0) { following_velocity_controller_.output_max = angular_velocity_max; following_velocity_controller_.output_min = -angular_velocity_max; @@ -151,7 +151,7 @@ class ChassisController case rmcs_msgs::ChassisMode::AUTO: break; case rmcs_msgs::ChassisMode::SPIN: { angular_velocity = - 0.4 * (spinning_forward_ ? angular_velocity_max : -angular_velocity_max); + 0.6 * (spinning_forward_ ? angular_velocity_max : -angular_velocity_max); } break; case rmcs_msgs::ChassisMode::STEP_DOWN: { double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/climbable_infantry_gimbal_controller copy.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/climbable_infantry_gimbal_controller copy.cpp new file mode 100644 index 00000000..d27704d5 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/climbable_infantry_gimbal_controller copy.cpp @@ -0,0 +1,97 @@ +#include + +#include +#include +#include +#include +#include + +#include "controller/gimbal/two_axis_gimbal_solver.hpp" + +namespace rmcs_core::controller::gimbal { + +using namespace rmcs_description; + +class SimpleGimbalController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + SimpleGimbalController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , two_axis_gimbal_solver( + *this, get_parameter("upper_limit").as_double(), + get_parameter("lower_limit").as_double()) { + + register_input("/remote/joystick/left", joystick_left_); + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + register_input("/remote/mouse/velocity", mouse_velocity_); + register_input("/remote/mouse", mouse_); + + register_input("/gimbal/auto_aim/control_direction", auto_aim_control_direction_, false); + + register_output("/gimbal/yaw/control_angle_error", yaw_angle_error_, nan_); + register_output("/gimbal/pitch/control_angle_error", pitch_angle_error_, nan_); + } + + void update() override { + auto angle_error = calculate_angle_error(); + *yaw_angle_error_ = angle_error.yaw_angle_error; + *pitch_angle_error_ = angle_error.pitch_angle_error; + } + + TwoAxisGimbalSolver::AngleError calculate_angle_error() { + auto switch_right = *switch_right_; + auto switch_left = *switch_left_; + auto mouse = *mouse_; + + using namespace rmcs_msgs; + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) + return two_axis_gimbal_solver.update(TwoAxisGimbalSolver::SetDisabled()); + + if (auto_aim_control_direction_.ready() && (mouse.right || switch_right == Switch::UP) + && !auto_aim_control_direction_->isZero()) + return two_axis_gimbal_solver.update( + TwoAxisGimbalSolver::SetControlDirection( + OdomImu::DirectionVector(*auto_aim_control_direction_))); + + if (!two_axis_gimbal_solver.enabled()) + return two_axis_gimbal_solver.update(TwoAxisGimbalSolver::SetToLevel()); + + constexpr double joystick_sensitivity = 0.006; + constexpr double mouse_sensitivity = 0.5; + + double yaw_shift = + joystick_sensitivity * joystick_left_->y() + mouse_sensitivity * mouse_velocity_->y(); + double pitch_shift = + -joystick_sensitivity * joystick_left_->x() - mouse_sensitivity * mouse_velocity_->x(); + + return two_axis_gimbal_solver.update( + TwoAxisGimbalSolver::SetControlShift(yaw_shift, pitch_shift)); + } + +private: + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + + InputInterface joystick_left_; + InputInterface switch_right_; + InputInterface switch_left_; + InputInterface mouse_velocity_; + InputInterface mouse_; + + InputInterface auto_aim_control_direction_; + + TwoAxisGimbalSolver two_axis_gimbal_solver; + + OutputInterface yaw_angle_error_, pitch_angle_error_; +}; + +} // namespace rmcs_core::controller::gimbal + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::gimbal::SimpleGimbalController, rmcs_executor::Component) \ No newline at end of file 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 5aecea6b..398a2a1b 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 @@ -89,6 +89,7 @@ class FrictionWheelController last_switch_right_ = switch_right; last_switch_left_ = switch_left; last_keyboard_ = keyboard; + // RCLCPP_INFO(logger_, "Bullet Fired: {%d}", *bullet_fired_); } if (!friction_enabled_) { reset_all_controls(); @@ -171,8 +172,8 @@ class FrictionWheelController if (differential < 0.1) primary_friction_velocity_decrease_integral_ += differential; else { - if (primary_friction_velocity_decrease_integral_ < -14.0 - && last_primary_friction_velocity_ < friction_working_velocities_[0] - 25.0) + if (primary_friction_velocity_decrease_integral_ < -10.0 + && last_primary_friction_velocity_ < friction_working_velocities_[0] - 15.0) fired = true; primary_friction_velocity_decrease_integral_ = 0; diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp index 3de0960a..38d090b7 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp @@ -1,5 +1,7 @@ #include +#include +#include #include #include @@ -26,13 +28,17 @@ class HeatController } void update() override { - shooter_heat_ = std::max(0, shooter_heat_ - *shooter_cooling_); + + int64_t shooter_cooling = *shooter_cooling_; + shooter_heat_ = std::max(0, shooter_heat_ - shooter_cooling); if (*bullet_fired_) - shooter_heat_ += heat_per_shot + 10; + shooter_heat_ += heat_per_shot + shooter_cooling; *control_bullet_allowance_ = std::max( 0, (*shooter_heat_limit_ - shooter_heat_ - reserved_heat) / heat_per_shot); + // RCLCPP_INFO(get_logger(), "shooter heat limit: {%ld}", *shooter_heat_limit_); + // RCLCPP_INFO(get_logger(), "Control bullet allowance: {%ld}", *control_bullet_allowance_); } private: 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 1cb9de96..6d8fbb4d 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 @@ -20,7 +20,7 @@ class ShootingRecorder register_input("/referee/shooter/initial_speed", initial_speed_); register_input("/referee/shooter/shoot_timestamp", shoot_timestamp_); - register_input("/friction_wheels/temperature", fractional_temperature_); + // register_input("/friction_wheels/temperature", fractional_temperature_); if (friction_wheel_count_ == 2) { const auto topic = std::array{ @@ -70,9 +70,9 @@ class ShootingRecorder if (friction_wheel_count_ == 2) { log_text = fmt::format( - "{},{},{},{},{}", timestamp, *initial_speed_, // - *friction_wheels_velocity_[0], *friction_wheels_velocity_[1], - *fractional_temperature_); + "{},{},{},{}", 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_, // From fc4438d263f3dec32177309561f9dd250e81e243 Mon Sep 17 00:00:00 2001 From: Fin_Resect Date: Wed, 22 Apr 2026 18:33:34 +0800 Subject: [PATCH 20/32] Resolved the issue of gimbal instability in follow mode. fixed heat_controller. --- .../config/climbable-infantry.yaml | 24 ++++++++++--------- .../chassis/chassis_climber_controller.cpp | 4 ++-- .../controller/chassis/chassis_controller.cpp | 2 +- .../gimbal/simple_gimbal_controller.cpp | 2 +- .../shooting/friction_wheel_controller.cpp | 4 ++-- .../controller/shooting/heat_controller.cpp | 10 ++++---- .../src/hardware/climbable-infantry.cpp | 12 ++++++---- 7 files changed, 31 insertions(+), 27 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml index 1ebe088c..afec14f0 100644 --- a/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml @@ -37,7 +37,7 @@ rmcs_executor: # - rmcs_auto_aim::AutoAimInitializer -> auto_aim_initializer # - rmcs_auto_aim::AutoAimController -> auto_aim_controller - - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster Infantry_hardware: ros__parameters: @@ -58,9 +58,11 @@ Infantry_hardware: value_broadcaster: ros__parameters: forward_list: - # - /gimbal/yaw/control_angle_error - - /gimbal/left_friction/velocity - - /gimbal/right_friction/velocity + - /gimbal/yaw/velocity + - /gimbal/yaw/control_velocity + # - /gimbal/yaw/control_torque + # - /gimbal/left_friction/velocity + # - /gimbal/right_friction/velocity # - /chassis/pitch_imu # - /gimbal/pitch/velocity_imu # - /gimbal/pitch/angle @@ -100,7 +102,7 @@ value_broadcaster: climber_controller: ros__parameters: front_climber_velocity: 60.0 - back_climber_velocity: 15.0 + back_climber_velocity: 20.0 auto_climb_dash_velocity: 3.0 auto_climb_support_retract_velocity: 45.0 sync_coefficient: 0.8 @@ -142,18 +144,18 @@ yaw_angle_pid_controller: ros__parameters: measurement: /gimbal/yaw/control_angle_error control: /gimbal/yaw/control_velocity - kp: 15.0 #20.00 + kp: 20.0 #20.00 ki: 0.0 - kd: 0.0 #0.8 + kd: 1.2 #0.8 yaw_velocity_pid_controller: ros__parameters: measurement: /gimbal/yaw/velocity_imu setpoint: /gimbal/yaw/control_velocity control: /gimbal/yaw/control_torque - kp: 40.0 #50.00 - ki: 0.001 - kd: 0.01 # + kp: 3.5 #50.00 + ki: 0.00 + kd: 0.2 # gimbal_player_viewer_controller: ros__parameters: @@ -180,7 +182,7 @@ friction_wheel_controller: heat_controller: ros__parameters: - heat_per_shot: 10 + heat_per_shot: 10000 reserved_heat: 0 bullet_feeder_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 ec72864b..0d320a6c 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 @@ -194,7 +194,7 @@ class ChassisClimberController AutoClimbControl control{ .front_track_velocity = track_velocity_max_, .back_climber_velocity = climber_back_control_velocity_abs_, - .override_chassis_vx = 0.3, + .override_chassis_vx = 0.5, }; if (is_back_climber_blocked()) { @@ -434,7 +434,7 @@ class ChassisClimberController static constexpr int kAutoClimbSupportConfirmTicks = 50; static constexpr int kAutoClimbDashMinTicks = 500; static constexpr int kAutoClimbDashTimeoutTicks = 3000; // 3000; - static constexpr int kAutoClimbSupportRetractTicks = 1000; + static constexpr int kAutoClimbSupportRetractTicks = 1200; static constexpr int kAutoClimbMaxStairs = 2; double sync_coefficient_; 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 f600caf6..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 @@ -19,7 +19,7 @@ class ChassisController : Node( get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) - , following_velocity_controller_(5.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; diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp index d27704d5..dd37c17a 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp @@ -67,7 +67,7 @@ class SimpleGimbalController double yaw_shift = joystick_sensitivity * joystick_left_->y() + mouse_sensitivity * mouse_velocity_->y(); double pitch_shift = - -joystick_sensitivity * joystick_left_->x() - mouse_sensitivity * mouse_velocity_->x(); + -joystick_sensitivity * joystick_left_->x() + mouse_sensitivity * mouse_velocity_->x(); return two_axis_gimbal_solver.update( TwoAxisGimbalSolver::SetControlShift(yaw_shift, pitch_shift)); 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 398a2a1b..4e22c13e 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 @@ -172,8 +172,8 @@ class FrictionWheelController if (differential < 0.1) primary_friction_velocity_decrease_integral_ += differential; else { - if (primary_friction_velocity_decrease_integral_ < -10.0 - && last_primary_friction_velocity_ < friction_working_velocities_[0] - 15.0) + if (primary_friction_velocity_decrease_integral_ < -14.0 + && last_primary_friction_velocity_ < friction_working_velocities_[0] - 20.0) fired = true; primary_friction_velocity_decrease_integral_ = 0; diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp index 38d090b7..37696201 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp @@ -29,16 +29,14 @@ class HeatController void update() override { - int64_t shooter_cooling = *shooter_cooling_; - shooter_heat_ = std::max(0, shooter_heat_ - shooter_cooling); + shooter_heat_ = std::max(0, shooter_heat_ - *shooter_cooling_); - if (*bullet_fired_) - shooter_heat_ += heat_per_shot + shooter_cooling; + if (*bullet_fired_) { + shooter_heat_ += heat_per_shot + 10; + } *control_bullet_allowance_ = std::max( 0, (*shooter_heat_limit_ - shooter_heat_ - reserved_heat) / heat_per_shot); - // RCLCPP_INFO(get_logger(), "shooter heat limit: {%ld}", *shooter_heat_limit_); - // RCLCPP_INFO(get_logger(), "Control bullet allowance: {%ld}", *control_bullet_allowance_); } private: diff --git a/rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp index e5308776..227e5330 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp @@ -144,12 +144,10 @@ class ClimbableInfantry device::DjiMotor::Config{device::DjiMotor::Type::kM3508} .set_reduction_ratio(1.) .set_reversed()); - climbable_infantry.register_output( "/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); climbable_infantry.register_output( "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); - imu_.set_coordinate_mapping( [](double x, double y, double z) { return std::make_tuple(x, y, z); }); } @@ -249,8 +247,8 @@ class ClimbableInfantry ClimbableInfantryCommand& climbable_infantry_command, std::string_view board_serial = {}) : librmcs::agent::CBoard(board_serial) - , logger_(climbable_infantry.get_logger()) + , tf_(climbable_infantry.tf_) , imu_(1000, 0.2, 0.0) , dr16_(climbable_infantry) , supercap_(climbable_infantry, climbable_infantry_command) @@ -358,8 +356,12 @@ class ClimbableInfantry chassis_front_climber_motor_[1].update_status(); chassis_back_climber_motor_[0].update_status(); chassis_back_climber_motor_[1].update_status(); + gimbal_yaw_motor_.update_status(); + tf_->set_state( + gimbal_yaw_motor_.angle()); + for (auto& motor : chassis_wheel_motors_) motor.update_status(); for (auto& motor : chassis_steering_motors_) @@ -395,7 +397,7 @@ class ClimbableInfantry builder.can2_transmit({ .can_id = 0x141, - .can_data = gimbal_yaw_motor_.generate_command().as_bytes(), + .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes(), }); // builder.can1_transmit({ @@ -482,6 +484,8 @@ class ClimbableInfantry rclcpp::Logger logger_; + OutputInterface& tf_; + device::Bmi088 imu_; device::Dr16 dr16_; device::Supercap supercap_; From 9694e305e1278a4491e010ac4435535f85d629d5 Mon Sep 17 00:00:00 2001 From: Fin_Resect Date: Wed, 22 Apr 2026 20:20:12 +0800 Subject: [PATCH 21/32] Adjusted the climb parameters --- rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml | 4 ++-- .../src/controller/chassis/chassis_climber_controller.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml index afec14f0..bc0bb254 100644 --- a/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml @@ -102,9 +102,9 @@ value_broadcaster: climber_controller: ros__parameters: front_climber_velocity: 60.0 - back_climber_velocity: 20.0 + back_climber_velocity: 13.0 auto_climb_dash_velocity: 3.0 - auto_climb_support_retract_velocity: 45.0 + auto_climb_support_retract_velocity: 47.0 sync_coefficient: 0.8 climb_timeout_limit: 1000 first_stair_approach_pitch: 0.590 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 0d320a6c..cb3fa7a9 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 @@ -194,7 +194,7 @@ class ChassisClimberController AutoClimbControl control{ .front_track_velocity = track_velocity_max_, .back_climber_velocity = climber_back_control_velocity_abs_, - .override_chassis_vx = 0.5, + .override_chassis_vx = auto_climb_dash_velocity_, }; if (is_back_climber_blocked()) { @@ -434,7 +434,7 @@ class ChassisClimberController static constexpr int kAutoClimbSupportConfirmTicks = 50; static constexpr int kAutoClimbDashMinTicks = 500; static constexpr int kAutoClimbDashTimeoutTicks = 3000; // 3000; - static constexpr int kAutoClimbSupportRetractTicks = 1200; + static constexpr int kAutoClimbSupportRetractTicks = 1500; static constexpr int kAutoClimbMaxStairs = 2; double sync_coefficient_; From 5ac68ad5a9be5351e0d261526bfa48f38f6736db Mon Sep 17 00:00:00 2001 From: Fin_Resect Date: Wed, 22 Apr 2026 21:15:18 +0800 Subject: [PATCH 22/32] removed redundant components --- ...mbable_infantry_gimbal_controller copy.cpp | 97 ------------------- 1 file changed, 97 deletions(-) delete mode 100644 rmcs_ws/src/rmcs_core/src/controller/gimbal/climbable_infantry_gimbal_controller copy.cpp diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/climbable_infantry_gimbal_controller copy.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/climbable_infantry_gimbal_controller copy.cpp deleted file mode 100644 index d27704d5..00000000 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/climbable_infantry_gimbal_controller copy.cpp +++ /dev/null @@ -1,97 +0,0 @@ -#include - -#include -#include -#include -#include -#include - -#include "controller/gimbal/two_axis_gimbal_solver.hpp" - -namespace rmcs_core::controller::gimbal { - -using namespace rmcs_description; - -class SimpleGimbalController - : public rmcs_executor::Component - , public rclcpp::Node { -public: - SimpleGimbalController() - : Node( - get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) - , two_axis_gimbal_solver( - *this, get_parameter("upper_limit").as_double(), - get_parameter("lower_limit").as_double()) { - - register_input("/remote/joystick/left", joystick_left_); - register_input("/remote/switch/right", switch_right_); - register_input("/remote/switch/left", switch_left_); - register_input("/remote/mouse/velocity", mouse_velocity_); - register_input("/remote/mouse", mouse_); - - register_input("/gimbal/auto_aim/control_direction", auto_aim_control_direction_, false); - - register_output("/gimbal/yaw/control_angle_error", yaw_angle_error_, nan_); - register_output("/gimbal/pitch/control_angle_error", pitch_angle_error_, nan_); - } - - void update() override { - auto angle_error = calculate_angle_error(); - *yaw_angle_error_ = angle_error.yaw_angle_error; - *pitch_angle_error_ = angle_error.pitch_angle_error; - } - - TwoAxisGimbalSolver::AngleError calculate_angle_error() { - auto switch_right = *switch_right_; - auto switch_left = *switch_left_; - auto mouse = *mouse_; - - using namespace rmcs_msgs; - if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) - || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) - return two_axis_gimbal_solver.update(TwoAxisGimbalSolver::SetDisabled()); - - if (auto_aim_control_direction_.ready() && (mouse.right || switch_right == Switch::UP) - && !auto_aim_control_direction_->isZero()) - return two_axis_gimbal_solver.update( - TwoAxisGimbalSolver::SetControlDirection( - OdomImu::DirectionVector(*auto_aim_control_direction_))); - - if (!two_axis_gimbal_solver.enabled()) - return two_axis_gimbal_solver.update(TwoAxisGimbalSolver::SetToLevel()); - - constexpr double joystick_sensitivity = 0.006; - constexpr double mouse_sensitivity = 0.5; - - double yaw_shift = - joystick_sensitivity * joystick_left_->y() + mouse_sensitivity * mouse_velocity_->y(); - double pitch_shift = - -joystick_sensitivity * joystick_left_->x() - mouse_sensitivity * mouse_velocity_->x(); - - return two_axis_gimbal_solver.update( - TwoAxisGimbalSolver::SetControlShift(yaw_shift, pitch_shift)); - } - -private: - static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - - InputInterface joystick_left_; - InputInterface switch_right_; - InputInterface switch_left_; - InputInterface mouse_velocity_; - InputInterface mouse_; - - InputInterface auto_aim_control_direction_; - - TwoAxisGimbalSolver two_axis_gimbal_solver; - - OutputInterface yaw_angle_error_, pitch_angle_error_; -}; - -} // namespace rmcs_core::controller::gimbal - -#include - -PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::gimbal::SimpleGimbalController, rmcs_executor::Component) \ No newline at end of file From 720c9228be4bc6a5e4b0d976391214c6edff94f2 Mon Sep 17 00:00:00 2001 From: Fin_Resect Date: Thu, 23 Apr 2026 23:54:49 +0800 Subject: [PATCH 23/32] ported chassiis_controller and climber_controller from steering-hero --- auth.json | 0 .../config/climbable-infantry.yaml | 15 +- rmcs_ws/src/rmcs_core/plugins.xml | 3 + .../chassis_climber_controller copy.cpp | 573 ------------------ .../chassis/chassis_climber_controller.cpp | 328 ++++++---- .../climbable_infantry_chassis_controller.cpp | 287 +++++++++ 6 files changed, 497 insertions(+), 709 deletions(-) create mode 100644 auth.json delete mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller copy.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/climbable_infantry_chassis_controller.cpp diff --git a/auth.json b/auth.json new file mode 100644 index 00000000..e69de29b diff --git a/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml index bc0bb254..07ebdf64 100644 --- a/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml @@ -29,7 +29,7 @@ rmcs_executor: - - rmcs_core::controller::chassis::ChassisController -> chassis_controller + - rmcs_core::controller::chassis::ClimbableInfantryChassisController -> 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 @@ -102,15 +102,12 @@ value_broadcaster: climber_controller: ros__parameters: front_climber_velocity: 60.0 - back_climber_velocity: 13.0 + back_climber_velocity: 15.0 #13.0 auto_climb_dash_velocity: 3.0 - auto_climb_support_retract_velocity: 47.0 + auto_climb_support_retract_velocity: 30.0 #47.0 sync_coefficient: 0.8 - climb_timeout_limit: 1000 first_stair_approach_pitch: 0.590 second_stair_approach_pitch: 0.345 - back_climber_burst_velocity: 15.0 - back_climber_burst_duration: 300 front_kp: 1.0 front_ki: 0.0 front_kd: 0.5 @@ -144,16 +141,16 @@ yaw_angle_pid_controller: ros__parameters: measurement: /gimbal/yaw/control_angle_error control: /gimbal/yaw/control_velocity - kp: 20.0 #20.00 + kp: 15.0 #20.00 ki: 0.0 - kd: 1.2 #0.8 + kd: 0.8 #0.8 yaw_velocity_pid_controller: ros__parameters: measurement: /gimbal/yaw/velocity_imu setpoint: /gimbal/yaw/control_velocity control: /gimbal/yaw/control_torque - kp: 3.5 #50.00 + kp: 2.5 #50.00 ki: 0.00 kd: 0.2 # diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 27ebbc06..7e28d096 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -23,6 +23,9 @@ Test plugin. + + Test plugin. + Test plugin. diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller copy.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller copy.cpp deleted file mode 100644 index 3b2f6380..00000000 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller copy.cpp +++ /dev/null @@ -1,573 +0,0 @@ -// #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, -// ALIGN, -// APPROACH, -// SUPPORT_DEPLOY, -// DASH, -// SUPPORT_RETRACT, -// FORERAKED -// }; - -// class ChassisClimberController -// : public rmcs_executor::Component -// , public rclcpp::Node { -// public: -// ChassisClimberController() -// : Node( -// get_component_name(), -// rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) -// , logger_(get_logger()) -// , front_velocity_pid_calculator_( -// get_parameter("front_kp").as_double(), get_parameter("front_ki").as_double(), -// get_parameter("front_kd").as_double()) -// , back_velocity_pid_calculator_( -// get_parameter("back_kp").as_double(), get_parameter("back_ki").as_double(), -// get_parameter("back_kd").as_double()) { - -// track_velocity_max_ = get_parameter("front_climber_velocity").as_double(); -// climber_back_control_velocity_abs_ = get_parameter("back_climber_velocity").as_double(); -// auto_climb_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(); -// first_stair_approach_pitch_ = get_parameter("first_stair_approach_pitch").as_double(); -// second_stair_approach_pitch_ = get_parameter("second_stair_approach_pitch").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_output("/chassis/climbing_forward_velocity", climbing_forward_velocity_, 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/keyboard", keyboard_); -// register_input("/remote/rotary_knob_switch", rotary_knob_switch_); -// register_input("/chassis/pitch_imu", chassis_pitch_imu_); - -// register_input("/gimbal/yaw/angle", gimbal_yaw_angle_); -// register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_); -// register_input("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); -// } - -// void update() override { -// using namespace rmcs_msgs; - -// auto switch_right = *switch_right_; -// auto switch_left = *switch_left_; -// auto keyboard = *keyboard_; -// auto rotary_knob_switch = *rotary_knob_switch_; - -// // RCLCPP_INFO(logger_, "%lf", *chassis_pitch_imu_); - -// bool rotary_knob_to_down = -// (last_rotary_knob_switch_ != Switch::DOWN && rotary_knob_switch == Switch::DOWN); -// bool rotary_knob_from_down = -// (last_rotary_knob_switch_ == Switch::DOWN && rotary_knob_switch != Switch::DOWN); - -// if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) -// || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { -// reset_all_controls(); -// } else { -// handle_auto_climb_requests( -// (!last_keyboard_.g && keyboard.g) || rotary_knob_to_down, rotary_knob_from_down, -// rotary_knob_switch); - -// if (auto_climb_state_ != AutoClimbState::IDLE) { -// stop_manual_support(); -// apply_auto_climb_control(update_auto_climb_control()); -// } else { -// apply_auto_climb_control(update_manual_support_control(keyboard)); -// apply_manual_support_zero_output(); -// } -// } - -// 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 = nan_; -// double back_climber_velocity = nan_; -// 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) { -// start_auto_climb( -// rotary_knob_switch == rmcs_msgs::Switch::UP ? "Rotary Knob" : "Keyboard G"); -// } else { -// abort_auto_climb("toggled again"); -// } -// } else if (abort_by_rotary && auto_climb_state_ != AutoClimbState::IDLE) { -// abort_auto_climb("rotary knob left UP"); -// } -// } - -// void start_auto_climb(const char* source) { -// stop_manual_support(); -// auto_climb_continue_ = true; -// auto_climb_stair_index_ = 0; -// auto_climb_align_stable_count_ = 0; -// auto_climb_support_block_count_ = 0; -// enter_auto_climb_state(AutoClimbState::ALIGN); - -// RCLCPP_INFO(logger_, "Auto climb started by %s. Entering ALIGN.", source); -// } - -// void abort_auto_climb(const char* reason) { -// reset_all_controls(); -// RCLCPP_INFO(logger_, "Auto climb aborted (%s).", reason); -// } - -// AutoClimbControl update_auto_climb_control() { - -// if (auto_climb_state_ == AutoClimbState::IDLE) { -// detect_is_foreraked(); -// if (auto_climb_state_ == AutoClimbState::IDLE) { -// return {}; -// } -// } - -// auto_climb_timer_++; - -// switch (auto_climb_state_) { -// case AutoClimbState::IDLE: return {}; -// case AutoClimbState::ALIGN: return update_auto_climb_align(); -// case AutoClimbState::APPROACH: return update_auto_climb_approach(); -// case AutoClimbState::SUPPORT_DEPLOY: return update_auto_climb_support_deploy(); -// case AutoClimbState::DASH: return update_auto_climb_dash(); -// case AutoClimbState::SUPPORT_RETRACT: return update_auto_climb_support_retract(); -// case AutoClimbState::FORERAKED: return update_forerake_defend(); -// } - -// return {}; -// } - -// AutoClimbControl update_manual_support_control(const rmcs_msgs::Keyboard& keyboard) { -// AutoClimbControl control; - -// if (keyboard.b) { -// stop_manual_support(); -// control.back_climber_velocity = climber_back_control_velocity_abs_; -// return control; -// } - -// if (last_keyboard_.b) { -// manual_support_retracting_ = true; -// manual_support_retract_block_count_ = 0; -// manual_support_zero_output_ = false; -// RCLCPP_INFO(logger_, "Manual support retract started."); -// } - -// if (!manual_support_retracting_) { -// return control; -// } - -// control.back_climber_velocity = -auto_climb_support_retract_velocity_abs_; - -// if (is_back_climber_blocked()) { -// manual_support_retract_block_count_++; -// } else { -// manual_support_retract_block_count_ = 0; -// } - -// RCLCPP_INFO_THROTTLE( -// logger_, *get_clock(), 500, "MANUAL_SUPPORT_RETRACT: blocked_ticks=%d", -// manual_support_retract_block_count_); - -// if (manual_support_retract_block_count_ >= kManualSupportRetractConfirmTicks) { -// stop_manual_support(); -// manual_support_zero_output_ = true; -// RCLCPP_INFO(logger_, "Manual support retract completed."); -// return {}; -// } - -// return control; -// } - -// AutoClimbControl update_auto_climb_align() { -// AutoClimbControl control{ -// .front_track_velocity = 0.0, -// .back_climber_velocity = 0.0, -// .override_chassis_vx = 0.0, -// }; - -// double gimbal_yaw_angle_error = *gimbal_yaw_angle_error_; -// if (gimbal_yaw_angle_error < 0) -// gimbal_yaw_angle_error += 2 * std::numbers::pi; - -// double err = gimbal_yaw_angle_error + *gimbal_yaw_angle_; -// while (err >= std::numbers::pi) -// err -= 2 * std::numbers::pi; -// while (err < -std::numbers::pi) -// err += 2 * std::numbers::pi; - -// double yaw_velocity = *gimbal_yaw_velocity_imu_; -// bool is_aligned = std::abs(err) < kAutoClimbAlignThreshold; -// bool is_stable = std::abs(yaw_velocity) < kAutoClimbAlignVelocityThreshold; - -// if (is_aligned && is_stable) { -// auto_climb_align_stable_count_++; -// } else { -// auto_climb_align_stable_count_ = 0; -// } - -// RCLCPP_INFO_THROTTLE( -// logger_, *get_clock(), 500, "ALIGN: err=%.3f, yaw_velocity=%.3f, stable_ticks=%d", -// err, yaw_velocity, auto_climb_align_stable_count_); - -// if (auto_climb_align_stable_count_ >= kAutoClimbAlignConfirmTicks) { -// enter_auto_climb_state(AutoClimbState::APPROACH); -// RCLCPP_INFO(logger_, "Chassis aligned. Entering APPROACH."); -// } - -// return control; -// } - -// AutoClimbControl update_auto_climb_approach() { -// AutoClimbControl control{ -// .front_track_velocity = track_velocity_max_, -// .back_climber_velocity = 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.5, -// }; - -// 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); -// } -// } - -// return control; -// } - -// 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_, -// }; - -// RCLCPP_INFO_THROTTLE( -// logger_, *get_clock(), 500, "SUPPORT_RETRACT (step %d): timer=%d", -// auto_climb_stair_index_ + 1, auto_climb_timer_); - -// if (auto_climb_timer_ > kAutoClimbSupportRetractTicks) { -// bool has_next_stair = -// auto_climb_continue_ && (auto_climb_stair_index_ + 1 < kAutoClimbMaxStairs); - -// if (has_next_stair) { -// auto_climb_stair_index_++; -// enter_auto_climb_state(AutoClimbState::APPROACH); -// RCLCPP_INFO( -// logger_, "Auto climb continuing to step %d.", auto_climb_stair_index_ + 1); -// } else { -// int finished_steps = auto_climb_stair_index_ + 1; -// stop_auto_climb(); -// RCLCPP_INFO(logger_, "Auto climb completed (finished %d steps).", -// finished_steps); -// } -// } - -// return control; -// } - -// AutoClimbControl update_forerake_defend() { -// AutoClimbControl control{ -// .front_track_velocity = track_velocity_max_, -// .back_climber_velocity = 0.0, -// .override_chassis_vx = nan_, -// }; - -// double pitch = *chassis_pitch_imu_; -// bool is_leveled = std::abs(pitch) < kAutoClimbLeveledPitchThreshold -// && auto_climb_timer_ > kAutoClimbDashMinTicks; -// bool timeout = auto_climb_timer_ > kAutoClimbForerakedTimeoutTicks; - -// if (is_leveled || timeout) { -// enter_auto_climb_state(AutoClimbState::IDLE); -// } - -// return control; -// } - -// 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 reset_all_controls() { -// *climber_front_left_control_torque_ = nan_; -// *climber_front_right_control_torque_ = nan_; -// *climber_back_left_control_torque_ = nan_; -// *climber_back_right_control_torque_ = nan_; -// *climbing_forward_velocity_ = nan_; -// stop_manual_support(); -// stop_auto_climb(); -// } - -// void apply_manual_support_zero_output() { -// if (!manual_support_zero_output_) { -// return; -// } - -// *climber_back_left_control_torque_ = 0.0; -// *climber_back_right_control_torque_ = 0.0; -// } - -// void stop_manual_support() { -// manual_support_retracting_ = false; -// manual_support_retract_block_count_ = 0; -// manual_support_zero_output_ = false; -// } - -// void stop_auto_climb() { -// auto_climb_state_ = AutoClimbState::IDLE; -// auto_climb_timer_ = 0; -// auto_climb_stair_index_ = 0; -// auto_climb_continue_ = false; -// auto_climb_align_stable_count_ = 0; -// auto_climb_support_block_count_ = 0; -// } - -// void enter_auto_climb_state(AutoClimbState state) { -// if (state == auto_climb_state_) { -// return; -// } -// auto_climb_state_ = state; -// auto_climb_timer_ = 0; -// auto_climb_align_stable_count_ = 0; -// auto_climb_support_block_count_ = 0; -// } - -// bool is_back_climber_blocked() const { -// return (std::abs(*climber_back_left_torque_) > kBackClimberBlockedTorqueThreshold -// && std::abs(*climber_back_left_velocity_) < kBackClimberBlockedVelocityThreshold) -// || (std::abs(*climber_back_right_torque_) > kBackClimberBlockedTorqueThreshold -// && std::abs(*climber_back_right_velocity_) < -// kBackClimberBlockedVelocityThreshold); -// } - -// void dual_motor_sync_control( -// double setpoint, double left_velocity, double right_velocity, -// pid::MatrixPidCalculator<2>& pid_calculator, double& left_torque_out, -// double& right_torque_out) { - -// if (std::isnan(setpoint)) { -// left_torque_out = nan_; -// right_torque_out = nan_; -// return; -// } - -// Eigen::Vector2d setpoint_error{setpoint - left_velocity, setpoint - right_velocity}; -// Eigen::Vector2d relative_velocity{ -// left_velocity - right_velocity, right_velocity - left_velocity}; - -// Eigen::Vector2d control_error = setpoint_error - sync_coefficient_ * relative_velocity; -// auto control_torques = pid_calculator.update(control_error); - -// left_torque_out = control_torques[0]; -// right_torque_out = control_torques[1]; -// } - -// void detect_is_foreraked() { -// if (auto_climb_state_ != AutoClimbState::IDLE) { -// return; -// } - -// double pitch = *chassis_pitch_imu_; -// bool is_foreraked = pitch < kForerakedetectPitchThreshold; -// if (is_foreraked) { -// RCLCPP_INFO(get_logger(), "Enter foreraked"); -// enter_auto_climb_state(AutoClimbState::FORERAKED); -// } -// } - -// rclcpp::Logger logger_; -// static constexpr double nan_ = std::numeric_limits::quiet_NaN(); -// static constexpr double kAutoClimbApproachVelocity = 1.0; -// static constexpr double kAutoClimbAlignThreshold = 0.10; -// static constexpr double kAutoClimbAlignVelocityThreshold = 0.2; -// static constexpr double kAutoClimbLeveledPitchThreshold = 0.1; -// static constexpr double kForerakedetectPitchThreshold = -0.05; -// static constexpr double kBackClimberBlockedTorqueThreshold = 0.1; -// static constexpr double kBackClimberBlockedVelocityThreshold = 0.1; -// static constexpr int kAutoClimbAlignConfirmTicks = 50; -// static constexpr int kAutoClimbSupportConfirmTicks = 50; -// static constexpr int kAutoClimbDashMinTicks = 500; -// static constexpr int kAutoClimbDashTimeoutTicks = 3000; -// static constexpr int kAutoClimbForerakedTimeoutTicks = 2000; -// static constexpr int kAutoClimbSupportRetractTicks = 1000; -// static constexpr int kAutoClimbMaxStairs = 2; -// static constexpr int kManualSupportRetractConfirmTicks = 50; - -// double sync_coefficient_; -// double first_stair_approach_pitch_; -// double second_stair_approach_pitch_; - -// double track_velocity_max_; -// double climber_back_control_velocity_abs_; -// double auto_climb_dash_velocity_; -// double auto_climb_support_retract_velocity_abs_; - -// AutoClimbState auto_climb_state_ = AutoClimbState::IDLE; -// int auto_climb_timer_ = 0; -// int auto_climb_stair_index_ = 0; -// int auto_climb_align_stable_count_ = 0; -// int auto_climb_support_block_count_ = 0; -// bool auto_climb_continue_ = false; -// bool manual_support_retracting_ = false; -// int manual_support_retract_block_count_ = 0; -// bool manual_support_zero_output_ = 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_; -// InputInterface climber_back_left_velocity_; -// InputInterface climber_back_right_velocity_; - -// InputInterface climber_back_left_torque_; -// InputInterface climber_back_right_torque_; - -// InputInterface switch_right_; -// InputInterface switch_left_; -// InputInterface keyboard_; -// InputInterface rotary_knob_switch_; - -// InputInterface chassis_pitch_imu_; -// InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_, gimbal_yaw_velocity_imu_; - -// rmcs_msgs::Switch last_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_; -// }; -// } // 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/chassis/chassis_climber_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp index cb3fa7a9..b74db7ef 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 @@ -2,7 +2,6 @@ #include "rmcs_msgs/keyboard.hpp" #include "rmcs_msgs/switch.hpp" #include -#include #include #include #include @@ -14,7 +13,15 @@ namespace rmcs_core::controller::chassis { -enum class AutoClimbState { IDLE, APPROACH, SUPPORT_DEPLOY, DASH, SUPPORT_RETRACT }; +enum class AutoClimbState { + IDLE, + ALIGN, + APPROACH, + SUPPORT_DEPLOY, + DASH, + SUPPORT_RETRACT, + FORERAKED +}; class ChassisClimberController : public rmcs_executor::Component @@ -38,16 +45,9 @@ class ChassisClimberController 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(); - - back_climber_block_count_ = 0; - back_climber_timer_ = 0; - register_output( "/chassis/climber/left_front_motor/control_torque", climber_front_left_control_torque_, nan_); @@ -73,10 +73,13 @@ 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_); + + register_input("/gimbal/yaw/angle", gimbal_yaw_angle_); + register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_); + register_input("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); } void update() override { @@ -87,25 +90,27 @@ class ChassisClimberController auto keyboard = *keyboard_; auto rotary_knob_switch = *rotary_knob_switch_; - // RCLCPP_INFO(logger_, "%lf", *climber_front_left_control_torque_); + // 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); + bool rotary_knob_to_down = + (last_rotary_knob_switch_ != Switch::DOWN && rotary_knob_switch == Switch::DOWN); + bool rotary_knob_from_down = + (last_rotary_knob_switch_ == Switch::DOWN && rotary_knob_switch != Switch::DOWN); if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { reset_all_controls(); } else { handle_auto_climb_requests( - (!last_keyboard_.g && keyboard.g) || rotary_knob_to_up, rotary_knob_from_up, + (!last_keyboard_.g && keyboard.g) || rotary_knob_to_down, rotary_knob_from_down, rotary_knob_switch); if (auto_climb_state_ != AutoClimbState::IDLE) { + stop_manual_support(); apply_auto_climb_control(update_auto_climb_control()); - } else if (switch_left != Switch::DOWN) { - update_manual_control(); + } else { + apply_auto_climb_control(update_manual_support_control(keyboard)); + apply_manual_support_zero_output(); } } @@ -117,8 +122,8 @@ class ChassisClimberController private: struct AutoClimbControl { - double front_track_velocity = 0.0; - double back_climber_velocity = 0.0; + double front_track_velocity = nan_; + double back_climber_velocity = nan_; double override_chassis_vx = nan_; }; @@ -138,12 +143,14 @@ class ChassisClimberController } void start_auto_climb(const char* source) { + stop_manual_support(); auto_climb_continue_ = true; auto_climb_stair_index_ = 0; + auto_climb_align_stable_count_ = 0; auto_climb_support_block_count_ = 0; - enter_auto_climb_state(AutoClimbState::APPROACH); + enter_auto_climb_state(AutoClimbState::ALIGN); - RCLCPP_INFO(logger_, "Auto climb started by %s.", source); + RCLCPP_INFO(logger_, "Auto climb started by %s. Entering ALIGN.", source); } void abort_auto_climb(const char* reason) { @@ -152,19 +159,110 @@ class ChassisClimberController } AutoClimbControl update_auto_climb_control() { + + if (auto_climb_state_ == AutoClimbState::IDLE) { + detect_is_foreraked(); + if (auto_climb_state_ == AutoClimbState::IDLE) { + return {}; + } + } + auto_climb_timer_++; switch (auto_climb_state_) { case AutoClimbState::IDLE: return {}; + case AutoClimbState::ALIGN: return update_auto_climb_align(); case AutoClimbState::APPROACH: return update_auto_climb_approach(); case AutoClimbState::SUPPORT_DEPLOY: return update_auto_climb_support_deploy(); case AutoClimbState::DASH: return update_auto_climb_dash(); case AutoClimbState::SUPPORT_RETRACT: return update_auto_climb_support_retract(); + case AutoClimbState::FORERAKED: return update_forerake_defend(); } return {}; } + AutoClimbControl update_manual_support_control(const rmcs_msgs::Keyboard& keyboard) { + AutoClimbControl control; + + if (keyboard.b) { + stop_manual_support(); + control.back_climber_velocity = climber_back_control_velocity_abs_; + return control; + } + + if (last_keyboard_.b) { + manual_support_retracting_ = true; + manual_support_retract_block_count_ = 0; + manual_support_zero_output_ = false; + RCLCPP_INFO(logger_, "Manual support retract started."); + } + + if (!manual_support_retracting_) { + return control; + } + + control.back_climber_velocity = -auto_climb_support_retract_velocity_abs_; + + if (is_back_climber_blocked()) { + manual_support_retract_block_count_++; + } else { + manual_support_retract_block_count_ = 0; + } + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "MANUAL_SUPPORT_RETRACT: blocked_ticks=%d", + manual_support_retract_block_count_); + + if (manual_support_retract_block_count_ >= kManualSupportRetractConfirmTicks) { + stop_manual_support(); + manual_support_zero_output_ = true; + RCLCPP_INFO(logger_, "Manual support retract completed."); + return {}; + } + + return control; + } + + AutoClimbControl update_auto_climb_align() { + AutoClimbControl control{ + .front_track_velocity = 0.0, + .back_climber_velocity = 0.0, + .override_chassis_vx = 0.0, + }; + + double gimbal_yaw_angle_error = *gimbal_yaw_angle_error_; + if (gimbal_yaw_angle_error < 0) + gimbal_yaw_angle_error += 2 * std::numbers::pi; + + double err = gimbal_yaw_angle_error + *gimbal_yaw_angle_; + while (err >= std::numbers::pi) + err -= 2 * std::numbers::pi; + while (err < -std::numbers::pi) + err += 2 * std::numbers::pi; + + double yaw_velocity = *gimbal_yaw_velocity_imu_; + bool is_aligned = std::abs(err) < kAutoClimbAlignThreshold; + bool is_stable = std::abs(yaw_velocity) < kAutoClimbAlignVelocityThreshold; + + if (is_aligned && is_stable) { + auto_climb_align_stable_count_++; + } else { + auto_climb_align_stable_count_ = 0; + } + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "ALIGN: err=%.3f, yaw_velocity=%.3f, stable_ticks=%d", err, + yaw_velocity, auto_climb_align_stable_count_); + + if (auto_climb_align_stable_count_ >= kAutoClimbAlignConfirmTicks) { + enter_auto_climb_state(AutoClimbState::APPROACH); + RCLCPP_INFO(logger_, "Chassis aligned. Entering APPROACH."); + } + + return control; + } + AutoClimbControl update_auto_climb_approach() { AutoClimbControl control{ .front_track_velocity = track_velocity_max_, @@ -192,9 +290,9 @@ class ChassisClimberController AutoClimbControl update_auto_climb_support_deploy() { AutoClimbControl control{ - .front_track_velocity = track_velocity_max_, + .front_track_velocity = 0.0, .back_climber_velocity = climber_back_control_velocity_abs_, - .override_chassis_vx = auto_climb_dash_velocity_, + .override_chassis_vx = 0.5, }; if (is_back_climber_blocked()) { @@ -231,33 +329,18 @@ class ChassisClimberController RCLCPP_INFO_THROTTLE( logger_, *get_clock(), 500, "DASH (step %d): pitch=%.3f, timer=%d", auto_climb_stair_index_ + 1, pitch, auto_climb_timer_); - if (auto_climb_stair_index_ == 0) { - 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); - } - } - } else { - if (is_leveled || timeout) { - enter_auto_climb_state(AutoClimbState::SUPPORT_RETRACT); - - if (pitch < 0 || 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); - } + + 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); } } @@ -294,6 +377,25 @@ class ChassisClimberController return control; } + AutoClimbControl update_forerake_defend() { + AutoClimbControl control{ + .front_track_velocity = track_velocity_max_, + .back_climber_velocity = 0.0, + .override_chassis_vx = nan_, + }; + + double pitch = *chassis_pitch_imu_; + bool is_leveled = std::abs(pitch) < kAutoClimbLeveledPitchThreshold + && auto_climb_timer_ > kAutoClimbDashMinTicks; + bool timeout = auto_climb_timer_ > kAutoClimbForerakedTimeoutTicks; + + if (is_leveled || timeout) { + enter_auto_climb_state(AutoClimbState::IDLE); + } + + return control; + } + void apply_auto_climb_control(const AutoClimbControl& control) { *climbing_forward_velocity_ = control.override_chassis_vx; @@ -308,70 +410,29 @@ class ChassisClimberController *climber_back_left_control_torque_, *climber_back_right_control_torque_); } - void update_manual_control() { + void reset_all_controls() { + *climber_front_left_control_torque_ = nan_; + *climber_front_right_control_torque_ = nan_; + *climber_back_left_control_torque_ = nan_; + *climber_back_right_control_torque_ = nan_; *climbing_forward_velocity_ = nan_; + stop_manual_support(); + stop_auto_climb(); + } - // 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_; + void apply_manual_support_zero_output() { + if (!manual_support_zero_output_) { + return; } - 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; - } + *climber_back_left_control_torque_ = 0.0; + *climber_back_right_control_torque_ = 0.0; } - 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; - *climbing_forward_velocity_ = nan_; - stop_auto_climb(); - reset_back_safety_counters(); + void stop_manual_support() { + manual_support_retracting_ = false; + manual_support_retract_block_count_ = 0; + manual_support_zero_output_ = false; } void stop_auto_climb() { @@ -379,20 +440,20 @@ class ChassisClimberController auto_climb_timer_ = 0; auto_climb_stair_index_ = 0; auto_climb_continue_ = false; + auto_climb_align_stable_count_ = 0; auto_climb_support_block_count_ = 0; } void enter_auto_climb_state(AutoClimbState state) { + if (state == auto_climb_state_) { + return; + } auto_climb_state_ = state; auto_climb_timer_ = 0; + auto_climb_align_stable_count_ = 0; auto_climb_support_block_count_ = 0; } - 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) @@ -422,32 +483,41 @@ class ChassisClimberController right_torque_out = control_torques[1]; } - int back_climber_block_count_; - int back_climber_timer_; + void detect_is_foreraked() { + if (auto_climb_state_ != AutoClimbState::IDLE) { + return; + } + + double pitch = *chassis_pitch_imu_; + bool is_foreraked = pitch < kForerakedetectPitchThreshold; + if (is_foreraked) { + RCLCPP_INFO(get_logger(), "Enter foreraked"); + enter_auto_climb_state(AutoClimbState::FORERAKED); + } + } rclcpp::Logger logger_; static constexpr double nan_ = std::numeric_limits::quiet_NaN(); static constexpr double kAutoClimbApproachVelocity = 1.0; + static constexpr double kAutoClimbAlignThreshold = 0.10; + static constexpr double kAutoClimbAlignVelocityThreshold = 0.2; static constexpr double kAutoClimbLeveledPitchThreshold = 0.1; + static constexpr double kForerakedetectPitchThreshold = -0.05; static constexpr double kBackClimberBlockedTorqueThreshold = 0.1; static constexpr double kBackClimberBlockedVelocityThreshold = 0.1; + static constexpr int kAutoClimbAlignConfirmTicks = 50; static constexpr int kAutoClimbSupportConfirmTicks = 50; static constexpr int kAutoClimbDashMinTicks = 500; - static constexpr int kAutoClimbDashTimeoutTicks = 3000; // 3000; - static constexpr int kAutoClimbSupportRetractTicks = 1500; + static constexpr int kAutoClimbDashTimeoutTicks = 3000; + static constexpr int kAutoClimbForerakedTimeoutTicks = 2000; + static constexpr int kAutoClimbSupportRetractTicks = 1000; static constexpr int kAutoClimbMaxStairs = 2; + static constexpr int kManualSupportRetractConfirmTicks = 50; 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_; - - bool front_climber_enable_ = false; - double back_climber_dir_ = -1; - double track_velocity_max_; double climber_back_control_velocity_abs_; double auto_climb_dash_velocity_; @@ -456,8 +526,12 @@ class ChassisClimberController AutoClimbState auto_climb_state_ = AutoClimbState::IDLE; int auto_climb_timer_ = 0; int auto_climb_stair_index_ = 0; + int auto_climb_align_stable_count_ = 0; int auto_climb_support_block_count_ = 0; bool auto_climb_continue_ = false; + bool manual_support_retracting_ = false; + int manual_support_retract_block_count_ = 0; + bool manual_support_zero_output_ = false; OutputInterface climber_front_left_control_torque_; OutputInterface climber_front_right_control_torque_; @@ -475,11 +549,11 @@ class ChassisClimberController InputInterface switch_right_; InputInterface switch_left_; - InputInterface joystick_right_; InputInterface keyboard_; InputInterface rotary_knob_switch_; InputInterface chassis_pitch_imu_; + InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_, gimbal_yaw_velocity_imu_; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; @@ -493,4 +567,4 @@ class ChassisClimberController #include PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::chassis::ChassisClimberController, rmcs_executor::Component) + rmcs_core::controller::chassis::ChassisClimberController, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/climbable_infantry_chassis_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/climbable_infantry_chassis_controller.cpp new file mode 100644 index 00000000..13d101ef --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/climbable_infantry_chassis_controller.cpp @@ -0,0 +1,287 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include "controller/pid/pid_calculator.hpp" + +namespace rmcs_core::controller::chassis { + +class ClimbableInfantryChassisController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + ClimbableInfantryChassisController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , 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; + + register_input("/remote/joystick/right", joystick_right_); + register_input("/remote/joystick/left", joystick_left_); + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + register_input("/remote/mouse/velocity", mouse_velocity_); + register_input("/remote/mouse", mouse_); + register_input("/remote/keyboard", keyboard_); + register_input("/remote/rotary_knob", rotary_knob_); + + register_input("/gimbal/yaw/angle", gimbal_yaw_angle_, false); + register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, false); + register_input("/chassis/velocity", chassis_velocity_, false); + register_input("/chassis/climbing_forward_velocity", climbing_forward_velocity_, false); + + register_output("/chassis/angle", chassis_angle_, nan); + register_output("/chassis/control_angle", chassis_control_angle_, nan); + + register_output("/chassis/control_mode", mode_); + register_output("/chassis/control_velocity", chassis_control_velocity_); + } + + void before_updating() override { + if (!gimbal_yaw_angle_.ready()) { + gimbal_yaw_angle_.make_and_bind_directly(0.0); + RCLCPP_WARN(get_logger(), "Failed to fetch \"/gimbal/yaw/angle\". Set to 0.0."); + } + if (!gimbal_yaw_angle_error_.ready()) { + gimbal_yaw_angle_error_.make_and_bind_directly(0.0); + RCLCPP_WARN( + get_logger(), "Failed to fetch \"/gimbal/yaw/control_angle_error\". Set to 0.0."); + } + chassis_velocity_feedback_ready_ = chassis_velocity_.ready(); + if (!chassis_velocity_feedback_ready_) { + chassis_velocity_.make_and_bind_directly(0.0, 0.0, 0.0); + } + } + + void update() override { + using namespace rmcs_msgs; + + auto switch_right = *switch_right_; + auto switch_left = *switch_left_; + auto keyboard = *keyboard_; + auto mode = *mode_; + + do { + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + reset_all_controls(); + break; + } + if (switch_left != Switch::DOWN) { + if (last_switch_right_ == Switch::MIDDLE && switch_right == Switch::DOWN) { + if (mode == rmcs_msgs::ChassisMode::SPIN) { + mode = rmcs_msgs::ChassisMode::STEP_DOWN; + } else { + 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::STEP_DOWN; + } else { + mode = rmcs_msgs::ChassisMode::SPIN; + spinning_forward_ = !spinning_forward_; + } + } else if (!last_keyboard_.x && keyboard.x) { + mode = rmcs_msgs::ChassisMode::STEP_DOWN; + step_down_facing_ = StepDownFacing::FRONT; + + } else if (!last_keyboard_.z && keyboard.z) { + if (mode == rmcs_msgs::ChassisMode::STEP_DOWN) { + mode = rmcs_msgs::ChassisMode::AUTO; + } else { + mode = rmcs_msgs::ChassisMode::STEP_DOWN; + step_down_facing_ = StepDownFacing::BACK; + } + } + *mode_ = mode; + } + + update_velocity_control(); + } while (false); + + last_switch_right_ = switch_right; + last_switch_left_ = switch_left; + last_keyboard_ = keyboard; + } + + void reset_all_controls() { + *mode_ = rmcs_msgs::ChassisMode::AUTO; + step_down_facing_ = StepDownFacing::BACK; + + *chassis_control_velocity_ = {nan, nan, nan}; + } + + void update_velocity_control() { + auto translational_velocity = update_translational_velocity_control(); + auto angular_velocity = update_angular_velocity_control(translational_velocity); + + chassis_control_velocity_->vector << translational_velocity, angular_velocity; + } + + 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}; + + Eigen::Vector2d translational_velocity = + Eigen::Rotation2Dd{*gimbal_yaw_angle_} * (*joystick_right_ + keyboard_move); + + if (translational_velocity.norm() > 1.0) + translational_velocity.normalize(); + + translational_velocity *= translational_velocity_max; + + return translational_velocity; + } + + double update_angular_velocity_control(const Eigen::Vector2d& translational_velocity) { + double angular_velocity = 0.0; + double chassis_control_angle = nan; + + if (!std::isnan(*climbing_forward_velocity_)) { + double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); + + constexpr double alignment = 2 * std::numbers::pi; + if (err > alignment / 2) + err -= alignment; + + angular_velocity = following_velocity_controller_.update(err); + + *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; + *chassis_control_angle_ = chassis_control_angle; + return angular_velocity; + } + + switch (*mode_) { + case rmcs_msgs::ChassisMode::AUTO: { + angular_velocity = + update_following_angular_velocity(StepDownFacing::BACK, chassis_control_angle); + + // Keep AUTO rear-following gentle at low translation speed and fully enabled at max. + const double measured_translational_speed = + chassis_velocity_feedback_ready_ ? chassis_velocity_->vector.head<2>().norm() + : translational_velocity.norm(); + angular_velocity *= + std::clamp(measured_translational_speed / translational_velocity_max, 0.0, 0.3); + } break; + case rmcs_msgs::ChassisMode::SPIN: { + angular_velocity = + 0.6 * (spinning_forward_ ? angular_velocity_max : -angular_velocity_max); + } break; + case rmcs_msgs::ChassisMode::STEP_DOWN: { + angular_velocity = + update_following_angular_velocity(step_down_facing_, chassis_control_angle); + } break; + case rmcs_msgs::ChassisMode::LAUNCH_RAMP: { + double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); + + // err: [0, 2pi) -> signed + // In launch ramp mode, only one direction can be used for alignment. + // TODO: Dynamically determine the split angle based on chassis velocity. + constexpr double alignment = 2 * std::numbers::pi; + if (err > alignment / 2) + err -= alignment; + + angular_velocity = following_velocity_controller_.update(err); + } break; + } + *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; + *chassis_control_angle_ = chassis_control_angle; + + return angular_velocity; + } + + double calculate_unsigned_chassis_angle_error(double& chassis_control_angle) { + chassis_control_angle = normalize_positive_angle(*gimbal_yaw_angle_error_); + + // err = setpoint - measurement + // ^ ^ + // |gimbal_yaw_angle_error |chassis_angle + // ^ + // |(2pi - gimbal_yaw_angle) + double err = normalize_positive_angle(chassis_control_angle + *gimbal_yaw_angle_); + + return err; + } + +private: + enum class StepDownFacing { FRONT, BACK }; + + double update_following_angular_velocity( + StepDownFacing target_facing, double& chassis_control_angle) { + double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); + if (target_facing == StepDownFacing::BACK) { + chassis_control_angle = + normalize_positive_angle(chassis_control_angle + std::numbers::pi); + err = normalize_positive_angle(err + std::numbers::pi); + } + + err = normalize_signed_angle(err); + return following_velocity_controller_.update(err); + } + + static double normalize_positive_angle(double angle) { + constexpr double full_turn = 2 * std::numbers::pi; + while (angle >= full_turn) + angle -= full_turn; + while (angle < 0.0) + angle += full_turn; + return angle; + } + + static double normalize_signed_angle(double angle) { + angle = normalize_positive_angle(angle); + if (angle > std::numbers::pi) + angle -= 2 * std::numbers::pi; + return angle; + } + + static constexpr double nan = std::numeric_limits::quiet_NaN(); + // Maximum control velocities + static constexpr double translational_velocity_max = 10.0; + static constexpr double angular_velocity_max = 16.0; + + InputInterface joystick_right_; + InputInterface joystick_left_; + InputInterface switch_right_; + InputInterface switch_left_; + InputInterface mouse_velocity_; + InputInterface mouse_; + InputInterface keyboard_; + InputInterface rotary_knob_; + + rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + + InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_; + InputInterface chassis_velocity_; + InputInterface climbing_forward_velocity_; + OutputInterface chassis_angle_, chassis_control_angle_; + + OutputInterface mode_; + bool spinning_forward_ = true; + bool chassis_velocity_feedback_ready_ = false; + StepDownFacing step_down_facing_ = StepDownFacing::BACK; + pid::PidCalculator following_velocity_controller_; + + OutputInterface chassis_control_velocity_; +}; + +} // namespace rmcs_core::controller::chassis + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::ClimbableInfantryChassisController, rmcs_executor::Component) \ No newline at end of file From 69256f3f7f89e37dbf473a1c6f52784e74c72e71 Mon Sep 17 00:00:00 2001 From: Fin_Resect Date: Fri, 24 Apr 2026 19:20:40 +0800 Subject: [PATCH 24/32] added necessary component --- .../config/climbable-infantry.yaml | 9 +- rmcs_ws/src/rmcs_core/plugins.xml | 6 + ...e_infantry_chassis_climber_controller.cpp} | 7 +- .../chassis/steering_wheel_status.cpp | 104 ++++++++++++++++++ 4 files changed, 122 insertions(+), 4 deletions(-) rename rmcs_ws/src/rmcs_core/src/controller/chassis/{chassis_climber_controller.cpp => climbable_infantry_chassis_climber_controller.cpp} (99%) create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/steering_wheel_status.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml index 07ebdf64..cb4dddad 100644 --- a/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml @@ -29,10 +29,12 @@ rmcs_executor: + + - rmcs_core::controller::chassis::SteeringWheelStatus -> steering_wheel_status - rmcs_core::controller::chassis::ClimbableInfantryChassisController -> 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::ClimbableInfantryChassisClimberController -> climber_controller # - rmcs_auto_aim::AutoAimInitializer -> auto_aim_initializer # - rmcs_auto_aim::AutoAimController -> auto_aim_controller @@ -225,6 +227,11 @@ bullet_feeder_velocity_pid_controller: ki: 0.0 kd: 0.0 +steering_wheel_status: + ros__parameters: + vehicle_radius: 0.28284271247462 + wheel_radius: 0.055 + steering_wheel_controller: ros__parameters: mess: 22.0 diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 7e28d096..f0892bc0 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -35,6 +35,9 @@ Steering wheel controller. + + Steering wheel controller. + Test plugin. @@ -113,6 +116,9 @@ Test plugin. + + Test plugin. + Test plugin. diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/climbable_infantry_chassis_climber_controller.cpp similarity index 99% rename from rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp rename to rmcs_ws/src/rmcs_core/src/controller/chassis/climbable_infantry_chassis_climber_controller.cpp index b74db7ef..576ae3e7 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/climbable_infantry_chassis_climber_controller.cpp @@ -23,11 +23,11 @@ enum class AutoClimbState { FORERAKED }; -class ChassisClimberController +class ClimbableInfantryChassisClimberController : public rmcs_executor::Component , public rclcpp::Node { public: - ChassisClimberController() + ClimbableInfantryChassisClimberController() : Node( get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) @@ -567,4 +567,5 @@ class ChassisClimberController #include PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::chassis::ChassisClimberController, rmcs_executor::Component) \ No newline at end of file + rmcs_core::controller::chassis::ClimbableInfantryChassisClimberController, + rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/steering_wheel_status.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/steering_wheel_status.cpp new file mode 100644 index 00000000..8450b386 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/steering_wheel_status.cpp @@ -0,0 +1,104 @@ +#include + +#include +#include +#include +#include + +namespace rmcs_core::controller::chassis { + +class SteeringWheelStatus + : public rmcs_executor::Component + , public rclcpp::Node { +public: + explicit SteeringWheelStatus() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , vehicle_radius_(get_parameter("vehicle_radius").as_double()) + , wheel_radius_(get_parameter("wheel_radius").as_double()) { + register_input("/chassis/left_front_steering/angle", left_front_steering_angle_); + register_input("/chassis/left_back_steering/angle", left_back_steering_angle_); + register_input("/chassis/right_back_steering/angle", right_back_steering_angle_); + register_input("/chassis/right_front_steering/angle", right_front_steering_angle_); + + register_input("/chassis/left_front_wheel/velocity", left_front_wheel_velocity_); + register_input("/chassis/left_back_wheel/velocity", left_back_wheel_velocity_); + register_input("/chassis/right_back_wheel/velocity", right_back_wheel_velocity_); + register_input("/chassis/right_front_wheel/velocity", right_front_wheel_velocity_); + + register_output("/chassis/velocity", chassis_velocity_, 0.0, 0.0, 0.0); + } + + void update() override { + auto steering_status = calculate_steering_status(); + auto wheel_velocities = calculate_wheel_velocities(); + chassis_velocity_->vector = calculate_chassis_velocity(steering_status, wheel_velocities); + } + +private: + struct SteeringStatus { + Eigen::Vector4d angle, cos_angle, sin_angle; + }; + + SteeringStatus calculate_steering_status() const { + SteeringStatus steering_status; + + steering_status.angle = { + *left_front_steering_angle_, // + *left_back_steering_angle_, // + *right_back_steering_angle_, // + *right_front_steering_angle_ // + }; + steering_status.angle.array() -= std::numbers::pi / 4; + steering_status.cos_angle = steering_status.angle.array().cos(); + steering_status.sin_angle = steering_status.angle.array().sin(); + + return steering_status; + } + + Eigen::Vector4d calculate_wheel_velocities() const { + return { + *left_front_wheel_velocity_, // + *left_back_wheel_velocity_, // + *right_back_wheel_velocity_, // + *right_front_wheel_velocity_ // + }; + } + + Eigen::Vector3d calculate_chassis_velocity( + const SteeringStatus& steering_status, const Eigen::Vector4d& wheel_velocities) const { + Eigen::Vector3d velocity; + double one_quarter_r = wheel_radius_ / 4.0; + velocity.x() = one_quarter_r * wheel_velocities.dot(steering_status.cos_angle); + velocity.y() = one_quarter_r * wheel_velocities.dot(steering_status.sin_angle); + velocity.z() = -one_quarter_r / vehicle_radius_ + * (-wheel_velocities[0] * steering_status.sin_angle[0] + + wheel_velocities[1] * steering_status.cos_angle[1] + + wheel_velocities[2] * steering_status.sin_angle[2] + - wheel_velocities[3] * steering_status.cos_angle[3]); + return velocity; + } + + const double vehicle_radius_; + const double wheel_radius_; + + InputInterface left_front_steering_angle_; + InputInterface left_back_steering_angle_; + InputInterface right_back_steering_angle_; + InputInterface right_front_steering_angle_; + + InputInterface left_front_wheel_velocity_; + InputInterface left_back_wheel_velocity_; + InputInterface right_back_wheel_velocity_; + InputInterface right_front_wheel_velocity_; + + OutputInterface chassis_velocity_; +}; + +} // namespace rmcs_core::controller::chassis + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::SteeringWheelStatus, rmcs_executor::Component) From 1548ca94fbe7c582a11fe6374336be321707d858 Mon Sep 17 00:00:00 2001 From: Fin_Resect Date: Sun, 26 Apr 2026 17:31:31 +0800 Subject: [PATCH 25/32] [fixed](heat_controller)More conservative parameters --- .../config/climbable-infantry.yaml | 19 +++---------------- .../controller/shooting/heat_controller.cpp | 2 +- 2 files changed, 4 insertions(+), 17 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml index cb4dddad..7632de77 100644 --- a/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml @@ -156,33 +156,20 @@ yaw_velocity_pid_controller: ki: 0.00 kd: 0.2 # -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/left_friction - /gimbal/right_friction friction_velocities: - - 615.0 - - 615.0 + - 590.0 + - 590.0 friction_soft_start_stop_time: 1.0 heat_controller: ros__parameters: heat_per_shot: 10000 - reserved_heat: 0 + reserved_heat: 25000 bullet_feeder_controller: ros__parameters: diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp index 37696201..6cbbda2b 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp @@ -32,7 +32,7 @@ class HeatController shooter_heat_ = std::max(0, shooter_heat_ - *shooter_cooling_); if (*bullet_fired_) { - shooter_heat_ += heat_per_shot + 10; + shooter_heat_ += heat_per_shot; } *control_bullet_allowance_ = std::max( From 848d99755b08054d97f85b5b7007536bada95c65 Mon Sep 17 00:00:00 2001 From: Fin_Resect Date: Mon, 27 Apr 2026 19:05:05 +0800 Subject: [PATCH 26/32] [feature][fix](climbable-infantry): Added supercap-related commands. Swapped commands about No.2 and No.3 steering --- .../config/climbable-infantry.yaml | 2 +- .../src/hardware/climbable-infantry.cpp | 33 +++++++------------ 2 files changed, 13 insertions(+), 22 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml index 7632de77..47ce46f2 100644 --- a/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml @@ -53,7 +53,7 @@ Infantry_hardware: # external_imu_port: /dev/ttyUSB0 # bullet_feeder_motor_zero_point: 15111 left_front_zero_point: 5816 - left_back_zero_point: 3776 + left_back_zero_point: 3776 right_back_zero_point: 3084 right_front_zero_point: 5077 diff --git a/rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp index 227e5330..a9530c41 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp @@ -251,8 +251,8 @@ class ClimbableInfantry , tf_(climbable_infantry.tf_) , imu_(1000, 0.2, 0.0) , dr16_(climbable_infantry) - , supercap_(climbable_infantry, climbable_infantry_command) , gimbal_yaw_motor_(climbable_infantry, climbable_infantry_command, "/gimbal/yaw") + , supercap_(climbable_infantry, climbable_infantry_command) , chassis_front_climber_motor_( {climbable_infantry, climbable_infantry_command, "/chassis/climber/left_front_motor"}, @@ -340,7 +340,6 @@ class ClimbableInfantry void update() { imu_.update_status(); dr16_.update_status(); - supercap_.update_status(); *chassis_yaw_velocity_imu_ = imu_.gz(); *chassis_pitch_imu_ = -std::asin(2.0 * (imu_.q0() * imu_.q2() - imu_.q3() * imu_.q1())); @@ -358,6 +357,7 @@ class ClimbableInfantry chassis_back_climber_motor_[1].update_status(); gimbal_yaw_motor_.update_status(); + supercap_.update_status(); tf_->set_state( gimbal_yaw_motor_.angle()); @@ -388,9 +388,9 @@ class ClimbableInfantry .can_data = device::CanPacket8{ chassis_steering_motors_[0].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, chassis_steering_motors_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + supercap_.generate_command(), } .as_bytes(), }); @@ -400,18 +400,6 @@ class ClimbableInfantry .can_data = gimbal_yaw_motor_.generate_torque_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 = 0x200, .can_data = @@ -437,7 +425,7 @@ class ClimbableInfantry chassis_wheel_motors_[1].store_status(data.can_data); } else if (can_id == 0x205) { chassis_steering_motors_[0].store_status(data.can_data); - } else if (can_id == 0x208) { + } else if (can_id == 0x206) { chassis_steering_motors_[1].store_status(data.can_data); } else if (can_id == 0x300) { supercap_.store_status(data.can_data); @@ -488,9 +476,11 @@ class ClimbableInfantry device::Bmi088 imu_; device::Dr16 dr16_; - device::Supercap supercap_; + device::LkMotor gimbal_yaw_motor_; + device::Supercap supercap_; + device::DjiMotor chassis_front_climber_motor_[2]; device::DjiMotor chassis_back_climber_motor_[2]; device::DjiMotor chassis_steering_motors_[2]; @@ -512,6 +502,7 @@ class ClimbableInfantry std::string_view board_serial = {}) : librmcs::agent::CBoard(board_serial) , logger_(climbable_infantry.get_logger()) + , gimbal_bullet_feeder_( climbable_infantry, climbable_infantry_command, "/gimbal/bullet_feeder") , chassis_steering_motors_( @@ -591,9 +582,9 @@ class ClimbableInfantry .can_data = device::CanPacket8{ device::CanPacket8::PaddingQuarter{}, - chassis_steering_motors_[0].generate_command(), - chassis_steering_motors_[1].generate_command(), device::CanPacket8::PaddingQuarter{}, + chassis_steering_motors_[1].generate_command(), + chassis_steering_motors_[0].generate_command(), } .as_bytes(), }); @@ -621,7 +612,7 @@ class ClimbableInfantry chassis_wheel_motors_[0].store_status(data.can_data); } else if (can_id == 0x203) { chassis_wheel_motors_[1].store_status(data.can_data); - } else if (can_id == 0x206) { + } else if (can_id == 0x208) { chassis_steering_motors_[0].store_status(data.can_data); } else if (can_id == 0x207) { chassis_steering_motors_[1].store_status(data.can_data); From ddb2c2c607c178182c54c8bd47151ea69508a6d9 Mon Sep 17 00:00:00 2001 From: Fin_Resect Date: Mon, 27 Apr 2026 22:41:10 +0800 Subject: [PATCH 27/32] [fix](chassis_controller)(yaml): corrected some parameters --- rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml | 2 +- .../src/rmcs_core/src/controller/chassis/chassis_controller.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml index 47ce46f2..6e85cd18 100644 --- a/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml @@ -46,7 +46,7 @@ Infantry_hardware: board_serial_top_board: "D4-632C-37BC-6A17-ACA2-E3FE-91E9" board_serial_bottom_board_one: "D4-6A83-030D-F3D0-9103-F8A5-1246" board_serial_bottom_board_two: "D4-6654-1C0F-7353-AA15-A77B-313D" - yaw_motor_zero_point: 12279 + yaw_motor_zero_point: 8054 pitch_motor_zero_point: 21931 # top_yaw_motor_zero_point: 58744 # viewer_motor_zero_point: 3030 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 af18ba43..050f2825 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_(10.0, 0.0, 1.2) { + , following_velocity_controller_(10.0, 0.0, 2.4) { following_velocity_controller_.output_max = angular_velocity_max; following_velocity_controller_.output_min = -angular_velocity_max; From b280f1a1f9c7f34053471bd250e3c88c6e281ff0 Mon Sep 17 00:00:00 2001 From: Fin_Resect Date: Tue, 28 Apr 2026 22:05:36 +0800 Subject: [PATCH 28/32] [optimize](yaml): changed shot_frequency --- rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml index 6e85cd18..85720d44 100644 --- a/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml @@ -27,9 +27,6 @@ rmcs_executor: - rmcs_core::controller::pid::PidController -> right_friction_velocity_pid_controller - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller - - - - rmcs_core::controller::chassis::SteeringWheelStatus -> steering_wheel_status - rmcs_core::controller::chassis::ClimbableInfantryChassisController -> chassis_controller - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller @@ -164,7 +161,7 @@ friction_wheel_controller: friction_velocities: - 590.0 - 590.0 - friction_soft_start_stop_time: 1.0 + friction_soft_start_stop_time: 0.3 heat_controller: ros__parameters: @@ -174,7 +171,7 @@ heat_controller: bullet_feeder_controller: ros__parameters: bullets_per_feeder_turn: 9.0 - shot_frequency: 30.0 + shot_frequency: 60.0 safe_shot_frequency: 10.0 eject_frequency: 10.0 eject_time: 0.05 From 33a642f861439e17083e9c7095db311bfc312ece Mon Sep 17 00:00:00 2001 From: Palejoker <2797572751@qq.com> Date: Sat, 2 May 2026 17:23:58 +0800 Subject: [PATCH 29/32] =?UTF-8?q?[feature](climbable-infantry=EF=BC=8Cclim?= =?UTF-8?q?bable-infantry)"Replace=202=20Cboards=20with=201RMCS=5Fboard=20?= =?UTF-8?q?in=20hardware=20and=20optimize=20following=20mode=20parameters.?= =?UTF-8?q?"?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../config/climbable-infantry.yaml | 26 +- .../controller/chassis/chassis_controller.cpp | 2 +- .../src/hardware/climbable-infantry.cpp | 406 ++++++++---------- 3 files changed, 204 insertions(+), 230 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml index 85720d44..db53a99d 100644 --- a/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml @@ -41,18 +41,18 @@ rmcs_executor: Infantry_hardware: ros__parameters: board_serial_top_board: "D4-632C-37BC-6A17-ACA2-E3FE-91E9" - board_serial_bottom_board_one: "D4-6A83-030D-F3D0-9103-F8A5-1246" - board_serial_bottom_board_two: "D4-6654-1C0F-7353-AA15-A77B-313D" - yaw_motor_zero_point: 8054 - pitch_motor_zero_point: 21931 + board_serial_bottom_board: "AF-EEF5-24BA-6675-F1B1-C797-50AF-869C-870E" + yaw_motor_zero_point: 7570 + pitch_motor_zero_point: 24243 # top_yaw_motor_zero_point: 58744 # viewer_motor_zero_point: 3030 # external_imu_port: /dev/ttyUSB0 # bullet_feeder_motor_zero_point: 15111 - left_front_zero_point: 5816 - left_back_zero_point: 3776 - right_back_zero_point: 3084 - right_front_zero_point: 5077 + left_front_zero_point: 5806 + right_front_zero_point: 5092 + left_back_zero_point: 3759 + right_back_zero_point: 3103 + value_broadcaster: ros__parameters: @@ -140,18 +140,18 @@ yaw_angle_pid_controller: ros__parameters: measurement: /gimbal/yaw/control_angle_error control: /gimbal/yaw/control_velocity - kp: 15.0 #20.00 + kp: 20.0 #20.00 ki: 0.0 - kd: 0.8 #0.8 + kd: 0.9 #0.9 yaw_velocity_pid_controller: ros__parameters: measurement: /gimbal/yaw/velocity_imu setpoint: /gimbal/yaw/control_velocity control: /gimbal/yaw/control_torque - kp: 2.5 #50.00 + kp: 2.5 #2.5 ki: 0.00 - kd: 0.2 # + kd: 0.6 #0.6 friction_wheel_controller: ros__parameters: @@ -166,7 +166,7 @@ friction_wheel_controller: heat_controller: ros__parameters: heat_per_shot: 10000 - reserved_heat: 25000 + reserved_heat: 5000 bullet_feeder_controller: ros__parameters: 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 050f2825..f183e349 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_(10.0, 0.0, 2.4) { + , following_velocity_controller_(9.0 , 0.0, 1000.0) { following_velocity_controller_.output_max = angular_velocity_max; following_velocity_controller_.output_min = -angular_velocity_max; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp index a9530c41..35948421 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp @@ -8,6 +8,7 @@ #include #include +#include #include #include #include @@ -43,7 +44,6 @@ class ClimbableInfantry 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)); @@ -51,11 +51,8 @@ class ClimbableInfantry top_board_ = std::make_unique( *this, *command_component_, get_parameter("board_serial_top_board").as_string()); - bottom_board_one_ = std::make_unique( - *this, *command_component_, get_parameter("board_serial_bottom_board_one").as_string()); - - bottom_board_two_ = std::make_unique( - *this, *command_component_, get_parameter("board_serial_bottom_board_two").as_string()); + bottom_board_ = std::make_unique( + *this, *command_component_, get_parameter("board_serial_bottom_board").as_string()); tf_->set_transform( Eigen::Translation3d{0.06603, 0.0, 0.082}); @@ -70,39 +67,38 @@ class ClimbableInfantry void update() override { top_board_->update(); - bottom_board_one_->update(); - bottom_board_two_->update(); + bottom_board_->update(); } void command_update() { top_board_->command_update(); - bottom_board_one_->command_update(); - bottom_board_two_->command_update(); + bottom_board_->command_update(); + } private: void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { RCLCPP_INFO( get_logger(), "[gimbal calibration] New yaw offset: %ld", - bottom_board_one_->gimbal_yaw_motor_.calibrate_zero_point()); + bottom_board_->gimbal_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 bullet feeder offset: %ld", - static_cast(bottom_board_two_->gimbal_bullet_feeder_.calibrate_zero_point())); + static_cast(bottom_board_->gimbal_bullet_feeder_.calibrate_zero_point())); RCLCPP_INFO( get_logger(), "[chassis calibration] left front steering offset: %d", - bottom_board_one_->chassis_steering_motors_[0].calibrate_zero_point()); + bottom_board_->chassis_front_steering_motors_[0].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[chassis calibration] right front steering offset: %d", + bottom_board_->chassis_front_steering_motors_[1].calibrate_zero_point()); RCLCPP_INFO( get_logger(), "[chassis calibration] left back steering offset: %d", - bottom_board_two_->chassis_steering_motors_[0].calibrate_zero_point()); + bottom_board_->chassis_back_steering_motors_[0].calibrate_zero_point()); RCLCPP_INFO( get_logger(), "[chassis calibration] right back steering offset: %d", - bottom_board_two_->chassis_steering_motors_[1].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[chassis calibration] right front steering offset: %d", - bottom_board_one_->chassis_steering_motors_[1].calibrate_zero_point()); + bottom_board_->chassis_back_steering_motors_[1].calibrate_zero_point()); } class ClimbableInfantryCommand : public rmcs_executor::Component { @@ -124,6 +120,7 @@ class ClimbableInfantry ClimbableInfantryCommand& climbable_infantry_command, std::string_view board_serial = {}) : librmcs::agent::CBoard(board_serial) + , logger_(climbable_infantry.get_logger()) , tf_(climbable_infantry.tf_) , imu_(1000, 0.2, 0.0) , gimbal_pitch_motor_(climbable_infantry, climbable_infantry_command, "/gimbal/pitch") @@ -194,8 +191,8 @@ class ClimbableInfantry .as_bytes(), }); - builder.can1_transmit({ - .can_id = 0x142, + builder.can2_transmit({ + .can_id = 0x143, .can_data = gimbal_pitch_motor_.generate_command().as_bytes(), }); } @@ -209,14 +206,16 @@ class ClimbableInfantry gimbal_right_friction_.store_status(data.can_data); } else if (can_id == 0x202) { gimbal_left_friction_.store_status(data.can_data); - } else if (can_id == 0x142) { - gimbal_pitch_motor_.store_status(data.can_data); - } + } } 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 == 0x143) { + gimbal_pitch_motor_.store_status(data.can_data); + } } void accelerometer_receive_callback( @@ -228,6 +227,8 @@ class ClimbableInfantry imu_.store_gyroscope_status(data.x, data.y, data.z); } + rclcpp::Logger logger_; + OutputInterface& tf_; device::Bmi088 imu_; @@ -239,20 +240,22 @@ class ClimbableInfantry OutputInterface gimbal_pitch_velocity_imu_; }; - class BottomBoard_one final : private librmcs::agent::CBoard { + class BottomBoard final : private librmcs::agent::RmcsBoardLite { public: friend class ClimbableInfantry; - explicit BottomBoard_one( + explicit BottomBoard( ClimbableInfantry& climbable_infantry, ClimbableInfantryCommand& climbable_infantry_command, std::string_view board_serial = {}) - : librmcs::agent::CBoard(board_serial) + : librmcs::agent::RmcsBoardLite(board_serial) , logger_(climbable_infantry.get_logger()) , tf_(climbable_infantry.tf_) , imu_(1000, 0.2, 0.0) , dr16_(climbable_infantry) , gimbal_yaw_motor_(climbable_infantry, climbable_infantry_command, "/gimbal/yaw") , supercap_(climbable_infantry, climbable_infantry_command) + , gimbal_bullet_feeder_( + climbable_infantry, climbable_infantry_command, "/gimbal/bullet_feeder") , chassis_front_climber_motor_( {climbable_infantry, climbable_infantry_command, "/chassis/climber/left_front_motor"}, @@ -263,36 +266,66 @@ class ClimbableInfantry "/chassis/climber/left_back_motor"}, {climbable_infantry, climbable_infantry_command, "/chassis/climber/right_back_motor"}) - , chassis_steering_motors_( + , chassis_front_steering_motors_( {climbable_infantry, climbable_infantry_command, "/chassis/left_front_steering"}, {climbable_infantry, climbable_infantry_command, "/chassis/right_front_steering"}) - , chassis_wheel_motors_( + , chassis_front_wheel_motors_( {climbable_infantry, climbable_infantry_command, "/chassis/left_front_wheel"}, - {climbable_infantry, climbable_infantry_command, "/chassis/right_front_wheel"}) { + {climbable_infantry, climbable_infantry_command, "/chassis/right_front_wheel"}) + , chassis_back_steering_motors_( + {climbable_infantry, climbable_infantry_command, "/chassis/left_back_steering"}, + {climbable_infantry, climbable_infantry_command, "/chassis/right_back_steering"}) + , chassis_back_wheel_motors_( + {climbable_infantry, climbable_infantry_command, "/chassis/left_back_wheel"}, + {climbable_infantry, climbable_infantry_command, "/chassis/right_back_wheel"}) { gimbal_yaw_motor_.configure( device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( static_cast( climbable_infantry.get_parameter("yaw_motor_zero_point").as_int()))); + gimbal_bullet_feeder_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .enable_multi_turn_angle()); - chassis_steering_motors_[0].configure( + chassis_front_steering_motors_[0].configure( device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} .set_encoder_zero_point( static_cast( climbable_infantry.get_parameter("left_front_zero_point").as_int())) .set_reversed()); - chassis_steering_motors_[1].configure( + chassis_front_steering_motors_[1].configure( device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} .set_encoder_zero_point( static_cast( climbable_infantry.get_parameter("right_front_zero_point").as_int())) .set_reversed()); + chassis_back_steering_motors_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_encoder_zero_point( + static_cast( + climbable_infantry.get_parameter("left_back_zero_point").as_int())) + .set_reversed()); + chassis_back_steering_motors_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_encoder_zero_point( + static_cast( + climbable_infantry.get_parameter("right_back_zero_point").as_int())) + .set_reversed()); - chassis_wheel_motors_[0].configure( + chassis_front_wheel_motors_[0].configure( device::DjiMotor::Config{device::DjiMotor::Type::kM3508} .set_reversed() .set_reduction_ratio(2232. / 169.)); - chassis_wheel_motors_[1].configure( + chassis_front_wheel_motors_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(2232. / 169.)); + chassis_back_wheel_motors_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(2232. / 169.)); + chassis_back_wheel_motors_[1].configure( device::DjiMotor::Config{device::DjiMotor::Type::kM3508} .set_reversed() .set_reduction_ratio(2232. / 169.)); @@ -319,7 +352,7 @@ class ClimbableInfantry [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); }; referee_serial_->write = [this](const std::byte* buffer, size_t size) { - start_transmit().uart1_transmit({ + start_transmit().uart0_transmit({ .uart_data = std::span{buffer, size} }); return size; @@ -330,12 +363,12 @@ class ClimbableInfantry climbable_infantry.register_output("/chassis/pitch_imu", chassis_pitch_imu_, 0.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(const BottomBoard&) = delete; + BottomBoard& operator=(const BottomBoard&) = delete; + BottomBoard(BottomBoard&&) = delete; + BottomBoard& operator=(BottomBoard&&) = delete; - ~BottomBoard_one() final = default; + ~BottomBoard() final = default; void update() { imu_.update_status(); @@ -344,13 +377,6 @@ class ClimbableInfantry *chassis_yaw_velocity_imu_ = imu_.gz(); *chassis_pitch_imu_ = -std::asin(2.0 * (imu_.q0() * imu_.q2() - imu_.q3() * imu_.q1())); - // RCLCPP_INFO( - // logger_, "[chassis calibration] left front steering offset: %lf", - // *chassis_pitch_imu_); - // RCLCPP_INFO( - // logger_, "[chassis calibration] right front steering offset: %d", - // chassis_steering_motors_[1].calibrate_zero_point()); - chassis_front_climber_motor_[0].update_status(); chassis_front_climber_motor_[1].update_status(); chassis_back_climber_motor_[0].update_status(); @@ -358,27 +384,56 @@ class ClimbableInfantry gimbal_yaw_motor_.update_status(); supercap_.update_status(); + gimbal_bullet_feeder_.update_status(); tf_->set_state( gimbal_yaw_motor_.angle()); - for (auto& motor : chassis_wheel_motors_) + for (auto& motor : chassis_front_wheel_motors_) + motor.update_status(); + for (auto& motor : chassis_front_steering_motors_) motor.update_status(); - for (auto& motor : chassis_steering_motors_) + for (auto& motor : chassis_back_wheel_motors_) + motor.update_status(); + for (auto& motor : chassis_back_steering_motors_) motor.update_status(); } void command_update() { auto builder = start_transmit(); + builder.can0_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_back_wheel_motors_[0].generate_command(), + chassis_back_wheel_motors_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + + builder.can0_transmit({ + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + chassis_back_steering_motors_[0].generate_command(), + chassis_back_steering_motors_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can1_transmit({ .can_id = 0x200, .can_data = device::CanPacket8{ - chassis_wheel_motors_[0].generate_command(), + chassis_front_wheel_motors_[0].generate_command(), + chassis_front_wheel_motors_[1].generate_command(), device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}, - chassis_wheel_motors_[1].generate_command(), } .as_bytes(), }); @@ -387,10 +442,22 @@ class ClimbableInfantry .can_id = 0x1FE, .can_data = device::CanPacket8{ - chassis_steering_motors_[0].generate_command(), - chassis_steering_motors_[1].generate_command(), + chassis_front_steering_motors_[0].generate_command(), + chassis_front_steering_motors_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + + builder.can2_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + gimbal_bullet_feeder_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}, - supercap_.generate_command(), } .as_bytes(), }); @@ -400,7 +467,19 @@ class ClimbableInfantry .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes(), }); - builder.can2_transmit({ + builder.can3_transmit({ + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + supercap_.generate_command(), + } + .as_bytes(), + }); + + builder.can3_transmit({ .can_id = 0x200, .can_data = device::CanPacket8{ @@ -411,24 +490,40 @@ class ClimbableInfantry } .as_bytes(), }); + + } private: + void can0_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_back_wheel_motors_[0].store_status(data.can_data); + } else if (can_id == 0x202) { + chassis_back_wheel_motors_[1].store_status(data.can_data); + } else if (can_id == 0x205) { + chassis_back_steering_motors_[0].store_status(data.can_data); + } else if (can_id == 0x206) { + chassis_back_steering_motors_[1].store_status(data.can_data); + } + } + 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_[0].store_status(data.can_data); - } else if (can_id == 0x204) { - chassis_wheel_motors_[1].store_status(data.can_data); + chassis_front_wheel_motors_[0].store_status(data.can_data); + } else if (can_id == 0x202) { + chassis_front_wheel_motors_[1].store_status(data.can_data); } else if (can_id == 0x205) { - chassis_steering_motors_[0].store_status(data.can_data); + chassis_front_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 == 0x300) { - supercap_.store_status(data.can_data); + chassis_front_steering_motors_[1].store_status(data.can_data); } } @@ -437,24 +532,43 @@ class ClimbableInfantry return; auto can_id = data.can_id; - if (can_id == 0x203) { - chassis_front_climber_motor_[0].store_status(data.can_data); - } else if (can_id == 0x204) { - chassis_front_climber_motor_[1].store_status(data.can_data); - } else if (can_id == 0x201) { + if (can_id == 0x201) { + gimbal_bullet_feeder_.store_status(data.can_data); + } else if (can_id == 0x141) { + gimbal_yaw_motor_.store_status(data.can_data); + } + } + + void can3_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_back_climber_motor_[0].store_status(data.can_data); } else if (can_id == 0x202) { chassis_back_climber_motor_[1].store_status(data.can_data); - } else if (can_id == 0x141) { - gimbal_yaw_motor_.store_status(data.can_data); + } else if (can_id == 0x203) { + chassis_front_climber_motor_[0].store_status(data.can_data); + } else if (can_id == 0x204) { + chassis_front_climber_motor_[1].store_status(data.can_data); + } else if (can_id == 0x300) { + supercap_.store_status(data.can_data); } } - void uart1_receive_callback(const librmcs::data::UartDataView& data) override { + void uart0_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()); + // if (!data.uart_data.empty()) { + // RCLCPP_INFO( + // logger_, "ref uart0 recv: size=%zu first=0x%02X", + // data.uart_data.size(), + // std::to_integer(data.uart_data[0])); + // } + } void dbus_receive_callback(const librmcs::data::UartDataView& data) override { @@ -480,11 +594,14 @@ class ClimbableInfantry device::LkMotor gimbal_yaw_motor_; device::Supercap supercap_; + device::DjiMotor gimbal_bullet_feeder_; 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]; + device::DjiMotor chassis_front_steering_motors_[2]; + device::DjiMotor chassis_front_wheel_motors_[2]; + device::DjiMotor chassis_back_steering_motors_[2]; + device::DjiMotor chassis_back_wheel_motors_[2]; rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; OutputInterface referee_serial_; @@ -493,156 +610,13 @@ class ClimbableInfantry OutputInterface chassis_pitch_imu_; }; - class BottomBoard_two final : private librmcs::agent::CBoard { - public: - friend class ClimbableInfantry; - explicit BottomBoard_two( - ClimbableInfantry& climbable_infantry, - ClimbableInfantryCommand& climbable_infantry_command, - std::string_view board_serial = {}) - : librmcs::agent::CBoard(board_serial) - , logger_(climbable_infantry.get_logger()) - - , gimbal_bullet_feeder_( - climbable_infantry, climbable_infantry_command, "/gimbal/bullet_feeder") - , chassis_steering_motors_( - {climbable_infantry, climbable_infantry_command, "/chassis/left_back_steering"}, - {climbable_infantry, climbable_infantry_command, "/chassis/right_back_steering"}) - , chassis_wheel_motors_( - {climbable_infantry, climbable_infantry_command, "/chassis/left_back_wheel"}, - {climbable_infantry, climbable_infantry_command, "/chassis/right_back_wheel"}) { - - gimbal_bullet_feeder_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .enable_multi_turn_angle()); - - chassis_steering_motors_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_encoder_zero_point( - static_cast( - climbable_infantry.get_parameter("left_back_zero_point").as_int())) - .set_reversed()); - chassis_steering_motors_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_encoder_zero_point( - static_cast( - climbable_infantry.get_parameter("right_back_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.)); - } - - 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; - - void update() { - // RCLCPP_INFO( - // logger_, "[chassis calibration] left back steering offset: %d", - // chassis_steering_motors_[0].calibrate_zero_point()); - // RCLCPP_INFO( - // logger_, "[chassis calibration] right back steering offset: %d", - // chassis_steering_motors_[1].calibrate_zero_point()); - - gimbal_bullet_feeder_.update_status(); - for (auto& motor : chassis_wheel_motors_) - motor.update_status(); - for (auto& motor : chassis_steering_motors_) - motor.update_status(); - } - - void command_update() { - auto builder = start_transmit(); - - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - chassis_wheel_motors_[0].generate_command(), - chassis_wheel_motors_[1].generate_command(), - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - - builder.can1_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - chassis_steering_motors_[1].generate_command(), - chassis_steering_motors_[0].generate_command(), - } - .as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_bullet_feeder_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - } - - private: - 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 == 0x202) { - chassis_wheel_motors_[0].store_status(data.can_data); - } else if (can_id == 0x203) { - chassis_wheel_motors_[1].store_status(data.can_data); - } else if (can_id == 0x208) { - chassis_steering_motors_[0].store_status(data.can_data); - } else if (can_id == 0x207) { - chassis_steering_motors_[1].store_status(data.can_data); - } - } - - 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 == 0x201) { - gimbal_bullet_feeder_.store_status(data.can_data); - } - } - - rclcpp::Logger logger_; - - device::DjiMotor gimbal_bullet_feeder_; - device::DjiMotor chassis_steering_motors_[2]; - device::DjiMotor chassis_wheel_motors_[2]; - }; - OutputInterface tf_; + InputInterface robot_chassis_power_limit_; rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; std::shared_ptr top_board_; - std::shared_ptr bottom_board_one_; - std::shared_ptr bottom_board_two_; + std::shared_ptr bottom_board_; }; } // namespace rmcs_core::hardware From a2fb3360da5a42800c6b6660e10f4029fad65729 Mon Sep 17 00:00:00 2001 From: Palejoker <2797572751@qq.com> Date: Sun, 3 May 2026 14:37:39 +0800 Subject: [PATCH 30/32] [feature](chassis_power_controller,climbable_infantry_chassis_climber_controller,climbable-infantry)"Add code for climbing stairs with supercapacitor and adjust stair-climbing parameters." --- rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml | 8 ++++---- .../src/controller/chassis/chassis_power_controller.cpp | 9 ++++++--- .../climbable_infantry_chassis_climber_controller.cpp | 4 ++-- 3 files changed, 12 insertions(+), 9 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml index db53a99d..95d8064f 100644 --- a/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml @@ -101,12 +101,12 @@ value_broadcaster: climber_controller: ros__parameters: front_climber_velocity: 60.0 - back_climber_velocity: 15.0 #13.0 - auto_climb_dash_velocity: 3.0 - auto_climb_support_retract_velocity: 30.0 #47.0 + back_climber_velocity: 20.0 #13.0 + auto_climb_dash_velocity: 4.0 #3.0 + auto_climb_support_retract_velocity: 40.0 #47.0 sync_coefficient: 0.8 first_stair_approach_pitch: 0.590 - second_stair_approach_pitch: 0.345 + second_stair_approach_pitch: 0.320 front_kp: 1.0 front_ki: 0.0 front_kd: 0.5 diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp index 2d084f8b..00123d45 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp @@ -33,6 +33,7 @@ class ChassisPowerController register_input("/referee/chassis/power_limit", chassis_power_limit_referee_); register_input("/referee/chassis/buffer_energy", chassis_buffer_energy_referee_); + register_input("/chassis/climber/left_front_motor/velocity", chassis_climber_left_front_motor_velocity_); register_output("/chassis/supercap/charge_power_limit", supercap_charge_power_limit_, 0.0); register_output("/chassis/control_power_limit", chassis_control_power_limit_, 0.0); @@ -63,7 +64,7 @@ class ChassisPowerController update_virtual_buffer_energy(); - boost_mode_ = keyboard.shift || rotary_knob < -0.9; + boost_mode_ = keyboard.shift || rotary_knob < -0.9 || rotary_knob > 0.9 || keyboard.g; update_control_power_limit(); } @@ -106,9 +107,9 @@ class ChassisPowerController void update_control_power_limit() { double power_limit; - + double chassis_climber_left_front_motor = *chassis_climber_left_front_motor_velocity_; if (boost_mode_ && *supercap_enabled_) - power_limit = *mode_ == rmcs_msgs::ChassisMode::LAUNCH_RAMP + power_limit = (*mode_ == rmcs_msgs::ChassisMode::LAUNCH_RAMP) || (chassis_climber_left_front_motor > 0.5) ? inf_ : *chassis_power_limit_referee_ + 80.0; else @@ -160,6 +161,8 @@ class ChassisPowerController InputInterface chassis_power_limit_referee_; InputInterface chassis_buffer_energy_referee_; + InputInterface chassis_climber_left_front_motor_velocity_; + bool boost_mode_ = false; OutputInterface supercap_charge_power_limit_; double chassis_power_limit_expected_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/climbable_infantry_chassis_climber_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/climbable_infantry_chassis_climber_controller.cpp index 576ae3e7..86278db2 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/climbable_infantry_chassis_climber_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/climbable_infantry_chassis_climber_controller.cpp @@ -506,11 +506,11 @@ class ClimbableInfantryChassisClimberController static constexpr double kBackClimberBlockedTorqueThreshold = 0.1; static constexpr double kBackClimberBlockedVelocityThreshold = 0.1; static constexpr int kAutoClimbAlignConfirmTicks = 50; - static constexpr int kAutoClimbSupportConfirmTicks = 50; + static constexpr int kAutoClimbSupportConfirmTicks = 100; static constexpr int kAutoClimbDashMinTicks = 500; static constexpr int kAutoClimbDashTimeoutTicks = 3000; static constexpr int kAutoClimbForerakedTimeoutTicks = 2000; - static constexpr int kAutoClimbSupportRetractTicks = 1000; + static constexpr int kAutoClimbSupportRetractTicks = 1500; static constexpr int kAutoClimbMaxStairs = 2; static constexpr int kManualSupportRetractConfirmTicks = 50; From 73e8136035555b9d38724002f225595944bdd18e Mon Sep 17 00:00:00 2001 From: Fin_Resect Date: Sun, 3 May 2026 15:09:27 +0800 Subject: [PATCH 31/32] [feature](ui): added ui-climbable-infantry. [fix](chassis power controller):adjusted excess_power_limit. --- .../config/climbable-infantry.yaml | 2 +- rmcs_ws/src/rmcs_core/plugins.xml | 3 + .../chassis/chassis_power_controller.cpp | 27 +- .../controller/shooting/putter_controller.cpp | 524 ------------------ .../referee/app/ui/ui-climbable-infantry.cpp | 172 ++++++ 5 files changed, 191 insertions(+), 537 deletions(-) delete mode 100644 rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/referee/app/ui/ui-climbable-infantry.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml index 95d8064f..8f464574 100644 --- a/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml @@ -7,7 +7,7 @@ rmcs_executor: - 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::Infantry -> referee_ui_hero + - rmcs_core::referee::app::ui::UIClimbableInfantry -> referee_ui_infantry - rmcs_core::referee::Command -> referee_command - rmcs_core::controller::gimbal::SimpleGimbalController -> gimbal_controller diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index f0892bc0..0bd9315c 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -110,6 +110,9 @@ Test plugin. + + Test plugin. + Test plugin. diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp index 00123d45..cf4cb1a8 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp @@ -33,7 +33,9 @@ class ChassisPowerController register_input("/referee/chassis/power_limit", chassis_power_limit_referee_); register_input("/referee/chassis/buffer_energy", chassis_buffer_energy_referee_); - register_input("/chassis/climber/left_front_motor/velocity", chassis_climber_left_front_motor_velocity_); + register_input( + "/chassis/climber/left_front_motor/velocity", + chassis_climber_left_front_motor_velocity_); register_output("/chassis/supercap/charge_power_limit", supercap_charge_power_limit_, 0.0); register_output("/chassis/control_power_limit", chassis_control_power_limit_, 0.0); @@ -52,9 +54,9 @@ class ChassisPowerController using namespace rmcs_msgs; auto switch_right = *switch_right_; - auto switch_left = *switch_left_; - auto keyboard = *keyboard_; - auto rotary_knob = *rotary_knob_; + auto switch_left = *switch_left_; + auto keyboard = *keyboard_; + auto rotary_knob = *rotary_knob_; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { @@ -75,8 +77,8 @@ class ChassisPowerController // charging_power_limit = constexpr double buffer_energy_control_line = 120; // = referee + excess - constexpr double buffer_energy_base_line = 30; // = referee - constexpr double buffer_energy_dead_line = 0; // = 0 + constexpr double buffer_energy_base_line = 30; // = referee + constexpr double buffer_energy_dead_line = 0; // = 0 *supercap_charge_power_limit_ = *chassis_power_limit_referee_ @@ -92,8 +94,8 @@ class ChassisPowerController } void reset_power_control() { - virtual_buffer_energy_ = virtual_buffer_energy_limit_; - boost_mode_ = false; + virtual_buffer_energy_ = virtual_buffer_energy_limit_; + boost_mode_ = false; *chassis_control_power_limit_ = 0.0; } @@ -109,7 +111,8 @@ class ChassisPowerController double power_limit; double chassis_climber_left_front_motor = *chassis_climber_left_front_motor_velocity_; if (boost_mode_ && *supercap_enabled_) - power_limit = (*mode_ == rmcs_msgs::ChassisMode::LAUNCH_RAMP) || (chassis_climber_left_front_motor > 0.5) + power_limit = (*mode_ == rmcs_msgs::ChassisMode::LAUNCH_RAMP) + || (chassis_climber_left_front_motor > 0.5) ? inf_ : *chassis_power_limit_referee_ + 80.0; else @@ -118,8 +121,8 @@ class ChassisPowerController // chassis_control_power_limit = constexpr double supercap_voltage_control_line = 12.5; // = supercap - constexpr double supercap_voltage_base_line = 12.0; // = referee - power_limit = *chassis_power_limit_referee_ + constexpr double supercap_voltage_base_line = 12.0; // = referee + power_limit = *chassis_power_limit_referee_ + (power_limit - *chassis_power_limit_referee_) * std::clamp( (*supercap_voltage_ - supercap_voltage_base_line) @@ -127,7 +130,7 @@ class ChassisPowerController 0.0, 1.0); // Maximum excess power when virtual buffer energy is full. - constexpr double excess_power_limit = 15; + constexpr double excess_power_limit = 0; // 15 power_limit += excess_power_limit; power_limit *= virtual_buffer_energy_ / virtual_buffer_energy_limit_; 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 deleted file mode 100644 index fd645577..00000000 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp +++ /dev/null @@ -1,524 +0,0 @@ -#include - -#include -#include - -#include - -#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/torque", bullet_feeder_torque_); - 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; - - 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; - 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; - 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); - - register_output("/gimbal/shoot/delay_ms", shoot_delay_ms_, nan_); - // initialize_shoot_delay_log(); - } - - ~PutterController() { - /* if (shoot_delay_log_stream_.is_open()) - shoot_delay_log_stream_.close(); */ - } - - void update() override { - const auto switch_right = *switch_right_; - const auto switch_left = *switch_left_; - const auto mouse = *mouse_; - const auto keyboard = *keyboard_; - // const bool bullet_fired_now = *bullet_fired_; - - using namespace rmcs_msgs; - - if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) - || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { - reset_all_controls(); - // update_shoot_delay_measurement(bullet_fired_now); - return; - } - - // 推杆已初始化后的正常控制流程 - if (putter_initialized) { - - // 供弹轮卡弹保护冷却期间的处理 - if (bullet_feeder_cool_down_ > 0) { - bullet_feeder_cool_down_--; - - // 使用角度环反转到“后退一格”的位置以解除卡弹 - 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, Retrying..."); - set_preloading(); - } - } 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) - if (last_photoelectric_sensor_status_ - && *photoelectric_sensor_status_) { - RCLCPP_INFO( - get_logger(), "Photoelectric sensor triggered, shooting!"); - set_shooting(); - } else { - set_preloading(); - } - } - } - if (shoot_stage_ == ShootStage::UPDATING) { - wait_bullet_ready(); - } - - if (shoot_stage_ == ShootStage::PRELOADING) { - // 盲拨模式：始终执行角度环定位 - 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 if (shoot_stage_ == ShootStage::SHOOTING) { - *bullet_feeder_control_torque_ = 0; - RCLCPP_INFO( - get_logger(), "Shooting! Angle: %.2f, Velocity: %.2f", - *bullet_feeder_angle_, *bullet_feeder_velocity_); - } 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 (*putter_angle_ - putter_startpoint >= putter_stroke_) { - if (!shooted) { - RCLCPP_INFO(get_logger(), "%f", *putter_angle_); - } - last_photoelectric_sensor_status_ = false; - 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_updating(); - shooted = false; - } else { - *putter_control_torque_ = - putter_return_velocity_pid_.update(-80. - *putter_velocity_); - } - } else { - // 子弹未发出：继续推进 - *putter_control_torque_ = - putter_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_ = putter_return_velocity_pid_.update(-80. - *putter_velocity_); - update_putter_jam_detection(); - } - - // update_shoot_delay_measurement(bullet_fired_now); - - // 保存当前状态用于下次比较 - 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(); - last_photoelectric_sensor_status_ = false; - bullet_feeder_reset(); - - 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_; - putter_return_velocity_pid_.reset(); - putter_velocity_pid_.reset(); - putter_return_angle_pid.reset(); - *putter_control_torque_ = nan_; - - last_preload_flag_ = false; - - bullet_feeder_faulty_count_ = 0; - bullet_feeder_cool_down_ = 0; - shot_delay_pending_ = false; - last_bullet_fired_ = false; - *shoot_delay_ms_ = nan_; - } - - void set_preloading() { - // RCLCPP_INFO(get_logger(), "PRELOADING"); - if (*photoelectric_sensor_status_) { - last_photoelectric_sensor_status_ = true; - } - 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() { - // RCLCPP_INFO(get_logger(), "PRELOADED"); - if (*photoelectric_sensor_status_) { - last_photoelectric_sensor_status_ = true; - } - shoot_stage_ = ShootStage::PRELOADED; - last_preload_flag_ = false; - } - - void set_shooting() { - // RCLCPP_INFO(get_logger(), "SHOOTING"); - shoot_stage_ = ShootStage::SHOOTING; - shoot_start_time_ = std::chrono::steady_clock::now(); - shot_delay_pending_ = true; - } - - void set_updating() { - // RCLCPP_INFO(get_logger(), "UPDATING"); - shoot_stage_ = ShootStage::UPDATING; - } - - void update_jam_detection() { - // RCLCPP_INFO(get_logger(), "%.2f --", *bullet_feeder_control_torque_); - if (*bullet_feeder_torque_ > 100 && *bullet_feeder_velocity_ < 0.1) { - bullet_feeder_faulty_count_++; - } else { - bullet_feeder_faulty_count_ = 0; - } - if (bullet_feeder_faulty_count_ >= 500) - enter_jam_protection(); - } - - void update_putter_jam_detection() { - if ((*putter_control_torque_ > -0.03 && shoot_stage_ == ShootStage::PRELOADING) - || ((*putter_control_torque_ < 0.08) && 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 { - // 发射状态下检测到堵转：认为子弹已发出 - RCLCPP_INFO(get_logger(), "dd"); - shooted = true; - } - } - } - - void wait_bullet_ready() { - if (bullet_ready_count_ >= 600) { - bullet_ready_count_ = 0; - set_preloading(); - } else { - bullet_ready_count_++; - } - } - - void enter_jam_protection() { - // 设置目标角度为当前角度后退 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(); - RCLCPP_INFO(get_logger(), "Jammed!"); - } - - void bullet_feeder_reset() {} - - static double normalize_feeder_phase(double angle) { - auto normalized = std::fmod(angle, bullet_feeder_angle_per_bullet_); - if (normalized < 0.0) - normalized += bullet_feeder_angle_per_bullet_; - return normalized; - } - - /* 记录发射延迟的测量逻辑： - void update_shoot_delay_measurement(bool bullet_fired_now) { - const bool bullet_fired_rising_edge = bullet_fired_now && !last_bullet_fired_; - - if (shot_delay_pending_ && bullet_fired_rising_edge) { - const auto now = std::chrono::steady_clock::now(); - const double delay_ms = - std::chrono::duration(now - shoot_start_time_).count(); - *shoot_delay_ms_ = delay_ms; - // RCLCPP_INFO( - // get_logger(), "Shoot delay[%zu]: %.3f ms", shoot_delay_sample_count_, delay_ms); - log_shoot_delay(delay_ms); - shot_delay_pending_ = false; - } - - // Shot ended without a bullet_fired edge: drop this sample. - if (shot_delay_pending_ && shoot_stage_ != ShootStage::SHOOTING) - shot_delay_pending_ = false; - - last_bullet_fired_ = bullet_fired_now; - } - - void initialize_shoot_delay_log() { - using namespace std::chrono; - const std::filesystem::path log_dir{"/robot_shoot"}; - std::error_code ec; - std::filesystem::create_directories(log_dir, ec); - if (ec) { - RCLCPP_WARN( - get_logger(), "Failed to create shoot delay log directory %s: %s", log_dir.c_str(), - ec.message().c_str()); - } - - const auto now = system_clock::now(); - const auto ms = duration_cast(now.time_since_epoch()).count(); - shoot_delay_log_path_ = (log_dir / ("shoot_delay_" + std::to_string(ms) + ".log")).string(); - shoot_delay_log_stream_.open(shoot_delay_log_path_); - - if (!shoot_delay_log_stream_.is_open()) { - RCLCPP_WARN( - get_logger(), "Failed to open shoot delay log file: %s", - shoot_delay_log_path_.c_str()); - return; - } - - shoot_delay_log_stream_ << "shot_index,delay_ms,timestamp_ms" << std::endl; - RCLCPP_INFO(get_logger(), "Shoot delay log file: %s", shoot_delay_log_path_.c_str()); - } - - void log_shoot_delay(double delay_ms) { - if (!shoot_delay_log_stream_.is_open()) - return; - - using namespace std::chrono; - const auto timestamp_ms = - duration_cast(system_clock::now().time_since_epoch()).count(); - shoot_delay_log_stream_ << shoot_delay_sample_count_++ << ',' << delay_ms << ',' - << timestamp_ms << std::endl; - shoot_delay_log_stream_.flush(); - } - - */ - - 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; - static constexpr double bullet_feeder_offset = 0.1; - - InputInterface photoelectric_sensor_status_; - InputInterface bullet_fired_; - bool shooted{false}; - bool last_photoelectric_sensor_status_ = 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_torque_; - 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 putter_return_velocity_pid_; - InputInterface putter_velocity_; - - pid::PidCalculator putter_velocity_pid_; - - enum class ShootStage { PRELOADING, PRELOADED, SHOOTING, UPDATING }; - 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; - int bullet_ready_count_ = 0; - - OutputInterface shoot_delay_ms_; - - std::chrono::steady_clock::time_point shoot_start_time_{}; - bool shot_delay_pending_ = false; - bool last_bullet_fired_ = false; - - std::ofstream shoot_delay_log_stream_; - std::string shoot_delay_log_path_; - std::size_t shoot_delay_sample_count_ = 0; - - OutputInterface shoot_mode_; -}; - -} // namespace rmcs_core::controller::shooting - -#include - -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::shooting::PutterController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/ui-climbable-infantry.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/ui-climbable-infantry.cpp new file mode 100644 index 00000000..5ff69bf7 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/ui-climbable-infantry.cpp @@ -0,0 +1,172 @@ +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "referee/app/ui/shape/shape.hpp" +#include "referee/app/ui/widget/status_ring.hpp" + +namespace rmcs_core::referee::app::ui { +using namespace std::chrono_literals; + +class UIClimbableInfantry + : public rmcs_executor::Component + , public rclcpp::Node { +public: + UIClimbableInfantry() + : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , aiming_mark_(Shape::Color::WHITE, 2, x_center - 12, y_center - 37, 10, 10) + , status_ring_(26.5, 26.5, 600, 300) + , chassis_power_number_(Shape::Color::WHITE, 20, 2, x_center - 40, 860, 0) + , yaw_indicator_guidelines_( + {Shape::Color::WHITE, 2, x_center - 32, 830, x_center + 32, 830}, + {Shape::Color::WHITE, 2, x_center, 830, x_center, 820}) + , chassis_direction_indicator_(Shape::Color::PINK, 8, x_center, y_center, 0, 0, 84, 84) + , chassis_control_power_limit_indicator_(Shape::Color::WHITE, 20, 2, x_center + 10, 820, 0) + , supercap_control_power_limit_indicator_(Shape::Color::WHITE, 20, 2, x_center + 10, 790, 0) + , time_reminder_(Shape::Color::PINK, 50, 5, x_center + 150, y_center + 65, 0, false) { + + chassis_control_direction_indicator_.set_x(x_center); + chassis_control_direction_indicator_.set_y(y_center); + + register_input("/chassis/control_mode", chassis_mode_); + + register_input("/chassis/angle", chassis_angle_); + register_input("/chassis/control_angle", chassis_control_angle_); + + register_input("/chassis/supercap/voltage", supercap_voltage_); + register_input("/chassis/supercap/enabled", supercap_enabled_); + + register_input("/chassis/voltage", chassis_voltage_); + register_input("/chassis/power", chassis_power_); + register_input("/chassis/control_power_limit", chassis_control_power_limit_); + register_input("/chassis/supercap/charge_power_limit", supercap_charge_power_limit_); + + register_input("/chassis/left_front_wheel/velocity", left_front_velocity_); + register_input("/chassis/left_back_wheel/velocity", left_back_velocity_); + register_input("/chassis/right_back_wheel/velocity", right_back_velocity_); + register_input("/chassis/right_front_wheel/velocity", right_front_velocity_); + + register_input("/referee/shooter/bullet_allowance", robot_bullet_allowance_); + + register_input("/gimbal/left_friction/control_velocity", left_friction_control_velocity_); + register_input("/gimbal/left_friction/velocity", left_friction_velocity_); + register_input("/gimbal/right_friction/velocity", right_friction_velocity_); + + register_input("/remote/mouse", mouse_); + + register_input("/referee/game/stage", game_stage_); + + // register_input("/auto_aim/ui_target", auto_aim_target_, false); + } + + void update() override { + update_chassis_direction_indicator(); + + chassis_control_power_limit_indicator_.set_value(*chassis_control_power_limit_); + supercap_control_power_limit_indicator_.set_value(*supercap_charge_power_limit_); + + chassis_power_number_.set_value(*chassis_power_); + + status_ring_.update_bullet_allowance(*robot_bullet_allowance_); + status_ring_.update_friction_wheel_speed( + std::min(*left_friction_velocity_, *right_friction_velocity_), + *left_friction_control_velocity_ > 0); + status_ring_.update_supercap(*supercap_voltage_, *supercap_enabled_); + status_ring_.update_battery_power(*chassis_voltage_); + + status_ring_.update_auto_aim_enable(mouse_->right == 1); + } + +private: + void update_time_reminder() { + if (!game_stage_.ready()) + return; + } + + void update_chassis_direction_indicator() { + auto chassis_mode = *chassis_mode_; + + auto to_referee_angle = [](double angle) { + return static_cast( + std::round((2 * std::numbers::pi - angle) / std::numbers::pi * 180)); + }; + chassis_direction_indicator_.set_color( + chassis_mode == rmcs_msgs::ChassisMode::SPIN ? Shape::Color::GREEN + : Shape::Color::PINK); + chassis_direction_indicator_.set_angle(to_referee_angle(*chassis_angle_), 30); + + bool chassis_control_direction_indicator_visible = false; + if (!std::isnan(*chassis_control_angle_)) { + if (chassis_mode == rmcs_msgs::ChassisMode::STEP_DOWN) { + chassis_control_direction_indicator_visible = true; + chassis_control_direction_indicator_.set_color(Shape::Color::CYAN); + chassis_control_direction_indicator_.set_width(8); + chassis_control_direction_indicator_.set_r(92); + chassis_control_direction_indicator_.set_angle( + to_referee_angle(*chassis_control_angle_), 30); + } else if (chassis_mode == rmcs_msgs::ChassisMode::LAUNCH_RAMP) { + chassis_control_direction_indicator_visible = true; + chassis_control_direction_indicator_.set_color(Shape::Color::CYAN); + chassis_control_direction_indicator_.set_width(28); + chassis_control_direction_indicator_.set_r(102); + chassis_control_direction_indicator_.set_angle( + to_referee_angle(*chassis_control_angle_), 4); + } + } + chassis_control_direction_indicator_.set_visible( + chassis_control_direction_indicator_visible); + } + + static constexpr uint16_t screen_width = 1920, screen_height = 1080; + static constexpr uint16_t x_center = screen_width / 2, y_center = screen_height / 2; + + InputInterface chassis_mode_; + InputInterface chassis_angle_, chassis_control_angle_; + + InputInterface supercap_voltage_; + InputInterface supercap_enabled_; + + InputInterface chassis_voltage_; + InputInterface chassis_power_; + InputInterface chassis_control_power_limit_; + InputInterface supercap_charge_power_limit_; + + InputInterface left_front_velocity_, left_back_velocity_, right_back_velocity_, + right_front_velocity_; + + InputInterface robot_bullet_allowance_; + + InputInterface left_friction_control_velocity_; + InputInterface left_friction_velocity_; + InputInterface right_friction_velocity_; + + InputInterface mouse_; + + InputInterface game_stage_; + + // InputInterface> auto_aim_target_; + + Circle aiming_mark_; + StatusRing status_ring_; + + Float chassis_power_number_; + Line yaw_indicator_guidelines_[2]; + + Arc chassis_direction_indicator_, chassis_control_direction_indicator_; + + Float chassis_control_power_limit_indicator_, supercap_control_power_limit_indicator_; + + Integer time_reminder_; +}; + +} // namespace rmcs_core::referee::app::ui + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::app::ui::UIClimbableInfantry, rmcs_executor::Component) \ No newline at end of file From ec3fa5ba70c2a0c017b54316a8e53f651ee29452 Mon Sep 17 00:00:00 2001 From: Fin_Resect Date: Sun, 3 May 2026 19:03:41 +0800 Subject: [PATCH 32/32] [feature][optimize](ui)(climber_controller): added track direction indicator and track start indicator adjusted some parameters --- ...le_infantry_chassis_climber_controller.cpp | 6 +-- .../referee/app/ui/ui-climbable-infantry.cpp | 38 +++++++++++++++++++ rmcs_ws/src/rmcs_core/third_party/tinympc | 1 + 3 files changed, 42 insertions(+), 3 deletions(-) create mode 160000 rmcs_ws/src/rmcs_core/third_party/tinympc diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/climbable_infantry_chassis_climber_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/climbable_infantry_chassis_climber_controller.cpp index 86278db2..652ac130 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/climbable_infantry_chassis_climber_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/climbable_infantry_chassis_climber_controller.cpp @@ -316,7 +316,7 @@ class ClimbableInfantryChassisClimberController AutoClimbControl update_auto_climb_dash() { AutoClimbControl control{ - .front_track_velocity = track_velocity_max_, + .front_track_velocity = 0.0, .back_climber_velocity = climber_back_control_velocity_abs_, .override_chassis_vx = auto_climb_dash_velocity_, }; @@ -349,7 +349,7 @@ class ClimbableInfantryChassisClimberController AutoClimbControl update_auto_climb_support_retract() { AutoClimbControl control{ - .front_track_velocity = track_velocity_max_, + .front_track_velocity = 0.0, .back_climber_velocity = -auto_climb_support_retract_velocity_abs_, .override_chassis_vx = auto_climb_dash_velocity_, }; @@ -498,7 +498,7 @@ class ClimbableInfantryChassisClimberController rclcpp::Logger logger_; static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - static constexpr double kAutoClimbApproachVelocity = 1.0; + static constexpr double kAutoClimbApproachVelocity = 1.5; static constexpr double kAutoClimbAlignThreshold = 0.10; static constexpr double kAutoClimbAlignVelocityThreshold = 0.2; static constexpr double kAutoClimbLeveledPitchThreshold = 0.1; diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/ui-climbable-infantry.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/ui-climbable-infantry.cpp index 5ff69bf7..7693cae4 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/ui-climbable-infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/ui-climbable-infantry.cpp @@ -6,6 +6,7 @@ #include #include #include +#include #include #include "referee/app/ui/shape/shape.hpp" @@ -27,6 +28,11 @@ class UIClimbableInfantry {Shape::Color::WHITE, 2, x_center - 32, 830, x_center + 32, 830}, {Shape::Color::WHITE, 2, x_center, 830, x_center, 820}) , chassis_direction_indicator_(Shape::Color::PINK, 8, x_center, y_center, 0, 0, 84, 84) + ,front_climber_motor_direction_indicator_{{ + Shape::Color::CYAN, 60, x_center, y_center, 0, 0, 64, 64 + }, { + Shape::Color::CYAN, 60, x_center, y_center, 0, 0, 64, 64 + }} , chassis_control_power_limit_indicator_(Shape::Color::WHITE, 20, 2, x_center + 10, 820, 0) , supercap_control_power_limit_indicator_(Shape::Color::WHITE, 20, 2, x_center + 10, 790, 0) , time_reminder_(Shape::Color::PINK, 50, 5, x_center + 150, y_center + 65, 0, false) { @@ -59,6 +65,7 @@ class UIClimbableInfantry register_input("/gimbal/right_friction/velocity", right_friction_velocity_); register_input("/remote/mouse", mouse_); + register_input("/remote/keyboard", keyboard_); register_input("/referee/game/stage", game_stage_); @@ -91,15 +98,42 @@ class UIClimbableInfantry void update_chassis_direction_indicator() { auto chassis_mode = *chassis_mode_; + auto keyboard = *keyboard_; auto to_referee_angle = [](double angle) { return static_cast( std::round((2 * std::numbers::pi - angle) / std::numbers::pi * 180)); }; + + int climber_motor_angle; + int half_intervel_angle = 30; chassis_direction_indicator_.set_color( chassis_mode == rmcs_msgs::ChassisMode::SPIN ? Shape::Color::GREEN : Shape::Color::PINK); chassis_direction_indicator_.set_angle(to_referee_angle(*chassis_angle_), 30); + if (keyboard.g) { + front_climber_motor_direction_indicator_[0].set_color(Shape::Color::ORANGE); + front_climber_motor_direction_indicator_[1].set_color(Shape::Color::ORANGE); + front_climber_motor_direction_indicator_[0].set_width(90); + front_climber_motor_direction_indicator_[1].set_width(90); + front_climber_motor_direction_indicator_[0].set_r(100); + front_climber_motor_direction_indicator_[1].set_r(100); + } else { + front_climber_motor_direction_indicator_[0].set_color(Shape::Color::CYAN); + front_climber_motor_direction_indicator_[1].set_color(Shape::Color::CYAN); + front_climber_motor_direction_indicator_[0].set_width(30); + front_climber_motor_direction_indicator_[1].set_width(30); + front_climber_motor_direction_indicator_[0].set_r(80); + front_climber_motor_direction_indicator_[1].set_r(80); + } + climber_motor_angle = to_referee_angle(*chassis_angle_); + front_climber_motor_direction_indicator_[1].set_angle( + climber_motor_angle + half_intervel_angle, 5); + if (climber_motor_angle - half_intervel_angle < 0) { + climber_motor_angle += 360; + } + front_climber_motor_direction_indicator_[0].set_angle( + climber_motor_angle - half_intervel_angle, 5); bool chassis_control_direction_indicator_visible = false; if (!std::isnan(*chassis_control_angle_)) { @@ -121,6 +155,8 @@ class UIClimbableInfantry } chassis_control_direction_indicator_.set_visible( chassis_control_direction_indicator_visible); + front_climber_motor_direction_indicator_[0].set_visible(true); + front_climber_motor_direction_indicator_[1].set_visible(true); } static constexpr uint16_t screen_width = 1920, screen_height = 1080; @@ -147,6 +183,7 @@ class UIClimbableInfantry InputInterface right_friction_velocity_; InputInterface mouse_; + InputInterface keyboard_; InputInterface game_stage_; @@ -159,6 +196,7 @@ class UIClimbableInfantry Line yaw_indicator_guidelines_[2]; Arc chassis_direction_indicator_, chassis_control_direction_indicator_; + Arc front_climber_motor_direction_indicator_[2]; Float chassis_control_power_limit_indicator_, supercap_control_power_limit_indicator_; diff --git a/rmcs_ws/src/rmcs_core/third_party/tinympc b/rmcs_ws/src/rmcs_core/third_party/tinympc new file mode 160000 index 00000000..6e710a2e --- /dev/null +++ b/rmcs_ws/src/rmcs_core/third_party/tinympc @@ -0,0 +1 @@ +Subproject commit 6e710a2e5813ce261117299b84577294ebc02be0