From fb9af165fd282acad00574fe1be8c34c169bd7fc Mon Sep 17 00:00:00 2001 From: qzhhhi Date: Tue, 2 Jun 2026 18:26:24 +0800 Subject: [PATCH 1/3] feat: Integrate steering-hero mainline support Integrate hardware, chassis, gimbal, shooting, and UI support for steering-hero and steering-hero-little. Also correct LK motor torque constants and retune the omni-infantry / sentry compensating parameters to keep mainline vehicle control output unchanged. Remove the unmaintained mecanum-hero configuration. Known issues (not blocking this merge): - The Hero UI shooter condition / state_word path is still leftover debug wiring and is not connected to the current active runtime path - HeroFrictionWheelController still preserves the historical friction-wheel index convention; for now this is only documented with a TODO and should later be replaced by an explicit first-stage mapping - steering-hero-little has a mismatch between player viewer limit parameters and control logic, which still needs follow-up calibration - hero_chassis_power_controller inherits the risk of reading uninitialized members from the existing chassis_power_controller - steering-hero-little uses different vehicle_radius parameters in steering_wheel_status and steering_wheel_controller --- .../src/rmcs_bringup/config/mecanum-hero.yaml | 161 ---- .../rmcs_bringup/config/omni-infantry.yaml | 6 +- rmcs_ws/src/rmcs_bringup/config/sentry.yaml | 4 +- .../config/steering-hero-little.yaml | 417 +++++++++ .../rmcs_bringup/config/steering-hero.yaml | 399 ++++++++ rmcs_ws/src/rmcs_core/CMakeLists.txt | 4 +- rmcs_ws/src/rmcs_core/plugins.xml | 9 + .../chassis/chassis_climber_controller.cpp | 722 +++++++++++++++ .../chassis/hero_chassis_controller.cpp | 303 ++++++ .../chassis/hero_chassis_power_controller.cpp | 232 +++++ .../hero_steering_wheel_controller.cpp | 518 +++++++++++ .../chassis/steering_wheel_status.cpp | 104 +++ .../controller/gimbal/dual_yaw_controller.cpp | 75 +- .../gimbal/hero_gimbal_controller.cpp | 48 +- .../src/controller/gimbal/player_viewer.cpp | 24 +- .../gimbal/precise_two_axis_gimbal_solver.hpp | 43 +- .../pid/friction_wheel_pid_recorder.cpp | 166 ++++ .../bullet_feeder_controller_42mm.cpp | 32 +- .../hero_friction_wheel_controller.cpp | 263 ++++++ .../shooting/hero_heat_controller.cpp | 63 ++ .../controller/shooting/putter_controller.cpp | 407 ++++++++ .../controller/shooting/shooting_recorder.cpp | 197 +++- .../src/hardware/device/lk_motor.hpp | 7 +- .../src/hardware/steering-hero-little.cpp | 865 ++++++++++++++++++ .../rmcs_core/src/hardware/steering-hero.cpp | 848 +++++++++++------ .../src/rmcs_core/src/referee/app/ui/hero.cpp | 393 +++++++- .../src/referee/app/ui/shape/shape.hpp | 35 +- .../include/rmcs_msgs/shoot_condiction.hpp | 14 + 28 files changed, 5785 insertions(+), 574 deletions(-) delete mode 100644 rmcs_ws/src/rmcs_bringup/config/mecanum-hero.yaml create mode 100644 rmcs_ws/src/rmcs_bringup/config/steering-hero-little.yaml 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/chassis/hero_chassis_controller.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_power_controller.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/hero_steering_wheel_controller.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/steering_wheel_status.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/pid/friction_wheel_pid_recorder.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/shooting/hero_friction_wheel_controller.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/shooting/hero_heat_controller.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little.cpp create mode 100644 rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/shoot_condiction.hpp diff --git a/rmcs_ws/src/rmcs_bringup/config/mecanum-hero.yaml b/rmcs_ws/src/rmcs_bringup/config/mecanum-hero.yaml deleted file mode 100644 index 41d3572a1..000000000 --- a/rmcs_ws/src/rmcs_bringup/config/mecanum-hero.yaml +++ /dev/null @@ -1,161 +0,0 @@ -rmcs_executor: - ros__parameters: - update_rate: 1000.0 - components: - - rmcs_core::hardware::MecanumHero -> hero_hardware - - - rmcs_core::referee::Status -> referee_status - - rmcs_core::referee::Command -> referee_command - - - 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::controller::gimbal::HeroGimbalController -> gimbal_controller - - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_controller - - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller - - - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller - - rmcs_core::controller::shooting::HeatController -> heat_controller - - rmcs_core::controller::shooting::BulletFeederController42mm -> bullet_feeder_controller - - rmcs_core::controller::pid::PidController -> second_left_friction_velocity_pid_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_right_friction_velocity_pid_controller - - - rmcs_core::controller::chassis::ChassisController -> chassis_controller - - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - - rmcs_core::controller::chassis::OmniWheelController -> omni_wheel_controller - -hero_hardware: - ros__parameters: - board_serial_top_board: "(TODO)" - board_serial_bottom_board: "(TODO)" - yaw_motor_zero_point: 61054 - pitch_motor_zero_point: 54062 - external_imu_port: /dev/ttyUSB0 - -gimbal_controller: - ros__parameters: - upper_limit: -0.4598 - lower_limit: 0.4362 - -yaw_angle_pid_controller: - ros__parameters: - measurement: /gimbal/yaw/control_angle_error - feedforward: -/chassis/yaw/velocity_imu - control: /gimbal/yaw/control_velocity - kp: 16.0 - ki: 0.0 - kd: 0.0 - -pitch_angle_pid_controller: - ros__parameters: - measurement: /gimbal/pitch/control_angle_error - control: /gimbal/pitch/control_velocity - kp: 16.00 - ki: 0.0 - kd: 0.0 - output_max: 10.0 - -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 - -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 - -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_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 - -bullet_feeder_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/bullet_feeder/velocity - setpoint: /gimbal/bullet_feeder/control_velocity - control: /gimbal/bullet_feeder/control_torque - kp: 1.0 - ki: 0.001 - kd: 0.0 - integral_min: -1000.0 - integral_max: 1000.0 - -left_front_wheel_velocity_pid_controller: - ros__parameters: - measurement: /chassis/left_front_wheel/velocity - setpoint: /chassis/left_front_wheel/control_velocity - control: /chassis/left_front_wheel/control_torque_unrestricted - kp: 0.185 - ki: 0.00 - kd: 0.00 - -left_back_wheel_velocity_pid_controller: - ros__parameters: - measurement: /chassis/left_back_wheel/velocity - setpoint: /chassis/left_back_wheel/control_velocity - control: /chassis/left_back_wheel/control_torque_unrestricted - kp: 0.185 - ki: 0.00 - kd: 0.00 - -right_back_wheel_velocity_pid_controller: - ros__parameters: - measurement: /chassis/right_back_wheel/velocity - setpoint: /chassis/right_back_wheel/control_velocity - control: /chassis/right_back_wheel/control_torque_unrestricted - kp: 0.185 - ki: 0.00 - kd: 0.00 - -right_front_wheel_velocity_pid_controller: - ros__parameters: - measurement: /chassis/right_front_wheel/velocity - setpoint: /chassis/right_front_wheel/control_velocity - control: /chassis/right_front_wheel/control_torque_unrestricted - kp: 0.185 - ki: 0.00 - kd: 0.00 diff --git a/rmcs_ws/src/rmcs_bringup/config/omni-infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/omni-infantry.yaml index 6364aec7e..304407cf5 100644 --- a/rmcs_ws/src/rmcs_bringup/config/omni-infantry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/omni-infantry.yaml @@ -53,9 +53,9 @@ yaw_velocity_pid_controller: measurement: /gimbal/yaw/velocity_imu setpoint: /gimbal/yaw/control_velocity control: /gimbal/yaw/control_torque - kp: 40.0 - ki: 0.001 - kd: 0.01 + kp: 4.4 + ki: 0.00011 + kd: 0.0011 pitch_angle_pid_controller: ros__parameters: diff --git a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml index 99c037100..cec1cd164 100644 --- a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml @@ -79,7 +79,7 @@ gimbal_controller: bottom_yaw_angle_kp: 15.0 bottom_yaw_angle_ki: 0.0 bottom_yaw_angle_kd: 0.0 - bottom_yaw_velocity_kp: 22.5 + bottom_yaw_velocity_kp: 2.8125 bottom_yaw_velocity_ki: 0.0 bottom_yaw_velocity_kd: 0.0 @@ -92,7 +92,7 @@ gimbal_controller: k_top_to_bottom: -1.0 - bottom_yaw_viscous_ff_gain: 0.002495 + bottom_yaw_viscous_ff_gain: 0.000311875 # bottom_yaw_coulomb_ff_gain: 0.457343 # bottom_yaw_coulomb_ff_tanh_gain: 100.0 top_yaw_viscous_ff_gain: 0.231 diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little.yaml new file mode 100644 index 000000000..539707670 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little.yaml @@ -0,0 +1,417 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::SteeringHeroLittle -> 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::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::HeroFrictionWheelController -> friction_wheel_controller + - rmcs_core::controller::shooting::HeroHeatController -> 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::SteeringWheelStatus -> steering_wheel_status + - rmcs_core::controller::chassis::HeroChassisController -> chassis_controller + - rmcs_core::controller::chassis::HeroChassisPowerController -> chassis_power_controller + - rmcs_core::controller::chassis::HeroSteeringWheelController -> 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 + # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster + + # - sp_vision_25::bridge::HeroAutoAimBridge -> hero_auto_aim_bridge + + # - rmcs_core::controller::identification::SweptFrequencyController -> pitch_swept_frequency_controller + # - rmcs_core::controller::identification::StaticTorqueTestController -> pitch_static_torque_test_controller + # - rmcs_core::controller::identification::SweptFrequencyController -> top_yaw_swept_frequency_controller + # - rmcs_core::controller::identification::SweptFrequencyController -> bottom_yaw_swept_frequency_controller + +hero_hardware: + ros__parameters: + board_serial_top_board: "AF-BFF7-0B6F-46A5-2B4B-AA20-89C2-E180-64B9" + serial_bottom_rmcs_board: "AF-60BB-7484-FA24-F3FC-399B-454D-22FA-1B1D" + bottom_yaw_motor_zero_point: 35078 + pitch_motor_zero_point: 57241 + top_yaw_motor_zero_point: 48537 + viewer_motor_zero_point: 31940 + external_imu_port: /dev/ttyUSB0 + bullet_feeder_motor_zero_point: 60480 #39045 + left_front_zero_point: 5799 + right_front_zero_point: 3700 + left_back_zero_point: 7862 + right_back_zero_point: 1608 + +value_broadcaster: + ros__parameters: + forward_list: + - /gimbal/top_yaw/control_angle + - /chassis/climber/left_front_motor/torque + - /chassis/climber/right_front_motor/torque + - /chassis/left_front_steering/torque + - /chassis/left_back_steering/torque + - /chassis/right_front_steering/torque + - /chassis/right_back_steering/torque + # - /shoot/heat12707 + # - /chassis/power + # - /referee/chassis/power + # - /referee/chassis/power_limit + # - /chassis/control_power_limit + # - /chassis/climber/front/control_power_limit + # - /chassis/climber/front/power_demand_estimate + # - /chassis/climber/front/actual_power_estimate + # - /chassis/steering_wheel/actual_power_estimate + # - /gimbal/putter/velocity + - /gimbal/first_left_friction/velocity + - /gimbal/first_right_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 + # - /gimbal/bottom_yaw/torque + # - /gimbal/bottom_yaw/angle + # - /gimbal/top_yaw/angle + # - /gimbal/pitch/angle + # - /chassis/power + - /chassis/supercap/voltage + # - /chassis/control_power_limit + # - /referee/chassis/power_limit + # - /referee/chassis/buffer_energy + # - /chassis/climber/front/control_power_limit + # - /chassis/climber/front/power_demand_estimate + # - /chassis/climber/front/actual_power_estimate + # - /chassis/steering_wheel/actual_power_estimate + - /gimbal/bullet_feeder/torque + - /gimbal/bullet_feeder/control_torque + - /gimbal/putter/angle + - /gimbal/putter/velocity + - /gimbal/putter/torque + - /gimbal/putter/control_torque + - /gimbal/bullet_feeder/velocity + - /gimbal/bullet_feeder/angle + + +climber_controller: + ros__parameters: + front_climber_velocity: 20.0 + back_climber_velocity: 30.0 + auto_climb_support_retract_velocity_fast: 60.0 + auto_climb_support_retract_velocity_slow: 20.0 + auto_climb_approach_chassis_velocity: 2.0 + auto_climb_support_deploy_chassis_velocity: 0.3 + auto_climb_support_retract_chassis_velocity: 0.3 + auto_climb_dash_chassis_velocity: 3.0 + first_stair_dash_leveled_pitch_threshold: 0.05 + second_stair_dash_leveled_pitch_threshold: -0.09 + sync_coefficient: 0.2 + first_stair_approach_pitch: 0.517 + second_stair_approach_pitch: 0.365 + front_kp: 1.0 + front_ki: 0.0 + front_kd: 0.5 + front_power_estimate_bias: 0.0 + front_power_estimate_k_tau2: 1.0 + front_power_estimate_k_mech: 1.0 + back_kp: 0.5 + back_ki: 0.0 + back_kd: 0.0 + +chassis_power_controller: + ros__parameters: + front_climber_power_limit_max: 50.0 + drive_power_limit_floor: 50.0 + auto_climb_min_control_power_limit: 130.0 + +gimbal_controller: + ros__parameters: + upper_limit: -0.668 + lower_limit: 0.320 + +dual_yaw_controller: + ros__parameters: + top_yaw_angle_kp: 14.0 #30.2 + top_yaw_angle_ki: 0.0 + top_yaw_angle_kd: 0.0 + top_yaw_velocity_kp: 9.37 #11.0 + top_yaw_velocity_ki: 0.00033 #0.00029 + top_yaw_velocity_kd: 0.0 + top_yaw_velocity_integral_min: -2500.0 + top_yaw_velocity_integral_max: 2500.0 + bottom_yaw_angle_kp: 10.0 #18.4 + bottom_yaw_angle_ki: 0.0 + bottom_yaw_angle_kd: 0.0 + bottom_yaw_velocity_kp: 2.00 #2.81 + bottom_yaw_velocity_ki: 0.000071 #0.00028 + bottom_yaw_velocity_kd: 0.0 + bottom_yaw_velocity_integral_min: -2500.0 + bottom_yaw_velocity_integral_max: 2500.0 + +pitch_angle_pid_controller: + ros__parameters: + measurement: /gimbal/pitch/control_angle_error + control: /gimbal/pitch/control_velocity + kp: 18.6 + ki: 0.0 + kd: 0.0 + +pitch_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/pitch/velocity_imu + setpoint: /gimbal/pitch/control_velocity + control: /gimbal/pitch/control_torque + kp: 16.05 + ki: 0.00014 + kd: 0.0 + integral_min: -2500.0 + integral_max: 2500.0 + +gimbal_player_viewer_controller: + ros__parameters: + upper_limit: 0.415996 + lower_limit: -0.066441 + +viewer_angle_pid_controller: + ros__parameters: + measurement: /gimbal/player_viewer/control_angle_error + control: /gimbal/player_viewer/control_velocity + kp: 4.00 + ki: 0.00 + kd: 0.50 + +bullet_feeder_controller: + ros__parameters: + bullet_feeder_velocity_kp: 5.0 + bullet_feeder_velocity_ki: 0.1 #1.1 + bullet_feeder_velocity_kd: 0.0 + bullet_feeder_velocity_integral_min: 0.0 + bullet_feeder_velocity_integral_max: 60.0 + bullet_feeder_angle_kp: 6.0 + bullet_feeder_angle_ki: 0.0 + bullet_feeder_angle_kd: 1.6 + putter_return_velocity_kp: 0.0025 + putter_return_velocity_ki: 0.00005 + putter_return_velocity_kd: 0.0 + putter_return_velocity_integral_min: -0.03 + putter_return_velocity_integral_max: 0.0 + photoelectric_allow: true + +friction_wheel_controller: + ros__parameters: + friction_wheels: + - /gimbal/second_left_friction + - /gimbal/second_right_friction + - /gimbal/first_left_friction + - /gimbal/first_right_friction + friction_velocities_profile_0: + - 380.0 + - 380.0 + - 545.0 + - 545.0 + friction_velocities_profile_1: + - 535.0 + - 535.0 + - 595.0 + - 595.0 + 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.006 + ki: 0.00 + kd: 0.00016 + +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.006 + ki: 0.00 + kd: 0.00016 + +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.006 + ki: 0.00 + kd: 0.00008 + +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.006 + ki: 0.00 + kd: 0.00008 + +steering_wheel_status: + ros__parameters: + vehicle_radius: 0.286378 + wheel_radius: 0.055 + +steering_wheel_controller: + ros__parameters: + mess: 22.0 + moment_of_inertia: 1.08 + vehicle_radius: 0.318198 + 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 + +hero_auto_aim_bridge: + ros__parameters: + config_file: "configs/standard3.yaml" + bullet_speed_fallback: 11.4 + result_timeout: 0.1 # 0.08 + debug: false + + +pitch_swept_frequency_controller: + ros__parameters: + target: /gimbal/pitch + + sweep: true + logarithmic: true + start_freq: 0.1 + end_freq: 10.0 + duration: 60.0 + amplitude: 10.0 + + pid: true + setpoint: 0.0 + position_kp: 20.0 + position_ki: 0.0 + position_kd: 0.0 + velocity_kp: 1.65 + velocity_ki: 0.0 + velocity_kd: 0.0 + dc_offset: 0.0 + +pitch_static_torque_test_controller: + ros__parameters: + target: /gimbal/pitch + + interval_angle: 0.05 + wait_time: 1.5 + border_clip: 0.05 + + position_kp: 12.0 + position_ki: 0.0 + position_kd: 0.0 + + velocity_kp: 3.2 + velocity_ki: 0.0005 + velocity_kd: 0.0 + + velocity_integral_min: -2500.0 + velocity_integral_max: 2500.0 + +top_yaw_swept_frequency_controller: + ros__parameters: + target: /gimbal/top_yaw + + sweep: true + logarithmic: true + start_freq: 0.1 + end_freq: 10.0 + duration: 60.0 + amplitude: 15.0 + + pid: true + setpoint: 0.0 + position_kp: 10.0 + position_ki: 0.0 + position_kd: 0.0 + velocity_kp: 3.0 + velocity_ki: 0.0 + velocity_kd: 0.0 + dc_offset: 0.0 + +bottom_yaw_swept_frequency_controller: + ros__parameters: + target: /gimbal/bottom_yaw + + sweep: true + logarithmic: true + start_freq: 0.1 + end_freq: 4.0 + duration: 80.0 + amplitude: 1.0 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 000000000..a6e01d35c --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml @@ -0,0 +1,399 @@ +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::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::HeroFrictionWheelController -> friction_wheel_controller + - rmcs_core::controller::shooting::HeroHeatController -> 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::SteeringWheelStatus -> steering_wheel_status + - rmcs_core::controller::chassis::HeroChassisController -> chassis_controller + - rmcs_core::controller::chassis::HeroChassisPowerController -> chassis_power_controller + - rmcs_core::controller::chassis::HeroSteeringWheelController -> 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 + # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster + + # - sp_vision_25::bridge::HeroAutoAimBridge -> hero_auto_aim_bridge + + # - rmcs_core::controller::identification::SweptFrequencyController -> pitch_swept_frequency_controller + # - rmcs_core::controller::identification::StaticTorqueTestController -> pitch_static_torque_test_controller + # - rmcs_core::controller::identification::SweptFrequencyController -> top_yaw_swept_frequency_controller + # - rmcs_core::controller::identification::SweptFrequencyController -> bottom_yaw_swept_frequency_controller + + + + +hero_hardware: + ros__parameters: + 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: 37424 + pitch_motor_zero_point: 14916 + top_yaw_motor_zero_point: 33076 + viewer_motor_zero_point: 3030 + external_imu_port: /dev/ttyUSB0 + bullet_feeder_motor_zero_point: 11865 + left_front_zero_point: 4398 + right_front_zero_point: 1081 + left_back_zero_point: 3794 + right_back_zero_point: 5839 + + + + +value_broadcaster: + ros__parameters: + forward_list: + # - /shoot/heat + - /chassis/power + # - /referee/chassis/power + # - /referee/chassis/power_limit + - /chassis/control_power_limit + - /chassis/climber/front/control_power_limit + - /chassis/climber/front/power_demand_estimate + - /chassis/climber/front/actual_power_estimate + - /chassis/steering_wheel/actual_power_estimate + # - /chassis/supercap/voltage + # - /gimbal/putter/velocity + - /gimbal/first_left_friction/velocity + - /gimbal/first_right_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 + # - /gimbal/bottom_yaw/torque + - /gimbal/bottom_yaw/control_angle_shift + - /gimbal/bottom_yaw/angle + - /gimbal/top_yaw/angle + - /gimbal/pitch/angle + + # - /gimbal/pitch/velocity_imu + # - /gimbal/pitch/control_velocity + # - /gimbal/pitch/control_torque + + - /gimbal/auto_aim/plan_yaw + - /gimbal/auto_aim/plan_pitch + +climber_controller: + ros__parameters: + front_climber_velocity: 20.0 + back_climber_velocity: 30.0 + auto_climb_support_retract_velocity_fast: 60.0 + auto_climb_support_retract_velocity_slow: 20.0 + auto_climb_approach_chassis_velocity: 2.0 + auto_climb_support_deploy_chassis_velocity: 0.3 + auto_climb_support_retract_chassis_velocity: 0.3 + auto_climb_dash_chassis_velocity: 3.0 + first_stair_dash_leveled_pitch_threshold: 0.05 + second_stair_dash_leveled_pitch_threshold: -0.09 + sync_coefficient: 0.2 + first_stair_approach_pitch: 0.517 + second_stair_approach_pitch: 0.365 + front_kp: 1.0 + front_ki: 0.0 + front_kd: 0.5 + front_power_estimate_bias: 0.0 + front_power_estimate_k_tau2: 1.0 + front_power_estimate_k_mech: 1.0 + back_kp: 0.5 + back_ki: 0.0 + back_kd: 0.0 + +chassis_power_controller: + ros__parameters: + front_climber_power_limit_max: 60.0 + drive_power_limit_floor: 50.0 + auto_climb_min_control_power_limit: 150.0 + +gimbal_controller: + ros__parameters: + upper_limit: -0.688 + lower_limit: 0.357 + +dual_yaw_controller: + ros__parameters: + top_yaw_angle_kp: 5.0 + top_yaw_angle_ki: 0.0 + top_yaw_angle_kd: 0.0 + top_yaw_velocity_kp: 10.0 + top_yaw_velocity_ki: 0.0 + top_yaw_velocity_kd: 0.0 + top_yaw_velocity_integral_min: -2500.0 + top_yaw_velocity_integral_max: 2500.0 + bottom_yaw_angle_kp: 13.9 + bottom_yaw_angle_ki: 0.0 + bottom_yaw_angle_kd: 0.0 + bottom_yaw_velocity_kp: 2.0 + bottom_yaw_velocity_ki: 0.0 + bottom_yaw_velocity_kd: 0.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: 10.0 + ki: 0.0 + kd: 0.0 + +pitch_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/pitch/velocity_imu + setpoint: /gimbal/pitch/control_velocity + control: /gimbal/pitch/control_torque + kp: 12.00 #45.00 + ki: 0.00 + kd: 0.0 #1.00 + +gimbal_player_viewer_controller: + ros__parameters: + upper_limit: 0.68 + lower_limit: 1.17 + +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_profile_0: + - 368.00 + - 368.00 + - 532.00 + - 532.00 + friction_velocities_profile_1: + - 525.0 + - 525.0 + - 585.0 + - 585.0 + 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 + +bullet_feeder_controller: + ros__parameters: + bullet_feeder_velocity_kp: 5.5 + bullet_feeder_velocity_ki: 1.1 + bullet_feeder_velocity_kd: 0.0 + bullet_feeder_velocity_integral_min: 0.0 + bullet_feeder_velocity_integral_max: 60.0 + bullet_feeder_angle_kp: 5.0 + bullet_feeder_angle_ki: 0.0 + bullet_feeder_angle_kd: 1.0 + putter_return_velocity_kp: 0.0015 + putter_return_velocity_ki: 0.00005 + putter_return_velocity_kd: 0.0 + putter_return_velocity_integral_min: -0.03 + putter_return_velocity_integral_max: 0.0 + photoelectric_allow: false + +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.0005 + ki: 0.00 + kd: 0.00004 + +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.0005 + ki: 0.00 + kd: 0.00004 + +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.0009 + ki: 0.00 + kd: 0.00008 + +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.0009 + ki: 0.00 + kd: 0.00008 + +steering_wheel_status: + ros__parameters: + vehicle_radius: 0.318198 + wheel_radius: 0.055 + +steering_wheel_controller: + ros__parameters: + mess: 22.0 + moment_of_inertia: 1.08 + vehicle_radius: 0.318198 + 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 + +# hero_auto_aim_bridge: +# ros__parameters: +# config_file: "configs/standard3.yaml" +# bullet_speed_fallback: 11.7 +# result_timeout: 0.1 # 0.08 +# debug: false + +pitch_swept_frequency_controller: + ros__parameters: + target: /gimbal/pitch + + sweep: true + logarithmic: true + start_freq: 0.1 + end_freq: 10.0 + duration: 60.0 + amplitude: 10.0 + + pid: true + setpoint: 0.0 + position_kp: 20.0 + position_ki: 0.0 + position_kd: 0.0 + velocity_kp: 1.65 + velocity_ki: 0.0 + velocity_kd: 0.0 + dc_offset: 0.0 + +top_yaw_swept_frequency_controller: + ros__parameters: + target: /gimbal/top_yaw + + sweep: true + logarithmic: true + start_freq: 0.1 + end_freq: 10.0 + duration: 60.0 + amplitude: 20.0 + + pid: true + setpoint: 0.0 + position_kp: 10.0 + position_ki: 0.0 + position_kd: 0.0 + velocity_kp: 3.0 + velocity_ki: 0.0 + velocity_kd: 0.0 + dc_offset: 0.0 + +bottom_yaw_swept_frequency_controller: + ros__parameters: + target: /gimbal/bottom_yaw + + sweep: true + logarithmic: true + start_freq: 0.1 + end_freq: 4.0 + duration: 80.0 + amplitude: 2.0 diff --git a/rmcs_ws/src/rmcs_core/CMakeLists.txt b/rmcs_ws/src/rmcs_core/CMakeLists.txt index 36e7e5706..dc9a98052 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.0/librmcs-sdk-src-3.1.0.zip - URL_HASH SHA256=07107e251745ddb23f7b3e39edec5d6910be1a197025d167ec9849c5c80dd954 + URL https://github.com/Alliance-Algorithm/librmcs/releases/download/v3.2.0b0/librmcs-sdk-src-3.2.0-beta.0.zip + URL_HASH SHA256=391b642a31473fdb639573912e0fc6266c3819d787b91440310cd1025af1dc3c 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 f314d28fc..82ffac1c3 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -1,6 +1,7 @@ + @@ -8,9 +9,14 @@ + + + + + @@ -23,9 +29,12 @@ + + + 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 000000000..cfd1a6695 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp @@ -0,0 +1,722 @@ +#include "controller/pid/matrix_pid_calculator.hpp" +#include "rmcs_msgs/keyboard.hpp" +#include "rmcs_msgs/switch.hpp" +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::controller::chassis { + +namespace { +double estimate_front_power( + double left_torque, double right_torque, double left_velocity, double right_velocity, + double bias, double k_tau2, double k_mech) { + + if (!std::isfinite(left_torque) || !std::isfinite(right_torque)) + return 0.0; + + return bias + k_tau2 * (std::pow(left_torque, 2) + std::pow(right_torque, 2)) + + k_mech + * (std::abs(left_torque * left_velocity) + std::abs(right_torque * right_velocity)); +} +} // namespace + +enum class AutoClimbState { IDLE, ALIGN, APPROACH, SUPPORT_DEPLOY, DASH, SUPPORT_RETRACT }; + +class ChassisClimberController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + ChassisClimberController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , logger_(get_logger()) + , front_velocity_pid_calculator_( + get_parameter("front_kp").as_double(), get_parameter("front_ki").as_double(), + get_parameter("front_kd").as_double()) + , back_velocity_pid_calculator_( + get_parameter("back_kp").as_double(), get_parameter("back_ki").as_double(), + get_parameter("back_kd").as_double()) { + + track_velocity_max_ = get_parameter("front_climber_velocity").as_double(); + climber_back_control_velocity_abs_ = get_parameter("back_climber_velocity").as_double(); + auto_climb_support_retract_velocity_fast_abs_ = + get_parameter("auto_climb_support_retract_velocity_fast").as_double(); + auto_climb_support_retract_velocity_slow_abs_ = + get_parameter("auto_climb_support_retract_velocity_slow").as_double(); + auto_climb_approach_chassis_velocity_ = + get_parameter("auto_climb_approach_chassis_velocity").as_double(); + auto_climb_support_deploy_chassis_velocity_ = + get_parameter("auto_climb_support_deploy_chassis_velocity").as_double(); + auto_climb_support_retract_chassis_velocity_ = + get_parameter("auto_climb_support_retract_chassis_velocity").as_double(); + auto_climb_dash_chassis_velocity_ = + get_parameter("auto_climb_dash_chassis_velocity").as_double(); + first_stair_dash_leveled_pitch_threshold_ = + get_parameter("first_stair_dash_leveled_pitch_threshold").as_double(); + second_stair_dash_leveled_pitch_threshold_ = + get_parameter("second_stair_dash_leveled_pitch_threshold").as_double(); + sync_coefficient_ = get_parameter("sync_coefficient").as_double(); + first_stair_approach_pitch_ = get_parameter("first_stair_approach_pitch").as_double(); + second_stair_approach_pitch_ = get_parameter("second_stair_approach_pitch").as_double(); + front_power_estimate_bias_ = get_parameter("front_power_estimate_bias").as_double(); + front_power_estimate_k_tau2_ = get_parameter("front_power_estimate_k_tau2").as_double(); + front_power_estimate_k_mech_ = get_parameter("front_power_estimate_k_mech").as_double(); + + register_output( + "/chassis/climber/left_front_motor/requested_control_torque", + climber_front_left_requested_control_torque_, nan_); + register_output( + "/chassis/climber/right_front_motor/requested_control_torque", + climber_front_right_requested_control_torque_, nan_); + register_output( + "/chassis/climber/left_back_motor/control_torque", climber_back_left_control_torque_, + nan_); + register_output( + "/chassis/climber/right_back_motor/control_torque", climber_back_right_control_torque_, + nan_); + register_output("/chassis/climbing_forward_velocity", climbing_forward_velocity_, nan_); + register_output("/chassis/climber/auto_climb_active", auto_climb_active_, false); + register_output( + "/chassis/climber/front/power_budget_active", front_power_budget_active_, false); + register_output( + "/chassis/climber/front/power_demand_estimate", front_power_demand_estimate_, 0.0); + + register_input("/chassis/climber/left_front_motor/velocity", climber_front_left_velocity_); + register_input( + "/chassis/climber/right_front_motor/velocity", climber_front_right_velocity_); + register_input("/chassis/climber/left_back_motor/velocity", climber_back_left_velocity_); + register_input("/chassis/climber/right_back_motor/velocity", climber_back_right_velocity_); + + register_input("/chassis/climber/left_back_motor/torque", climber_back_left_torque_); + register_input("/chassis/climber/right_back_motor/torque", climber_back_right_torque_); + + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + register_input("/remote/keyboard", keyboard_); + register_input("/remote/rotary_knob_switch", rotary_knob_switch_); + register_input("/chassis/pitch_imu", chassis_pitch_imu_); + + register_input("/gimbal/yaw/angle", gimbal_yaw_angle_); + register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_); + register_input("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); + + front_power_limiter_ = create_partner_component( + get_component_name() + "_front_power_limiter", front_power_estimate_bias_, + front_power_estimate_k_tau2_, front_power_estimate_k_mech_); + } + + void update() override { + using namespace rmcs_msgs; + auto switch_right = *switch_right_; + auto switch_left = *switch_left_; + auto keyboard = *keyboard_; + auto rotary_knob_switch = *rotary_knob_switch_; + + bool rotary_knob_to_down = + (last_rotary_knob_switch_ != Switch::DOWN && rotary_knob_switch == Switch::DOWN); + bool rotary_knob_from_down = + (last_rotary_knob_switch_ == Switch::DOWN && rotary_knob_switch != Switch::DOWN); + + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + reset_all_controls(); + } else { + handle_auto_climb_requests( + (!last_keyboard_.g && keyboard.g) || rotary_knob_to_down, rotary_knob_from_down, + rotary_knob_switch); + + if (auto_climb_state_ != AutoClimbState::IDLE) { + stop_manual_support(); + apply_climb_control(update_auto_climb_control()); + } else { + apply_climb_control(update_manual_support_control(keyboard)); + } + } + + last_keyboard_ = keyboard; + last_rotary_knob_switch_ = rotary_knob_switch; + } + +private: + struct AutoClimbControl { + double front_track_velocity = nan_; + double back_climber_velocity = nan_; + double override_chassis_vx = nan_; + }; + + class ChassisClimberFrontPowerLimiter : public rmcs_executor::Component { + public: + ChassisClimberFrontPowerLimiter(double bias, double k_tau2, double k_mech) + : front_power_estimate_bias_(bias) + , front_power_estimate_k_tau2_(k_tau2) + , front_power_estimate_k_mech_(k_mech) { + register_input( + "/chassis/climber/left_front_motor/requested_control_torque", + left_requested_control_torque_); + register_input( + "/chassis/climber/right_front_motor/requested_control_torque", + right_requested_control_torque_); + register_input("/chassis/climber/left_front_motor/velocity", left_velocity_); + register_input("/chassis/climber/right_front_motor/velocity", right_velocity_); + register_input("/chassis/climber/left_front_motor/max_torque", left_max_torque_); + register_input("/chassis/climber/right_front_motor/max_torque", right_max_torque_); + register_input("/chassis/climber/front/control_power_limit", control_power_limit_); + + register_output( + "/chassis/climber/left_front_motor/control_torque", left_control_torque_, nan_); + register_output( + "/chassis/climber/right_front_motor/control_torque", right_control_torque_, nan_); + register_output( + "/chassis/climber/front/actual_power_estimate", actual_power_estimate_, 0.0); + } + + void update() override { + const double left_requested = *left_requested_control_torque_; + const double right_requested = *right_requested_control_torque_; + + if (!std::isfinite(left_requested) || !std::isfinite(right_requested)) { + *left_control_torque_ = nan_; + *right_control_torque_ = nan_; + *actual_power_estimate_ = 0.0; + return; + } + + if (*control_power_limit_ <= 0.0) { + *left_control_torque_ = 0.0; + *right_control_torque_ = 0.0; + *actual_power_estimate_ = estimate_front_power( + *left_control_torque_, *right_control_torque_, *left_velocity_, + *right_velocity_, front_power_estimate_bias_, front_power_estimate_k_tau2_, + front_power_estimate_k_mech_); + return; + } + + const double left_torque = + std::clamp(left_requested, -*left_max_torque_, *left_max_torque_); + const double right_torque = + std::clamp(right_requested, -*right_max_torque_, *right_max_torque_); + + const double estimated_power = estimate_front_power( + left_torque, right_torque, *left_velocity_, *right_velocity_, + front_power_estimate_bias_, front_power_estimate_k_tau2_, + front_power_estimate_k_mech_); + + if (estimated_power <= *control_power_limit_ || estimated_power <= 0.0) { + *left_control_torque_ = left_torque; + *right_control_torque_ = right_torque; + *actual_power_estimate_ = estimated_power; + return; + } + + const double scale = std::clamp(*control_power_limit_ / estimated_power, 0.0, 1.0); + *left_control_torque_ = left_torque * scale; + *right_control_torque_ = right_torque * scale; + *actual_power_estimate_ = estimate_front_power( + *left_control_torque_, *right_control_torque_, *left_velocity_, *right_velocity_, + front_power_estimate_bias_, front_power_estimate_k_tau2_, + front_power_estimate_k_mech_); + } + + private: + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + + double front_power_estimate_bias_; + double front_power_estimate_k_tau2_; + double front_power_estimate_k_mech_; + + InputInterface left_requested_control_torque_; + InputInterface right_requested_control_torque_; + InputInterface left_velocity_; + InputInterface right_velocity_; + InputInterface left_max_torque_; + InputInterface right_max_torque_; + InputInterface control_power_limit_; + + OutputInterface left_control_torque_; + OutputInterface right_control_torque_; + OutputInterface actual_power_estimate_; + }; + + void handle_auto_climb_requests( + bool start_requested, bool abort_by_rotary, rmcs_msgs::Switch rotary_knob_switch) { + + if (start_requested) { + if (auto_climb_state_ == AutoClimbState::IDLE) { + start_auto_climb( + rotary_knob_switch == rmcs_msgs::Switch::UP ? "Rotary Knob" : "Keyboard G"); + } else { + abort_auto_climb("toggled again"); + } + } else if (abort_by_rotary && auto_climb_state_ != AutoClimbState::IDLE) { + abort_auto_climb("rotary knob left UP"); + } + } + + void start_auto_climb(const char* source) { + stop_manual_support(); + back_climber_zero_velocity_hold_ = false; + auto_climb_stair_index_ = 0; + auto_climb_align_stable_count_ = 0; + auto_climb_support_block_count_ = 0; + enter_auto_climb_state(AutoClimbState::ALIGN); + + RCLCPP_INFO(logger_, "Auto climb started by %s. Entering ALIGN.", source); + } + + void abort_auto_climb(const char* reason) { + stop_auto_climb(); + start_back_climber_retract("Auto climb exit"); + RCLCPP_INFO(logger_, "Auto climb aborted (%s).", reason); + } + + AutoClimbControl update_auto_climb_control() { + if (auto_climb_state_ == AutoClimbState::IDLE) + return {}; + + auto_climb_timer_++; + + switch (auto_climb_state_) { + case AutoClimbState::IDLE: return {}; + case AutoClimbState::ALIGN: return update_auto_climb_align(); + case AutoClimbState::APPROACH: return update_auto_climb_approach(); + case AutoClimbState::SUPPORT_DEPLOY: return update_auto_climb_support_deploy(); + case AutoClimbState::DASH: return update_auto_climb_dash(); + case AutoClimbState::SUPPORT_RETRACT: return update_auto_climb_support_retract(); + } + + return {}; + } + + AutoClimbControl update_manual_support_control(const rmcs_msgs::Keyboard& keyboard) { + AutoClimbControl control; + + if (keyboard.b) { + back_climber_zero_velocity_hold_ = false; + control.back_climber_velocity = climber_back_control_velocity_abs_; + return control; + } + + if (last_keyboard_.b) { + start_back_climber_retract("Manual support"); + } + + if (!manual_support_retracting_) { + if (back_climber_zero_velocity_hold_) + control.back_climber_velocity = 0.0; + return control; + } + + if (back_climber_recover_count > 1200) { + control.back_climber_velocity = -auto_climb_support_retract_velocity_slow_abs_; + } else { + control.back_climber_velocity = -auto_climb_support_retract_velocity_fast_abs_; + } + + if (is_back_climber_blocked()) + manual_support_retract_block_count_++; + else + manual_support_retract_block_count_ = 0; + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "MANUAL_SUPPORT_RETRACT: blocked_ticks=%d", + manual_support_retract_block_count_); + + if (manual_support_retract_block_count_ >= kManualSupportRetractConfirmTicks) { + stop_manual_support(); + back_climber_zero_velocity_hold_ = true; + control.back_climber_velocity = 0.0; + RCLCPP_INFO(logger_, "Manual support retract completed."); + } + + return control; + } + + AutoClimbControl update_auto_climb_align() { + AutoClimbControl control{ + .front_track_velocity = 0.0, + .back_climber_velocity = -5.0, + .override_chassis_vx = 0.0, + }; + + double gimbal_yaw_angle_error = *gimbal_yaw_angle_error_; + if (gimbal_yaw_angle_error < 0) + gimbal_yaw_angle_error += 2 * std::numbers::pi; + + double err = gimbal_yaw_angle_error + *gimbal_yaw_angle_; + while (err >= std::numbers::pi) + err -= 2 * std::numbers::pi; + while (err < -std::numbers::pi) + err += 2 * std::numbers::pi; + + double yaw_velocity = *gimbal_yaw_velocity_imu_; + bool is_aligned = std::abs(err) < kAutoClimbAlignThreshold; + bool is_stable = std::abs(yaw_velocity) < kAutoClimbAlignVelocityThreshold; + + if (is_aligned && is_stable) + auto_climb_align_stable_count_++; + else + auto_climb_align_stable_count_ = 0; + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "ALIGN: err=%.3f, yaw_velocity=%.3f, stable_ticks=%d", err, + yaw_velocity, auto_climb_align_stable_count_); + + if (auto_climb_align_stable_count_ >= kAutoClimbAlignConfirmTicks) { + enter_auto_climb_state(AutoClimbState::APPROACH); + RCLCPP_INFO(logger_, "Chassis aligned. Entering APPROACH."); + } + + return control; + } + + AutoClimbControl update_auto_climb_approach() { + AutoClimbControl control{ + .front_track_velocity = track_velocity_max_, + .back_climber_velocity = -5.0, + .override_chassis_vx = auto_climb_approach_chassis_velocity_, + }; + + double pitch = *chassis_pitch_imu_; + double target_pitch = auto_climb_stair_index_ == 0 ? first_stair_approach_pitch_ + : second_stair_approach_pitch_; + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "APPROACH (step %d): pitch=%.3f, target>%.3f", + auto_climb_stair_index_ + 1, pitch, target_pitch); + + if (pitch > target_pitch) { + enter_auto_climb_state(AutoClimbState::SUPPORT_DEPLOY); + RCLCPP_INFO( + logger_, "Auto climb entering SUPPORT_DEPLOY (step %d).", + auto_climb_stair_index_ + 1); + } + + return control; + } + + AutoClimbControl update_auto_climb_support_deploy() { + AutoClimbControl control{ + .front_track_velocity = 0.0, + .back_climber_velocity = climber_back_control_velocity_abs_, + .override_chassis_vx = auto_climb_support_deploy_chassis_velocity_, + }; + + if (is_back_climber_blocked()) + auto_climb_support_block_count_++; + else + auto_climb_support_block_count_ = 0; + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "SUPPORT_DEPLOY (step %d): blocked_ticks=%d", + auto_climb_stair_index_ + 1, auto_climb_support_block_count_); + + if (auto_climb_support_block_count_ >= kAutoClimbSupportConfirmTicks) { + enter_auto_climb_state(AutoClimbState::DASH); + RCLCPP_INFO( + logger_, "Auto climb entering DASH (step %d).", auto_climb_stair_index_ + 1); + } + + return control; + } + + AutoClimbControl update_auto_climb_dash() { + AutoClimbControl control{ + .front_track_velocity = 0, + .back_climber_velocity = climber_back_control_velocity_abs_, + .override_chassis_vx = auto_climb_dash_chassis_velocity_, + }; + + double pitch = *chassis_pitch_imu_; + double leveled_pitch_threshold = auto_climb_stair_index_ == 0 + ? first_stair_dash_leveled_pitch_threshold_ + : second_stair_dash_leveled_pitch_threshold_; + bool is_leveled = + pitch < leveled_pitch_threshold && auto_climb_timer_ > kAutoClimbDashMinTicks; + bool timeout = auto_climb_timer_ > kAutoClimbDashTimeoutTicks; + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "DASH (step %d): pitch=%.3f, threshold=%.3f, timer=%d", + auto_climb_stair_index_ + 1, pitch, leveled_pitch_threshold, auto_climb_timer_); + + if (is_leveled || timeout) { + enter_auto_climb_state(AutoClimbState::SUPPORT_RETRACT); + + if (timeout) { + RCLCPP_WARN( + logger_, "Auto climb DASH timeout on step %d. Entering SUPPORT_RETRACT.", + auto_climb_stair_index_ + 1); + } else { + RCLCPP_INFO( + logger_, "Auto climb reached platform on step %d.", + auto_climb_stair_index_ + 1); + } + } + + return control; + } + + AutoClimbControl update_auto_climb_support_retract() { + AutoClimbControl control{ + .front_track_velocity = track_velocity_max_, + .back_climber_velocity = back_climber_recover_count <= 1200 + ? -auto_climb_support_retract_velocity_fast_abs_ + : -auto_climb_support_retract_velocity_slow_abs_, + .override_chassis_vx = auto_climb_support_retract_chassis_velocity_, + }; + + if (is_back_climber_blocked()) + auto_climb_support_block_count_++; + else + auto_climb_support_block_count_ = 0; + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "SUPPORT_RETRACT (step %d): blocked_ticks=%d", + auto_climb_stair_index_ + 1, auto_climb_support_block_count_); + + if (auto_climb_support_block_count_ >= kAutoClimbSupportRetractConfirmTicks) { + bool has_next_stair = auto_climb_stair_index_ + 1 < kAutoClimbMaxStairs; + + if (has_next_stair) { + auto_climb_stair_index_++; + enter_auto_climb_state(AutoClimbState::APPROACH); + RCLCPP_INFO( + logger_, "Auto climb continuing to step %d.", auto_climb_stair_index_ + 1); + } else { + int finished_steps = auto_climb_stair_index_ + 1; + stop_auto_climb(); + back_climber_zero_velocity_hold_ = true; + control.front_track_velocity = nan_; + control.back_climber_velocity = 0.0; + control.override_chassis_vx = nan_; + RCLCPP_INFO(logger_, "Auto climb completed (finished %d steps).", finished_steps); + } + } + + return control; + } + + void apply_climb_control(const AutoClimbControl& control) { + *climbing_forward_velocity_ = control.override_chassis_vx; + *auto_climb_active_ = auto_climb_state_ != AutoClimbState::IDLE; + if (back_climber_recover_count != 0) { + back_climber_recover_count--; + } + + dual_motor_sync_control( + control.front_track_velocity, *climber_front_left_velocity_, + *climber_front_right_velocity_, front_velocity_pid_calculator_, + *climber_front_left_requested_control_torque_, + *climber_front_right_requested_control_torque_); + + dual_motor_sync_control( + control.back_climber_velocity, *climber_back_left_velocity_, + *climber_back_right_velocity_, back_velocity_pid_calculator_, + *climber_back_left_control_torque_, *climber_back_right_control_torque_); + + if (back_climber_recover_count > 1200) { + limit_back_climber_retract_torque( + control.back_climber_velocity, *climber_back_left_control_torque_, + *climber_back_right_control_torque_, back_climber_retract_first_torque_); + } else { + limit_back_climber_retract_torque( + control.back_climber_velocity, *climber_back_left_control_torque_, + *climber_back_right_control_torque_, back_climber_retract_second_torque_); + } + + *front_power_budget_active_ = is_front_power_budget_active(); + *front_power_demand_estimate_ = estimate_front_power( + *climber_front_left_requested_control_torque_, + *climber_front_right_requested_control_torque_, *climber_front_left_velocity_, + *climber_front_right_velocity_, front_power_estimate_bias_, + front_power_estimate_k_tau2_, front_power_estimate_k_mech_); + } + + void reset_all_controls() { + *climber_front_left_requested_control_torque_ = nan_; + *climber_front_right_requested_control_torque_ = nan_; + *climber_back_left_control_torque_ = nan_; + *climber_back_right_control_torque_ = nan_; + *climbing_forward_velocity_ = nan_; + *auto_climb_active_ = false; + *front_power_budget_active_ = false; + *front_power_demand_estimate_ = 0.0; + stop_manual_support(); + stop_auto_climb(); + back_climber_zero_velocity_hold_ = false; + } + + void stop_manual_support() { + back_climber_recover_count = 1500; + manual_support_retracting_ = false; + manual_support_retract_block_count_ = 0; + } + + void start_back_climber_retract(const char* source) { + if (!back_climber_recover_count) { + back_climber_recover_count = 1500; + } + manual_support_retracting_ = true; + manual_support_retract_block_count_ = 0; + back_climber_zero_velocity_hold_ = false; + RCLCPP_INFO(logger_, "%s back climber retract started.", source); + } + + void stop_auto_climb() { + auto_climb_state_ = AutoClimbState::IDLE; + auto_climb_timer_ = 0; + auto_climb_stair_index_ = 0; + auto_climb_align_stable_count_ = 0; + auto_climb_support_block_count_ = 0; + } + + void enter_auto_climb_state(AutoClimbState state) { + if (state == auto_climb_state_) + return; + auto_climb_state_ = state; + auto_climb_timer_ = 0; + auto_climb_align_stable_count_ = 0; + auto_climb_support_block_count_ = 0; + } + + bool is_back_climber_blocked() const { + return (std::abs(*climber_back_left_torque_) > kBackClimberBlockedTorqueThreshold + && std::abs(*climber_back_left_velocity_) < kBackClimberBlockedVelocityThreshold) + || (std::abs(*climber_back_right_torque_) > kBackClimberBlockedTorqueThreshold + && std::abs(*climber_back_right_velocity_) < kBackClimberBlockedVelocityThreshold); + } + + bool is_front_power_budget_active() const { + return auto_climb_state_ == AutoClimbState::APPROACH + || auto_climb_state_ == AutoClimbState::SUPPORT_RETRACT; + } + + void dual_motor_sync_control( + double setpoint, double left_velocity, double right_velocity, + pid::MatrixPidCalculator<2>& pid_calculator, double& left_torque_out, + double& right_torque_out) { + + if (std::isnan(setpoint)) { + left_torque_out = nan_; + right_torque_out = nan_; + return; + } + + Eigen::Vector2d setpoint_error{setpoint - left_velocity, setpoint - right_velocity}; + Eigen::Vector2d relative_velocity{ + left_velocity - right_velocity, right_velocity - left_velocity}; + + Eigen::Vector2d control_error = setpoint_error - sync_coefficient_ * relative_velocity; + auto control_torques = pid_calculator.update(control_error); + + left_torque_out = control_torques[0]; + right_torque_out = control_torques[1]; + } + + void limit_back_climber_retract_torque( + double back_climber_velocity_setpoint, double& left_torque, double& right_torque, + double max_torque) const { + + if (!std::isfinite(back_climber_velocity_setpoint) || back_climber_velocity_setpoint >= 0.0) + return; + if (!(max_torque > 0.0)) + return; + + const double peak = std::max(std::abs(left_torque), std::abs(right_torque)); + if (peak <= max_torque) + return; + + const double scale = max_torque / peak; + left_torque *= scale; + right_torque *= scale; + } + + rclcpp::Logger logger_; + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + static constexpr double kAutoClimbAlignThreshold = 0.10; + static constexpr double kAutoClimbAlignVelocityThreshold = 0.2; + static constexpr double kBackClimberBlockedTorqueThreshold = 0.1; + static constexpr double kBackClimberBlockedVelocityThreshold = 0.1; + static constexpr int kAutoClimbAlignConfirmTicks = 50; + static constexpr int kAutoClimbSupportConfirmTicks = 50; + static constexpr int kAutoClimbDashMinTicks = 100; + static constexpr int kAutoClimbDashTimeoutTicks = 3000; + static constexpr int kAutoClimbSupportRetractConfirmTicks = 50; + static constexpr int kAutoClimbMaxStairs = 2; + static constexpr int kManualSupportRetractConfirmTicks = 50; + + double sync_coefficient_; + double first_stair_approach_pitch_; + double second_stair_approach_pitch_; + + double track_velocity_max_; + double climber_back_control_velocity_abs_; + double auto_climb_support_retract_velocity_fast_abs_; + double auto_climb_support_retract_velocity_slow_abs_; + double auto_climb_approach_chassis_velocity_; + double auto_climb_support_deploy_chassis_velocity_; + double auto_climb_support_retract_chassis_velocity_; + double auto_climb_dash_chassis_velocity_; + double first_stair_dash_leveled_pitch_threshold_; + double second_stair_dash_leveled_pitch_threshold_; + double front_power_estimate_bias_; + double front_power_estimate_k_tau2_; + double front_power_estimate_k_mech_; + + AutoClimbState auto_climb_state_ = AutoClimbState::IDLE; + int auto_climb_timer_ = 0; + int auto_climb_stair_index_ = 0; + int auto_climb_align_stable_count_ = 0; + int auto_climb_support_block_count_ = 0; + bool manual_support_retracting_ = false; + int manual_support_retract_block_count_ = 0; + bool back_climber_zero_velocity_hold_ = false; + + OutputInterface climber_front_left_requested_control_torque_; + OutputInterface climber_front_right_requested_control_torque_; + OutputInterface climber_back_left_control_torque_; + OutputInterface climber_back_right_control_torque_; + OutputInterface climbing_forward_velocity_; + OutputInterface auto_climb_active_; + OutputInterface front_power_budget_active_; + OutputInterface front_power_demand_estimate_; + + InputInterface climber_front_left_velocity_; + InputInterface climber_front_right_velocity_; + InputInterface climber_back_left_velocity_; + InputInterface climber_back_right_velocity_; + + InputInterface climber_back_left_torque_; + InputInterface climber_back_right_torque_; + + InputInterface switch_right_; + InputInterface switch_left_; + InputInterface keyboard_; + InputInterface rotary_knob_switch_; + + InputInterface chassis_pitch_imu_; + InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_, gimbal_yaw_velocity_imu_; + + rmcs_msgs::Switch last_rotary_knob_switch_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + + pid::MatrixPidCalculator<2> front_velocity_pid_calculator_, back_velocity_pid_calculator_; + + std::shared_ptr front_power_limiter_; + + double back_climber_retract_first_torque_ = 8.0; + double back_climber_retract_second_torque_ = 0.5; + int back_climber_recover_count = 0; +}; +} // namespace rmcs_core::controller::chassis + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::ChassisClimberController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp new file mode 100644 index 000000000..dd12e09ee --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp @@ -0,0 +1,303 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "controller/pid/pid_calculator.hpp" + +namespace rmcs_core::controller::chassis { + +class HeroChassisController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + HeroChassisController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , 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; + + 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::AUTO; + } else { + mode = rmcs_msgs::ChassisMode::SPIN; + spinning_forward_ = !spinning_forward_; + } + } else if (!last_keyboard_.x && keyboard.x) { + if (mode != rmcs_msgs::ChassisMode::STEP_DOWN + || (mode == rmcs_msgs::ChassisMode::STEP_DOWN + && step_down_facing_ == StepDownFacing::BACK)) { + mode = rmcs_msgs::ChassisMode::STEP_DOWN; + step_down_facing_ = StepDownFacing::FRONT; + } else { + mode = rmcs_msgs::ChassisMode::AUTO; + step_down_facing_ = StepDownFacing::FRONT; + } + } else if (!last_keyboard_.z && keyboard.z) { + if (mode != rmcs_msgs::ChassisMode::STEP_DOWN + || (mode == rmcs_msgs::ChassisMode::STEP_DOWN + && step_down_facing_ == StepDownFacing::FRONT)) { + mode = rmcs_msgs::ChassisMode::STEP_DOWN; + step_down_facing_ = StepDownFacing::BACK; + } else { + mode = rmcs_msgs::ChassisMode::AUTO; + step_down_facing_ = StepDownFacing::BACK; + } + } + *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::HeroChassisController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_power_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_power_controller.cpp new file mode 100644 index 000000000..ee59e1ce5 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_power_controller.cpp @@ -0,0 +1,232 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "referee/app/ui/shape/shape.hpp" + +namespace rmcs_core::controller::chassis { + +using namespace referee::app; + +class HeroChassisPowerController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + HeroChassisPowerController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { + get_parameter("front_climber_power_limit_max", front_climber_power_limit_max_); + get_parameter("drive_power_limit_floor", drive_power_limit_floor_); + get_parameter("auto_climb_min_control_power_limit", auto_climb_min_control_power_limit_); + register_input("/chassis/control_mode", mode_); + + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + register_input("/remote/keyboard", keyboard_); + register_input("/remote/rotary_knob", rotary_knob_); + + register_input("/chassis/power", chassis_power_); + register_input("/chassis/supercap/voltage", supercap_voltage_); + register_input("/chassis/supercap/enabled", supercap_enabled_); + + register_input("/referee/chassis/power_limit", chassis_power_limit_referee_); + register_input("/referee/chassis/buffer_energy", chassis_buffer_energy_referee_); + register_input( + "/chassis/climber/front/power_budget_active", front_power_budget_active_, false); + register_input( + "/chassis/climber/front/power_demand_estimate", front_power_demand_estimate_, false); + register_input("/chassis/climber/auto_climb_active", auto_climb_active_, false); + + 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); + register_output( + "/chassis/climber/front/control_power_limit", front_climber_control_power_limit_, 0.0); + + register_output( + "/chassis/supercap/voltage/control_line", supercap_voltage_control_line_, 12.5); + register_output("/chassis/supercap/voltage/base_line", supercap_voltage_base_line_, 12.0); + register_output("/chassis/supercap/voltage/dead_line", supercap_voltage_dead_line_, 11.0); + } + + void before_updating() override { + if (!front_power_budget_active_.ready()) + front_power_budget_active_.make_and_bind_directly(false); + if (!front_power_demand_estimate_.ready()) + front_power_demand_estimate_.make_and_bind_directly(0.0); + if (!auto_climb_active_.ready()) + auto_climb_active_.make_and_bind_directly(false); + } + + void update() override { + update_charging_power_limit(); + + update_ui(); + + using namespace rmcs_msgs; + + auto switch_right = *switch_right_; + 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)) { + reset_power_control(); + return; + } + + update_virtual_buffer_energy(); + + boost_mode_ = keyboard.shift || rotary_knob < -0.9 || *auto_climb_active_; + update_control_power_limit(); + } + +private: + void update_charging_power_limit() { + // Maximum excess power when buffer energy is sufficient. + constexpr double excess_power_limit = 35; + + // 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 + + *supercap_charge_power_limit_ = + *chassis_power_limit_referee_ + * std::clamp( + (*chassis_buffer_energy_referee_ - buffer_energy_dead_line) + / (buffer_energy_base_line - buffer_energy_dead_line), + 0.0, 1.0) + + excess_power_limit + * std::clamp( + (*chassis_buffer_energy_referee_ - buffer_energy_base_line) + / (buffer_energy_control_line - buffer_energy_base_line), + 0.0, 1.0); + } + + void reset_power_control() { + virtual_buffer_energy_ = virtual_buffer_energy_limit_; + boost_mode_ = false; + *chassis_control_power_limit_ = 0.0; + *front_climber_control_power_limit_ = 0.0; + } + + void update_virtual_buffer_energy() { + constexpr double dt = 1e-3; + virtual_buffer_energy_ += dt * (chassis_power_limit_expected_ - *chassis_power_); + virtual_buffer_energy_ = std::clamp( + virtual_buffer_energy_, 0.0, + std::min(*chassis_buffer_energy_referee_, virtual_buffer_energy_limit_)); + } + + void update_control_power_limit() { + double total_power_limit; + + if (boost_mode_ && *supercap_enabled_) + total_power_limit = *mode_ == rmcs_msgs::ChassisMode::LAUNCH_RAMP + ? inf_ + : *chassis_power_limit_referee_ + 80.0; + else + total_power_limit = *chassis_power_limit_referee_; + chassis_power_limit_expected_ = total_power_limit; + + // chassis_control_power_limit = + constexpr double supercap_voltage_control_line = 12.5; // = supercap + constexpr double supercap_voltage_base_line = 12.0; // = referee + total_power_limit = + *chassis_power_limit_referee_ + + (total_power_limit - *chassis_power_limit_referee_) + * std::clamp( + (*supercap_voltage_ - supercap_voltage_base_line) + / (supercap_voltage_control_line - supercap_voltage_base_line), + 0.0, 1.0); + + // Maximum excess power when virtual buffer energy is full. + constexpr double excess_power_limit = 0; + + total_power_limit += excess_power_limit; + total_power_limit *= virtual_buffer_energy_ / virtual_buffer_energy_limit_; + + if (*auto_climb_active_) + total_power_limit = std::max(total_power_limit, auto_climb_min_control_power_limit_); + + const auto [drive_limit, front_limit] = split_control_power_limit(total_power_limit); + *chassis_control_power_limit_ = drive_limit; + *front_climber_control_power_limit_ = front_limit; + } + + std::pair split_control_power_limit(double total_power_limit) const { + if (!*front_power_budget_active_) + return {total_power_limit, 0.0}; + + if (total_power_limit <= drive_power_limit_floor_) + return {total_power_limit, 0.0}; + + const double front_limit = std::min( + {*front_power_demand_estimate_, front_climber_power_limit_max_, + total_power_limit - drive_power_limit_floor_}); + return {total_power_limit - front_limit, front_limit}; + } + + void update_ui() { + chassis_power_ui_.set_value(static_cast(std::round(*chassis_power_))); + chassis_control_power_limit_ui_.set_value( + static_cast(std::round(*chassis_control_power_limit_))); + } + + static constexpr double inf_ = std::numeric_limits::infinity(); + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + + InputInterface mode_; + + InputInterface switch_right_; + InputInterface switch_left_; + InputInterface keyboard_; + InputInterface rotary_knob_; + + InputInterface chassis_power_; + static constexpr double virtual_buffer_energy_limit_ = 30.0; + double virtual_buffer_energy_; + + InputInterface supercap_voltage_; + InputInterface supercap_enabled_; + + InputInterface chassis_power_limit_referee_; + InputInterface chassis_buffer_energy_referee_; + InputInterface front_power_budget_active_; + InputInterface front_power_demand_estimate_; + InputInterface auto_climb_active_; + + bool boost_mode_ = false; + OutputInterface supercap_charge_power_limit_; + double chassis_power_limit_expected_; + OutputInterface chassis_control_power_limit_; + OutputInterface front_climber_control_power_limit_; + double front_climber_power_limit_max_; + double drive_power_limit_floor_; + double auto_climb_min_control_power_limit_ = 0.0; + + OutputInterface supercap_voltage_control_line_; + OutputInterface supercap_voltage_base_line_; + OutputInterface supercap_voltage_dead_line_; + + ui::Integer chassis_power_ui_{ui::Shape::Color::WHITE, 15, 2, ui::x_center, 100, 0}; + ui::Integer chassis_control_power_limit_ui_{ + ui::Shape::Color::WHITE, 15, 2, ui::x_center, 150, 0}; +}; + +} // namespace rmcs_core::controller::chassis + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::HeroChassisPowerController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_steering_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_steering_wheel_controller.cpp new file mode 100644 index 000000000..1db280c50 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_steering_wheel_controller.cpp @@ -0,0 +1,518 @@ +#include + +#include +#include +#include +#include +#include +#include + +#include "controller/chassis/qcp_solver.hpp" +#include "controller/pid/matrix_pid_calculator.hpp" +#include "controller/pid/pid_calculator.hpp" +#include "filter/low_pass_filter.hpp" + +namespace rmcs_core::controller::chassis { + +class HeroSteeringWheelController + : public rmcs_executor::Component + , public rclcpp::Node { + + using Formula = std::tuple; + +public: + explicit HeroSteeringWheelController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , mess_(get_parameter("mess").as_double()) + , moment_of_inertia_(get_parameter("moment_of_inertia").as_double()) + , vehicle_radius_(get_parameter("vehicle_radius").as_double()) + , wheel_radius_(get_parameter("wheel_radius").as_double()) + , friction_coefficient_(get_parameter("friction_coefficient").as_double()) + , k1_(get_parameter("k1").as_double()) + , k2_(get_parameter("k2").as_double()) + , no_load_power_(get_parameter("no_load_power").as_double()) + , control_acceleration_filter_(5.0, 1000.0) + , chassis_velocity_expected_(Eigen::Vector3d::Zero()) + , chassis_translational_velocity_pid_(5.0, 0.0, 1.0) + , chassis_angular_velocity_pid_(5.0, 0.0, 1.0) + , cos_varphi_(1, 0, -1, 0) // 0, pi/2, pi, 3pi/2 + , sin_varphi_(0, 1, 0, -1) + , steering_velocity_pid_(0.15, 0.0, 0.0) + , steering_angle_pid_(30.0, 0.0, 0.0) + , wheel_velocity_pid_(0.6, 0.0, 0.0) { + + register_input("/remote/joystick/right", joystick_right_); + register_input("/remote/joystick/left", joystick_left_); + + 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_steering/velocity", left_front_steering_velocity_); + register_input("/chassis/left_back_steering/velocity", left_back_steering_velocity_); + register_input("/chassis/right_back_steering/velocity", right_back_steering_velocity_); + register_input("/chassis/right_front_steering/velocity", right_front_steering_velocity_); + + 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_input("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_); + register_input("/chassis/control_velocity", chassis_control_velocity_); + register_input("/chassis/control_power_limit", power_limit_); + + register_output( + "/chassis/left_front_steering/control_torque", left_front_steering_control_torque_); + register_output( + "/chassis/left_back_steering/control_torque", left_back_steering_control_torque_); + register_output( + "/chassis/right_back_steering/control_torque", right_back_steering_control_torque_); + register_output( + "/chassis/right_front_steering/control_torque", right_front_steering_control_torque_); + + register_output( + "/chassis/left_front_wheel/control_torque", left_front_wheel_control_torque_); + register_output("/chassis/left_back_wheel/control_torque", left_back_wheel_control_torque_); + register_output( + "/chassis/right_back_wheel/control_torque", right_back_wheel_control_torque_); + register_output( + "/chassis/right_front_wheel/control_torque", right_front_wheel_control_torque_); + register_output( + "/chassis/steering_wheel/actual_power_estimate", steering_wheel_actual_power_estimate_, + 0.0); + } + + void update() override { + if (std::isnan(chassis_control_velocity_->vector[0])) { + reset_all_controls(); + return; + } + + integral_yaw_angle_imu(); + + auto steering_status = calculate_steering_status(); + auto wheel_velocities = calculate_wheel_velocities(); + auto chassis_velocity = calculate_chassis_velocity(steering_status, wheel_velocities); + + auto chassis_status_expected = calculate_chassis_status_expected(chassis_velocity); + auto chassis_control_velocity = calculate_chassis_control_velocity(); + + auto chassis_acceleration = calculate_chassis_control_acceleration( + chassis_status_expected.velocity, chassis_control_velocity); + + double power_limit = + *power_limit_ - no_load_power_ - k2_ * wheel_velocities.array().pow(2).sum(); + + auto wheel_pid_torques = + calculate_wheel_pid_torques(steering_status, wheel_velocities, chassis_status_expected); + + auto constrained_chassis_acceleration = constrain_chassis_control_acceleration( + steering_status, wheel_velocities, chassis_acceleration, wheel_pid_torques, + power_limit); + auto filtered_chassis_acceleration = + odom_to_base_link_vector(control_acceleration_filter_.update( + base_link_to_odom_vector(constrained_chassis_acceleration))); + + auto steering_torques = calculate_steering_control_torques( + steering_status, chassis_status_expected, filtered_chassis_acceleration); + auto wheel_torques = calculate_wheel_control_torques( + steering_status, filtered_chassis_acceleration, wheel_pid_torques); + + update_control_torques(steering_torques, wheel_torques); + *steering_wheel_actual_power_estimate_ = + calculate_actual_power_estimate(wheel_velocities, wheel_torques); + update_chassis_velocity_expected(filtered_chassis_acceleration); + } + +private: + struct SteeringStatus { + Eigen::Vector4d angle, cos_angle, sin_angle; + Eigen::Vector4d velocity; + }; + + struct ChassisStatus { + Eigen::Vector3d velocity; + Eigen::Vector4d wheel_velocity_x, wheel_velocity_y; + }; + + void reset_all_controls() { + control_acceleration_filter_.reset(); + + chassis_yaw_angle_imu_ = 0.0; + chassis_velocity_expected_ = Eigen::Vector3d::Zero(); + + *left_front_steering_control_torque_ = 0.0; + *left_back_steering_control_torque_ = 0.0; + *right_back_steering_control_torque_ = 0.0; + *right_front_steering_control_torque_ = 0.0; + + *left_front_wheel_control_torque_ = 0.0; + *left_back_wheel_control_torque_ = 0.0; + *right_back_wheel_control_torque_ = 0.0; + *right_front_wheel_control_torque_ = 0.0; + *steering_wheel_actual_power_estimate_ = 0.0; + } + + void integral_yaw_angle_imu() { + chassis_yaw_angle_imu_ += *chassis_yaw_velocity_imu_ * dt_; + chassis_yaw_angle_imu_ = std::fmod(chassis_yaw_angle_imu_, 2 * std::numbers::pi); + } + + SteeringStatus calculate_steering_status() { + 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(); + + steering_status.velocity = { + *left_front_steering_velocity_, // + *left_back_steering_velocity_, // + *right_back_steering_velocity_, // + *right_front_steering_velocity_ // + }; + + return steering_status; + } + + Eigen::Vector4d calculate_wheel_velocities() { + 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; + } + + ChassisStatus calculate_chassis_status_expected(const Eigen::Vector3d& chassis_velocity) { + auto calculate_energy = [this](const Eigen::Vector3d& velocity) { + return mess_ * velocity.head<2>().squaredNorm() + + moment_of_inertia_ * velocity.z() * velocity.z(); + }; + auto chassis_energy = calculate_energy(chassis_velocity); + auto chassis_energy_expected = calculate_energy(chassis_velocity_expected_); + if (chassis_energy_expected > chassis_energy) { + double k = std::sqrt(chassis_energy / chassis_energy_expected); + chassis_velocity_expected_ *= k; + } + + ChassisStatus chassis_status_expected; + chassis_status_expected.velocity = odom_to_base_link_vector(chassis_velocity_expected_); + + const auto& [vx, vy, vz] = chassis_status_expected.velocity; + chassis_status_expected.wheel_velocity_x = vx - vehicle_radius_ * vz * sin_varphi_.array(); + chassis_status_expected.wheel_velocity_y = vy + vehicle_radius_ * vz * cos_varphi_.array(); + + return chassis_status_expected; + } + + Eigen::Vector3d calculate_chassis_control_velocity() { + Eigen::Vector3d chassis_control_velocity = chassis_control_velocity_->vector; + chassis_control_velocity.head<2>() = + Eigen::Rotation2Dd(-std::numbers::pi / 4) * chassis_control_velocity.head<2>(); + + return chassis_control_velocity; + } + + Eigen::Vector3d calculate_chassis_control_acceleration( + const Eigen::Vector3d& chassis_velocity_expected, + const Eigen::Vector3d& chassis_control_velocity) { + + Eigen::Vector2d translational_control_velocity = chassis_control_velocity.head<2>(); + Eigen::Vector2d translational_velocity = chassis_velocity_expected.head<2>(); + Eigen::Vector2d translational_control_acceleration = + chassis_translational_velocity_pid_.update( + translational_control_velocity - translational_velocity); + + const double& angular_control_velocity = chassis_control_velocity[2]; + const double& angular_velocity = chassis_velocity_expected[2]; + double angular_control_acceleration = + chassis_angular_velocity_pid_.update(angular_control_velocity - angular_velocity); + + Eigen::Vector3d chassis_control_acceleration; + chassis_control_acceleration << translational_control_acceleration, + angular_control_acceleration; + + if (chassis_control_acceleration.lpNorm<1>() < 1e-1) + chassis_control_acceleration.setZero(); + + return chassis_control_acceleration; + } + + Eigen::Vector4d calculate_wheel_pid_torques( + const SteeringStatus& steering_status, const Eigen::Vector4d& wheel_velocities, + const ChassisStatus& chassis_status_expected) { + Eigen::Vector4d wheel_control_velocity = + chassis_status_expected.wheel_velocity_x.array() * steering_status.cos_angle.array() + + chassis_status_expected.wheel_velocity_y.array() * steering_status.sin_angle.array(); + return wheel_velocity_pid_.update( + wheel_control_velocity / wheel_radius_ - wheel_velocities); + } + + Eigen::Vector3d constrain_chassis_control_acceleration( + const SteeringStatus& steering_status, const Eigen::Vector4d& wheel_velocities, + const Eigen::Vector3d& chassis_acceleration, const Eigen::Vector4d& wheel_pid_torques, + const double& power_limit) { + + Eigen::Vector2d translational_acceleration_direction = chassis_acceleration.head<2>(); + double translational_acceleration_max = translational_acceleration_direction.norm(); + if (translational_acceleration_max > 0.0) + translational_acceleration_direction /= translational_acceleration_max; + + double angular_acceleration_max = chassis_acceleration.z(); + double angular_acceleration_direction = angular_acceleration_max > 0 ? 1.0 : -1.0; + angular_acceleration_max *= angular_acceleration_direction; + + const double rhombus_right = friction_coefficient_ * g_; + const double rhombus_top = rhombus_right * mess_ * vehicle_radius_ / moment_of_inertia_; + + auto [a, b, c, d, e, f] = calculate_ellipse_parameters( + steering_status, wheel_velocities, translational_acceleration_direction, + angular_acceleration_direction, wheel_pid_torques); + + Eigen::Vector2d best_point = qcp_solver_.solve( + {1.0, 0.2}, {translational_acceleration_max, angular_acceleration_max}, + {rhombus_right, rhombus_top}, {a, b, c, d, e, f - power_limit}); + + Eigen::Vector3d best_acceleration; + best_acceleration << best_point.x() * translational_acceleration_direction, + best_point.y() * angular_acceleration_direction; + return best_acceleration; + } + + Eigen::Vector calculate_ellipse_parameters( + const SteeringStatus& steering_status, const Eigen::Vector4d& wheel_velocities, + const Eigen::Vector2d& translational_acceleration_direction, + const double& angular_acceleration_direction, const Eigen::Vector4d& wheel_torque_base) { + Eigen::Vector4d cos_alpha_minus_gamma = + steering_status.cos_angle.array() * translational_acceleration_direction.x() + + steering_status.sin_angle.array() * translational_acceleration_direction.y(); + Eigen::Vector4d sin_alpha_minus_varphi = + cos_varphi_.array() * steering_status.sin_angle.array() + - sin_varphi_.array() * steering_status.cos_angle.array(); + Eigen::Vector4d double_k1_torque_base_plus_wheel_velocities = + 2 * k1_ * wheel_torque_base.array() + wheel_velocities.array(); + + Eigen::Vector formula; + auto& [a, b, c, d, e, f] = formula; + + a = (k1_ * mess_ * mess_ * wheel_radius_ * wheel_radius_ / 16.0) + * cos_alpha_minus_gamma.array().pow(2).sum(); + b = ((k1_ * mess_ * moment_of_inertia_ * wheel_radius_ * wheel_radius_) + / (8.0 * vehicle_radius_)) + * angular_acceleration_direction + * (cos_alpha_minus_gamma.array() * sin_alpha_minus_varphi.array()).sum(); + c = ((k1_ * moment_of_inertia_ * moment_of_inertia_ * wheel_radius_ * wheel_radius_) + / (16.0 * vehicle_radius_ * vehicle_radius_)) + * sin_alpha_minus_varphi.array().pow(2).sum(); + d = (mess_ * wheel_radius_ / 4.0) + * (double_k1_torque_base_plus_wheel_velocities.array() * cos_alpha_minus_gamma.array()) + .sum(); + e = ((moment_of_inertia_ * wheel_radius_) / (4.0 * vehicle_radius_)) + * angular_acceleration_direction + * (double_k1_torque_base_plus_wheel_velocities.array() * sin_alpha_minus_varphi.array()) + .sum(); + f = (wheel_torque_base.array() + * (k1_ * wheel_torque_base.array() + wheel_velocities.array())) + .sum(); + + return formula; + } + + Eigen::Vector4d calculate_steering_control_torques( + const SteeringStatus& steering_status, const ChassisStatus& chassis_status_expected, + const Eigen::Vector3d& chassis_acceleration) { + + const auto& [vx, vy, vz] = chassis_status_expected.velocity; + const auto& [ax, ay, az] = chassis_acceleration; + + Eigen::Vector4d dot_r_squared = chassis_status_expected.wheel_velocity_x.array().square() + + chassis_status_expected.wheel_velocity_y.array().square(); + + Eigen::Vector4d steering_control_velocities = + vx * ay - vy * ax - vz * (vx * vx + vy * vy) + + vehicle_radius_ * (az * vx - vz * (ax + vz * vy)) * cos_varphi_.array() + + vehicle_radius_ * (az * vy - vz * (ay - vz * vx)) * sin_varphi_.array(); + Eigen::Vector4d steering_control_angles; + + for (int i = 0; i < steering_control_velocities.size(); ++i) { + if (dot_r_squared[i] > 1e-2) { + steering_control_velocities[i] /= dot_r_squared[i]; + steering_control_angles[i] = std::atan2( + chassis_status_expected.wheel_velocity_y[i], + chassis_status_expected.wheel_velocity_x[i]); + } else { + auto x = ax - vehicle_radius_ * (az * sin_varphi_[i] + 0 * cos_varphi_[i]); + auto y = ay + vehicle_radius_ * (az * cos_varphi_[i] - 0 * sin_varphi_[i]); + if (x * x + y * y > 1e-6) { + steering_control_velocities[i] = 0.0; + steering_control_angles[i] = std::atan2(y, x); + } else { + steering_control_velocities[i] = nan_; + steering_control_angles[i] = nan_; + } + } + } + + Eigen::Vector4d steering_torques = steering_velocity_pid_.update( + steering_control_velocities + + steering_angle_pid_.update( + (steering_control_angles - steering_status.angle).unaryExpr([](double diff) { + diff = std::fmod(diff, std::numbers::pi); + if (diff < -std::numbers::pi / 2) { + diff += std::numbers::pi; + } else if (diff > std::numbers::pi / 2) { + diff -= std::numbers::pi; + } + return diff; + })) + - steering_status.velocity); + return steering_torques.unaryExpr([](double v) { return std::isnan(v) ? 0.0 : v; }); + } + + Eigen::Vector4d calculate_wheel_control_torques( + const SteeringStatus& steering_status, const Eigen::Vector3d& chassis_acceleration, + const Eigen::Vector4d& wheel_pid_torques) { + + const auto& [ax, ay, az] = chassis_acceleration; + Eigen::Vector4d wheel_torques = + wheel_radius_ + * (ax * mess_ * steering_status.cos_angle.array() + + ay * mess_ * steering_status.sin_angle.array() + + az * moment_of_inertia_ + * (cos_varphi_.array() * steering_status.sin_angle.array() + - sin_varphi_.array() * steering_status.cos_angle.array()) + / vehicle_radius_) + / 4.0; + + wheel_torques += wheel_pid_torques; + + return wheel_torques; + } + + double calculate_actual_power_estimate( + const Eigen::Vector4d& wheel_velocities, const Eigen::Vector4d& wheel_torques) const { + return no_load_power_ + k2_ * wheel_velocities.array().pow(2).sum() + + (wheel_torques.array() * (k1_ * wheel_torques.array() + wheel_velocities.array())) + .sum(); + } + + void update_control_torques( + const Eigen::Vector4d& steering_torques, const Eigen::Vector4d& wheel_torques) { + *left_front_steering_control_torque_ = steering_torques[0]; + *left_back_steering_control_torque_ = steering_torques[1]; + *right_back_steering_control_torque_ = steering_torques[2]; + *right_front_steering_control_torque_ = steering_torques[3]; + + *left_front_wheel_control_torque_ = wheel_torques[0]; + *left_back_wheel_control_torque_ = wheel_torques[1]; + *right_back_wheel_control_torque_ = wheel_torques[2]; + *right_front_wheel_control_torque_ = wheel_torques[3]; + } + + void update_chassis_velocity_expected(const Eigen::Vector3d& chassis_acceleration) { + auto acceleration_odom = base_link_to_odom_vector(chassis_acceleration); + chassis_velocity_expected_ += dt_ * acceleration_odom; + } + + Eigen::Vector3d base_link_to_odom_vector(Eigen::Vector3d vector) const { + vector.head<2>() = Eigen::Rotation2Dd(chassis_yaw_angle_imu_) * vector.head<2>(); + return vector; + } + + Eigen::Vector3d odom_to_base_link_vector(Eigen::Vector3d vector) const { + vector.head<2>() = Eigen::Rotation2Dd(-chassis_yaw_angle_imu_) * vector.head<2>(); + return vector; + } + + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + static constexpr double inf_ = std::numeric_limits::infinity(); + + static constexpr double dt_ = 1e-3; + static constexpr double g_ = 9.81; + + const double mess_; + const double moment_of_inertia_; + const double vehicle_radius_; + const double wheel_radius_; + const double friction_coefficient_; + + const double k1_, k2_, no_load_power_; + + InputInterface joystick_right_; + InputInterface joystick_left_; + + InputInterface left_front_steering_angle_; + InputInterface left_back_steering_angle_; + InputInterface right_back_steering_angle_; + InputInterface right_front_steering_angle_; + + InputInterface left_front_steering_velocity_; + InputInterface left_back_steering_velocity_; + InputInterface right_back_steering_velocity_; + InputInterface right_front_steering_velocity_; + + InputInterface left_front_wheel_velocity_; + InputInterface left_back_wheel_velocity_; + InputInterface right_back_wheel_velocity_; + InputInterface right_front_wheel_velocity_; + + InputInterface chassis_yaw_velocity_imu_; + InputInterface chassis_control_velocity_; + InputInterface power_limit_; + + OutputInterface left_front_steering_control_torque_; + OutputInterface left_back_steering_control_torque_; + OutputInterface right_back_steering_control_torque_; + OutputInterface right_front_steering_control_torque_; + + OutputInterface left_front_wheel_control_torque_; + OutputInterface left_back_wheel_control_torque_; + OutputInterface right_back_wheel_control_torque_; + OutputInterface right_front_wheel_control_torque_; + OutputInterface steering_wheel_actual_power_estimate_; + + QcpSolver qcp_solver_; + filter::LowPassFilter<3> control_acceleration_filter_; + + double chassis_yaw_angle_imu_ = 0.0; + Eigen::Vector3d chassis_velocity_expected_ = Eigen::Vector3d::Zero(); + + pid::MatrixPidCalculator<2> chassis_translational_velocity_pid_; + pid::PidCalculator chassis_angular_velocity_pid_; + + const Eigen::Vector4d cos_varphi_, sin_varphi_; + + pid::MatrixPidCalculator<4> steering_velocity_pid_, steering_angle_pid_, wheel_velocity_pid_; +}; + +} // namespace rmcs_core::controller::chassis + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::HeroSteeringWheelController, rmcs_executor::Component) 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 000000000..8450b3863 --- /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) 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 739b58592..0d49184e8 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 @@ -1,9 +1,13 @@ +#include +#include #include #include +#include #include #include +#include #include "controller/pid/pid_calculator.hpp" @@ -39,14 +43,17 @@ class DualYawController register_input("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); register_input("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_); + register_input("/gimbal/mode", gimbal_mode_); register_input("/gimbal/yaw/control_angle_error", control_angle_error_); register_input("/gimbal/yaw/control_angle_shift", control_angle_shift_, false); 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); + 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_); + register_output("/gimbal/top_yaw/control_angle_shift", top_yaw_control_angle_shift_, nan_); + register_output("/gimbal/bottom_yaw/control_angle", bottom_yaw_control_angle_, nan_); status_component_ = create_partner_component(get_component_name() + "_status"); @@ -61,6 +68,31 @@ class DualYawController } void update() override { + + const auto mode = *gimbal_mode_; + if (mode == rmcs_msgs::GimbalMode::ENCODER) { + const bool entering_encoder = last_gimbal_mode_ != rmcs_msgs::GimbalMode::ENCODER; + if (entering_encoder) { + if (std::isfinite(*top_yaw_angle_)) { + top_yaw_encoder_locked_ = true; + } else { + top_yaw_encoder_locked_ = false; + } + } + *bottom_yaw_control_angle_shift_ = *control_angle_shift_; + if (top_yaw_encoder_locked_) + *top_yaw_control_angle_ = top_yaw_encoder_angle_; + else + *top_yaw_control_angle_ = nan_; + } else { + *top_yaw_control_angle_ = nan_; + *bottom_yaw_control_angle_shift_ = nan_; + top_yaw_encoder_angle_ = *top_yaw_angle_; + top_yaw_encoder_locked_ = false; + } + + last_gimbal_mode_ = mode; + if (std::isnan(*control_angle_error_)) { *top_yaw_control_torque_ = nan_; *bottom_yaw_control_torque_ = nan_; @@ -72,23 +104,30 @@ class DualYawController bottom_yaw_angle_pid_.update(bottom_yaw_control_error()) - bottom_yaw_velocity_imu()); } - - if (std::isnan(*control_angle_shift_)) { - *top_yaw_control_angle_ = nan_; - *bottom_yaw_control_angle_shift_ = nan_; - } else { - *top_yaw_control_angle_ = 0.0; - *bottom_yaw_control_angle_shift_ = *control_angle_shift_; - } } private: static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + double wrap_angle(double angle) { + while (angle > 0) + angle += 2 * std::numbers::pi; + while (angle >= 2 * std::numbers::pi) + angle -= 2 * std::numbers::pi; + return angle; + } + double bottom_yaw_control_error() { - double err = *top_yaw_angle_ + *control_angle_error_; - if (err > std::numbers::pi) - err -= 2 * std::numbers::pi; + if (!std::isfinite(*top_yaw_angle_) || !std::isfinite(*control_angle_error_)) + return nan_; + + // Avoid relying on top_yaw_angle in [0, 2pi) and control_angle_error in [-pi, pi]. + constexpr double alignment = 2 * std::numbers::pi; + double err = + std::fmod(*top_yaw_angle_ + *control_angle_error_ + std::numbers::pi, alignment); + if (err < 0) + err += alignment; + err -= std::numbers::pi; return err; } @@ -99,6 +138,7 @@ class DualYawController InputInterface gimbal_yaw_velocity_imu_, chassis_yaw_velocity_imu_; + InputInterface gimbal_mode_; InputInterface control_angle_error_, control_angle_shift_; pid::PidCalculator top_yaw_angle_pid_, top_yaw_velocity_pid_; @@ -110,6 +150,13 @@ class DualYawController OutputInterface top_yaw_control_angle_; OutputInterface bottom_yaw_control_angle_shift_; + OutputInterface bottom_yaw_control_angle_; + OutputInterface top_yaw_control_angle_shift_; + + rmcs_msgs::GimbalMode last_gimbal_mode_ = rmcs_msgs::GimbalMode::IMU; + bool top_yaw_encoder_locked_ = false; + double top_yaw_encoder_angle_ = nan_; + class DualYawStatus : public rmcs_executor::Component { public: explicit DualYawStatus() { 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 f20bd98fe..939f17f6d 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 @@ -1,5 +1,7 @@ +#include #include +#include #include #include #include @@ -23,12 +25,10 @@ class HeroGimbalController : Node( get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) - , imu_gimbal_solver( - *this, get_parameter("upper_limit").as_double(), - get_parameter("lower_limit").as_double()) - , encoder_gimbal_solver( - *this, get_parameter("upper_limit").as_double(), - get_parameter("lower_limit").as_double()) { + , upper_limit_(get_parameter("upper_limit").as_double()) + , lower_limit_(get_parameter("lower_limit").as_double()) + , imu_gimbal_solver(*this, upper_limit_, lower_limit_) + , encoder_gimbal_solver(*this, upper_limit_, lower_limit_) { register_input("/remote/joystick/left", joystick_left_); register_input("/remote/switch/left", switch_left_); @@ -38,6 +38,8 @@ class HeroGimbalController register_input("/remote/keyboard", keyboard_); register_input("/gimbal/auto_aim/control_direction", auto_aim_control_direction_, false); + register_input("/gimbal/pitch/angle", gimbal_pitch_angle_); + register_input("/gimbal/pitch/raw_angle", gimbal_pitch_raw_angle_); register_output("/gimbal/mode", gimbal_mode_, rmcs_msgs::GimbalMode::IMU); @@ -59,14 +61,17 @@ class HeroGimbalController break; } - if (!last_keyboard_.q && keyboard_->q) { - if (gimbal_mode_keyboard_ == GimbalMode::IMU) + if (!last_keyboard_.e && keyboard_->e) { + if (gimbal_mode_keyboard_ == GimbalMode::IMU) { + encoder_init_pitch_ = keyboard_->ctrl ? kCtrlEInitPitch : kEInitPitch; gimbal_mode_keyboard_ = GimbalMode::ENCODER; - else + } 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 : GimbalMode::IMU; if (*gimbal_mode_ == GimbalMode::IMU) { auto angle_error = update_imu_control(); @@ -76,6 +81,7 @@ class HeroGimbalController encoder_gimbal_solver.update(PreciseTwoAxisGimbalSolver::SetDisabled{}); *yaw_control_angle_shift_ = nan_; *pitch_control_angle_ = nan_; + } else { imu_gimbal_solver.update(TwoAxisGimbalSolver::SetDisabled{}); *yaw_angle_error_ = nan_; @@ -121,24 +127,26 @@ 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}); } PreciseTwoAxisGimbalSolver::ControlAngle update_encoder_control() { - if (!encoder_gimbal_solver.enabled()) - return encoder_gimbal_solver.update(PreciseTwoAxisGimbalSolver::SetControlPitch{-0.31}); + if (!encoder_gimbal_solver.enabled()) { + return encoder_gimbal_solver.update( + PreciseTwoAxisGimbalSolver::SetControlPitch{encoder_init_pitch_}); + } - constexpr double joystick_sensitivity = 0.006 * 0.1; constexpr double mouse_yaw_sensitivity = 0.5 * 0.114; constexpr double mouse_pitch_sensitivity = 0.5 * 0.095; + constexpr double joystick_sensitivity = 0.006 * 0.05; 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}); @@ -147,6 +155,9 @@ class HeroGimbalController private: static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + static constexpr double kEInitPitch = -0.476972; // Initial angle for standalone E. + static constexpr double kCtrlEInitPitch = -0.541591; // Initial angle for Ctrl+E. + double encoder_init_pitch_ = kEInitPitch; InputInterface joystick_left_; InputInterface switch_right_; InputInterface switch_left_; @@ -157,10 +168,13 @@ class HeroGimbalController rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); InputInterface auto_aim_control_direction_; + InputInterface gimbal_pitch_angle_; + InputInterface gimbal_pitch_raw_angle_; rmcs_msgs::GimbalMode gimbal_mode_keyboard_ = rmcs_msgs::GimbalMode::IMU; OutputInterface gimbal_mode_; + const double upper_limit_, lower_limit_; TwoAxisGimbalSolver imu_gimbal_solver; PreciseTwoAxisGimbalSolver encoder_gimbal_solver; @@ -173,4 +187,4 @@ class HeroGimbalController #include PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::gimbal::HeroGimbalController, rmcs_executor::Component) \ No newline at end of file + rmcs_core::controller::gimbal::HeroGimbalController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/player_viewer.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/player_viewer.cpp index 2e8ba4f7c..d4d98715f 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/player_viewer.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/player_viewer.cpp @@ -27,7 +27,8 @@ class PlayerViewer register_input("/remote/mouse/mouse_wheel", mouse_wheel_); register_input("/remote/keyboard", keyboard_); - register_input("/gimbal/pitch/angle", gimbal_pitch_angle_); + register_input("/gimbal/player_viewer/angle", gimbal_pitch_angle_); + register_input("/gimbal/player_viewer/raw_angle", gimbal_pitch_raw_angle_); register_input("/gimbal/player_viewer/angle", gimbal_player_viewer_angle_); register_output( @@ -53,13 +54,16 @@ class PlayerViewer || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { reset_all_controls(); } else { - if (!last_keyboard_.e && keyboard.e) - viewer_reset_ = true; if (!last_keyboard_.q && keyboard.q) { scope_active_ = !scope_active_; *is_scope_active_ = scope_active_; scope_viewer_reset_ = scope_active_; } + if (!last_keyboard_.e && keyboard.e) { + viewer_init_angle_ = keyboard.ctrl ? kCtrlInitViewerAngle : kEInitViewerAngle; + viewer_reset_ = true; + scope_viewer_reset_ = false; + } update_viewer_control(); }; @@ -92,7 +96,7 @@ class PlayerViewer *viewer_delta_angle_by_mouse_wheel_ = 0.5 * *mouse_wheel_ * unit_sensitivity(0.09); if (viewer_reset_) { - *viewer_control_angle_ = upper_limit_; + *viewer_control_angle_ = viewer_init_angle_; viewer_reset_ = false; } else { if (scope_viewer_reset_) { @@ -115,9 +119,9 @@ class PlayerViewer *viewer_control_angle_ - norm_angle(*gimbal_player_viewer_angle_); if (scope_active_) { - *scope_control_torque_ = 0.2; + *scope_control_torque_ = 0.13; } else { - *scope_control_torque_ = -0.2; + *scope_control_torque_ = -0.13; } } @@ -125,9 +129,14 @@ class PlayerViewer static constexpr double nan_ = std::numeric_limits::quiet_NaN(); static constexpr double pi_ = std::numbers::pi; + // The steering-hero viewer angle limit range is [0.68, 1.17]. + static constexpr double kEInitViewerAngle = 0.38905; // Move here when E is pressed. + static constexpr double kCtrlInitViewerAngle = 0.38905; // Move here when Ctrl is pressed. + bool scope_viewer_reset_{false}; const double upper_limit_, lower_limit_; + double viewer_init_angle_ = kEInitViewerAngle; InputInterface switch_right_; InputInterface switch_left_; @@ -135,6 +144,7 @@ class PlayerViewer InputInterface mouse_wheel_; InputInterface gimbal_pitch_angle_; + InputInterface gimbal_pitch_raw_angle_; InputInterface gimbal_player_viewer_angle_; OutputInterface scope_control_torque_; @@ -155,4 +165,4 @@ class PlayerViewer #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::gimbal::PlayerViewer, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::gimbal::PlayerViewer, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/precise_two_axis_gimbal_solver.hpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/precise_two_axis_gimbal_solver.hpp index 9a5c29480..daa13997b 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/precise_two_axis_gimbal_solver.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/precise_two_axis_gimbal_solver.hpp @@ -25,8 +25,48 @@ class PreciseTwoAxisGimbalSolver { : upper_limit_(upper_limit) , lower_limit_(lower_limit) { component.register_input("/gimbal/pitch/angle", gimbal_pitch_angle_); + component.register_input("/tf", tf_); } + struct SetControlDirection : Operation { + friend class PreciseTwoAxisGimbalSolver; + + explicit SetControlDirection(OdomImu::DirectionVector target) + : target_(std::move(target)) {} + + private: + double update(PreciseTwoAxisGimbalSolver& super) const { + if (!super.tf_.ready()) { + super.control_pitch_angle_ = PreciseTwoAxisGimbalSolver::nan_; + return PreciseTwoAxisGimbalSolver::nan_; + } + + auto dir_yaw_link = fast_tf::cast(target_, *super.tf_); + Eigen::Vector3d dir = *dir_yaw_link; + + const double norm = dir.norm(); + if (norm < kEps) { + super.control_pitch_angle_ = PreciseTwoAxisGimbalSolver::nan_; + return PreciseTwoAxisGimbalSolver::nan_; + } + dir /= norm; + + const double xy_norm = std::hypot(dir.x(), dir.y()); + + // In YawLink, yaw is the increment relative to the current total yaw. + const double yaw_shift = std::atan2(dir.y(), dir.x()); + + // Mechanical pitch convention: level is 0, pitching downward is positive, so negate it. + const double pitch_angle = -std::atan2(dir.z(), std::max(xy_norm, kEps)); + + super.set_pitch_angle(pitch_angle); + return yaw_shift; + } + + static constexpr double kEps = 1e-9; + OdomImu::DirectionVector target_; + }; + struct SetDisabled : Operation { friend class PreciseTwoAxisGimbalSolver; @@ -91,10 +131,11 @@ class PreciseTwoAxisGimbalSolver { static constexpr double nan_ = std::numeric_limits::quiet_NaN(); rmcs_executor::Component::InputInterface gimbal_pitch_angle_; + rmcs_executor::Component::InputInterface tf_; const double upper_limit_, lower_limit_; double control_pitch_angle_ = nan_; }; -} // namespace rmcs_core::controller::gimbal \ No newline at end of file +} // namespace rmcs_core::controller::gimbal 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 000000000..366f66b62 --- /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/bullet_feeder_controller_42mm.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_42mm.cpp index fb0e433d1..d8bd95cb6 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,6 +31,7 @@ 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; @@ -38,7 +39,7 @@ class BulletFeederController42mm 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; @@ -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_; @@ -100,12 +100,10 @@ class BulletFeederController42mm } } - double err_abs = std::abs(bullet_feeder_control_angle_ - *bullet_feeder_angle_); - // RCLCPP_INFO( - // get_logger(), "%.2f %.2f %.2f", bullet_feeder_control_angle_, - // *bullet_feeder_angle_, err_abs); + double angle_error_abs = + std::abs(bullet_feeder_control_angle_ - *bullet_feeder_angle_); if (shoot_stage_ == ShootStage::PRELOADING) { - if (err_abs < 0.1) + if (angle_error_abs < 0.1) set_preloaded(); } if (shoot_stage_ == ShootStage::PRELOADED) { @@ -113,23 +111,19 @@ class BulletFeederController42mm set_compressing(); } if (shoot_stage_ == ShootStage::COMPRESSING) { - if (err_abs < 0.1) + if (angle_error_abs < 0.1) set_compressed(); } if (shoot_stage_ == ShootStage::SHOOTING) { - if (err_abs < 0.1) + if (angle_error_abs < 0.1) set_preloading(); } } - double velocity_err = bullet_feeder_angle_pid_.update( - bullet_feeder_control_angle_ - *bullet_feeder_angle_) - - *bullet_feeder_velocity_; - // bullet_feeder_velocity_pid_.integral_max = std::clamp(1000.0 * velocity_err, - // 0.0, 60.0); - *bullet_feeder_control_torque_ = bullet_feeder_velocity_pid_.update(velocity_err); - // RCLCPP_INFO( - // get_logger(), "%.2f %.2f", velocity_err, *bullet_feeder_control_torque_); + double velocity_error = 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_error); update_jam_detection(); } @@ -199,7 +193,6 @@ class BulletFeederController42mm } void update_jam_detection() { - // RCLCPP_INFO(get_logger(), "%.2f --", *bullet_feeder_control_torque_); if (*bullet_feeder_control_torque_ < 300.0) { bullet_feeder_faulty_count_ = 0; return; @@ -228,6 +221,7 @@ class BulletFeederController42mm 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_; @@ -266,4 +260,4 @@ class BulletFeederController42mm #include PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::shooting::BulletFeederController42mm, rmcs_executor::Component) \ No newline at end of file + rmcs_core::controller::shooting::BulletFeederController42mm, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/hero_friction_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/hero_friction_wheel_controller.cpp new file mode 100644 index 000000000..9f4e62f01 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/hero_friction_wheel_controller.cpp @@ -0,0 +1,263 @@ +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::controller::shooting { + +class HeroFrictionWheelController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + HeroFrictionWheelController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , logger_(get_logger()) { + + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + register_input("/remote/keyboard", keyboard_); + + auto friction_wheels = get_parameter("friction_wheels").as_string_array(); + auto friction_profile_0 = get_parameter("friction_velocities_profile_0").as_double_array(); + auto friction_profile_1 = get_parameter("friction_velocities_profile_1").as_double_array(); + if (friction_wheels.size() != friction_profile_0.size() + || friction_wheels.size() != friction_profile_1.size()) { + throw std::runtime_error( + "'friction_wheels' and both friction velocity profiles must have the same length!"); + } else if (friction_wheels.size() == 0) + throw std::runtime_error( + "Empty array error: 'friction_wheels' and 'friction_velocities' cannot be empty!"); + + friction_profile_0_.assign(friction_profile_0.begin(), friction_profile_0.end()); + friction_profile_1_.assign(friction_profile_1.begin(), friction_profile_1.end()); + friction_count_ = friction_wheels.size(); + friction_velocities_ = std::make_unique[]>(friction_count_); + friction_control_velocities_ = std::make_unique[]>(friction_count_); + for (size_t i = 0; i < friction_count_; i++) { + register_input(friction_wheels[i] + "/velocity", friction_velocities_[i]); + register_output( + friction_wheels[i] + "/control_velocity", friction_control_velocities_[i], nan_); + } + + friction_soft_start_stop_step_ = + (1 / 1000.0) / get_parameter("friction_soft_start_stop_time").as_double(); + + register_output("/gimbal/friction_ready", friction_ready_, false); + register_output("/gimbal/friction_jammed", friction_jammed_, false); + register_output("/gimbal/bullet_fired", bullet_fired_, false); + register_output("/gimbal/friction_profile_1_active", friction_profile_1_active_, false); + } + + void update() override { + const auto switch_right = *switch_right_; + const auto switch_left = *switch_left_; + const auto keyboard = *keyboard_; + if (!last_keyboard_.f && keyboard.f) { + active_profile_ ^= 1; + + if (!std::isnan(friction_soft_start_stop_percentage_)) { + double sum = 0.0; + for (size_t i = 0; i < friction_count_; i++) + sum += *friction_velocities_[i] / target_friction_velocity(i); + friction_soft_start_stop_percentage_ = + std::clamp(sum / static_cast(friction_count_), 0.0, 1.0); + } + } + const bool profile_switch_now = keyboard.ctrl && keyboard.f; + const bool profile_switch_last = last_keyboard_.ctrl && last_keyboard_.f; + + if (!profile_switch_last && profile_switch_now) { + active_profile_ ^= 1; + + if (!std::isnan(friction_soft_start_stop_percentage_)) { + double sum = 0.0; + for (size_t i = 0; i < friction_count_; i++) + sum += *friction_velocities_[i] / target_friction_velocity(i); + friction_soft_start_stop_percentage_ = + std::clamp(sum / static_cast(friction_count_), 0.0, 1.0); + } + } + + *friction_profile_1_active_ = active_profile_ == 1; + 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 (switch_right != Switch::DOWN) { + if ((!last_keyboard_.v && keyboard.v) + || (last_switch_left_ == Switch::MIDDLE && switch_left == Switch::UP)) { + friction_enabled_ = !friction_enabled_; + } + + update_friction_velocities(); + update_friction_status(); + if (*friction_jammed_) + RCLCPP_INFO(logger_, "Friction Jammed!"); + if (*bullet_fired_) + RCLCPP_INFO(logger_, "Bullet Fired!"); + + last_switch_right_ = switch_right; + last_switch_left_ = switch_left; + last_keyboard_ = keyboard; + } + if (!friction_enabled_) { + reset_all_controls(); + } + } + +private: + void reset_all_controls() { + friction_enabled_ = false; + + last_primary_friction_velocity_ = nan_; + primary_friction_velocity_decrease_integral_ = 0; + + friction_soft_start_stop_percentage_ = nan_; + + for (size_t i = 0; i < friction_count_; i++) + *friction_control_velocities_[i] = nan_; + + *friction_ready_ = *friction_jammed_ = *bullet_fired_ = false; + } + + void update_friction_velocities() { + if (std::isnan(friction_soft_start_stop_percentage_)) { + friction_soft_start_stop_percentage_ = 0.0; + for (size_t i = 0; i < friction_count_; i++) + friction_soft_start_stop_percentage_ += + *friction_velocities_[i] / target_friction_velocity(i); + friction_soft_start_stop_percentage_ /= static_cast(friction_count_); + } + friction_soft_start_stop_percentage_ += + friction_enabled_ ? friction_soft_start_stop_step_ : -friction_soft_start_stop_step_; + friction_soft_start_stop_percentage_ = + std::clamp(friction_soft_start_stop_percentage_, 0.0, 1.0); + + for (size_t i = 0; i < friction_count_; i++) + *friction_control_velocities_[i] = + friction_soft_start_stop_percentage_ * target_friction_velocity(i); + } + + void update_friction_status() { + *friction_ready_ = *friction_jammed_ = *bullet_fired_ = false; + + if (!friction_enabled_) + return; + if (friction_soft_start_stop_percentage_ < 1.0) + return; + + if (detect_friction_faulty()) { + if (friction_faulty_count_ == 200) { + friction_enabled_ = false; + *friction_jammed_ = true; + } else { + friction_faulty_count_++; + *friction_ready_ = true; + } + return; + } + + *friction_ready_ = true; + *bullet_fired_ = detect_bullet_fire(); + } + + bool detect_friction_faulty() { + for (size_t i = 0; i < friction_count_; i++) { + if (abs(*friction_velocities_[i]) < abs(*friction_control_velocities_[i] * 0.5)) + return true; + } + return false; + } + + rmcs_utility::FpsCounter fps_counter_; + + bool detect_bullet_fire() { + bool fired = false; + + // TODO(steering-hero): This intentionally keeps the legacy merge behavior by monitoring + // friction_velocities_[2]. Historically, hero config ordering mapped [2,3] to the first + // stage used for fire detection. Replace this hard-coded index with an explicit first- + // stage mapping once the wheel ordering semantics are unified. + if (!std::isnan(last_primary_friction_velocity_)) { + double differential = *friction_velocities_[2] - last_primary_friction_velocity_; + if (differential < 0.1) + primary_friction_velocity_decrease_integral_ += differential; + else { + if (primary_friction_velocity_decrease_integral_ < -14.0 + && last_primary_friction_velocity_ < target_friction_velocity(2) - 25.0) + fired = true; + + primary_friction_velocity_decrease_integral_ = 0; + } + } + last_primary_friction_velocity_ = *friction_velocities_[2]; + + return fired; + } + + double target_friction_velocity(size_t i) const { + return active_profile_ == 0 ? friction_profile_0_[i] : friction_profile_1_[i]; + } + + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + + rclcpp::Logger logger_; + + InputInterface switch_right_; + InputInterface switch_left_; + InputInterface keyboard_; + + 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(); + + size_t friction_count_; + + std::vector friction_profile_0_; + std::vector friction_profile_1_; + size_t active_profile_ = 0; + + std::unique_ptr[]> friction_velocities_; + + bool friction_enabled_ = false; + + double friction_soft_start_stop_step_; + double friction_soft_start_stop_percentage_ = nan_; + std::unique_ptr[]> friction_control_velocities_; + + OutputInterface friction_ready_; + + int friction_faulty_count_ = 0; + OutputInterface friction_jammed_; + + double last_primary_friction_velocity_ = nan_; + double primary_friction_velocity_decrease_integral_ = 0; + OutputInterface bullet_fired_; + OutputInterface friction_profile_1_active_; +}; + +} // namespace rmcs_core::controller::shooting + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::shooting::HeroFrictionWheelController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/hero_heat_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/hero_heat_controller.cpp new file mode 100644 index 000000000..dfd2df620 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/hero_heat_controller.cpp @@ -0,0 +1,63 @@ +#include + +#include +#include +#include +#include + +namespace rmcs_core::controller::shooting { + +class HeroHeatController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + HeroHeatController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , heat_per_shot(get_parameter("heat_per_shot").as_int()) + , reserved_heat(get_parameter("reserved_heat").as_int()) { + + register_input("/referee/shooter/cooling", shooter_cooling_); + register_input("/referee/shooter/heat_limit", shooter_heat_limit_); + + register_input("/gimbal/bullet_fired", bullet_fired_); + + register_output( + "/gimbal/control_bullet_allowance/limited_by_heat", control_bullet_allowance_, 0); + register_output("/shoot/heat", shooting_heat_, 0.0); + } + + void update() override { + shooter_heat_ = std::max(0, shooter_heat_ - *shooter_cooling_); + + if (*bullet_fired_) + shooter_heat_ += heat_per_shot; + + *control_bullet_allowance_ = std::max( + 0, (*shooter_heat_limit_ - shooter_heat_ - reserved_heat) / heat_per_shot); + + *shooting_heat_ = static_cast(shooter_heat_); + } + +private: + InputInterface shooter_cooling_; + InputInterface shooter_heat_limit_; + + InputInterface bullet_fired_; + + const int64_t heat_per_shot; + const int64_t reserved_heat; + + int64_t shooter_heat_ = 0; + OutputInterface shooting_heat_; + + OutputInterface control_bullet_allowance_; +}; + +} // namespace rmcs_core::controller::shooting + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::shooting::HeroHeatController, rmcs_executor::Component) 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 000000000..0b0c63755 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp @@ -0,0 +1,407 @@ +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "controller/pid/pid_calculator.hpp" +#include + +namespace rmcs_core::controller::shooting { + +/** + * @class PutterController + * @brief Putter mechanism controller + * + * Firing mechanism notes: + * Since the photoelectric sensor is placed at the chamber opening, testing showed that + * the double-middle action first triggers the putter to retract, and then stall detection + * confirms that the reset is complete. + * By default, a small holding force is applied so the putter does not slide down. The entire + * process uses angle-loop advancement, and grayscale detection determines whether to feed. + * Bullet firing is detected from two sources: the friction wheels and the putter stroke. + * The full scheme completed stress testing before the summer break. + */ +class PutterController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + PutterController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { + auto set_pid_parameter = [this](pid::PidCalculator& pid, const std::string& name) { + pid.kp = get_parameter(name + "_kp").as_double(); + pid.ki = get_parameter(name + "_ki").as_double(); + pid.kd = get_parameter(name + "_kd").as_double(); + get_parameter(name + "_integral_min", pid.integral_min); + get_parameter(name + "_integral_max", pid.integral_max); + get_parameter(name + "_output_min", pid.output_min); + get_parameter(name + "_output_max", pid.output_max); + }; + + 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/grayscale_sensor", grayscale_sensor_status_); + register_input("/gimbal/bullet_fired", bullet_fired_); + + register_input("/gimbal/putter/angle", putter_angle_); + register_input("/gimbal/putter/velocity", putter_velocity_); + + set_pid_parameter(bullet_feeder_velocity_pid_, "bullet_feeder_velocity"); + set_pid_parameter(putter_return_velocity_pid_, "putter_return_velocity"); + + 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/shoot/delay_ms", shoot_delay_ms_, nan_); + + // auto_aim + register_input("/gimbal/auto_aim/fire_control", fire_control_, false); + + register_output("/gimbal/shooter/mode", shoot_mode_, rmcs_msgs::ShootMode::SINGLE); + register_output("/gimbal/shooter/condiction", shoot_condiction_); + } + + ~PutterController() {} + + 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; + } + + // Normal control flow after the putter has been initialized. + if (putter_is_initialized_) { + // Handling during bullet-feeder jam-protection cooldown. + if (bullet_feeder_reverse_end_ > 0) { + bullet_feeder_reverse_end_--; + + // Early cooldown stage: reverse the feeder to clear the jam. + if (bullet_feeder_reverse_end_ > 500) + *bullet_feeder_control_torque_ = bullet_feeder_velocity_pid_.update( + -low_latency_velocity_ / 2 - *bullet_feeder_velocity_); + else { + // Late cooldown stage: stop control. + bullet_feeder_velocity_pid_.reset(); + *bullet_feeder_control_torque_ = 0.0; + } + + } else { + // Normal operating mode: only fire when the friction wheels are ready. + if (*friction_ready_) { + // Detect fire triggers. + if (switch_right != Switch::DOWN) { + + const auto now = std::chrono::steady_clock::now(); + const bool left_click_edge = (!last_mouse_.left && mouse.left); + if (left_click_edge) { + if (now - last_click_time_ < std::chrono::milliseconds(500)) { + click_count_++; + } else { + click_count_ = 1; + } + last_click_time_ = now; + } + + const bool manual_trigger = + (!last_mouse_.left && mouse.left && !mouse.right) + || (last_switch_left_ == rmcs_msgs::Switch::MIDDLE + && switch_left == rmcs_msgs::Switch::DOWN); + + const bool auto_fire_now = + (switch_right == Switch::UP || (mouse.right && mouse.left)) + && (*fire_control_); + + const bool auto_trigger_emergence = mouse.right && (click_count_ >= 2); + + const bool auto_trigger = + auto_fire_now + && (now - last_fire_time_ > std::chrono::milliseconds(1000)); + + if (manual_trigger || auto_trigger || auto_trigger_emergence) { + if (*control_bullet_allowance_limited_by_heat_ > 0 + && (shoot_stage_ == ShootStage::PRELOADED || first_shot_)) { + set_shooting(); + last_fire_time_ = now; + first_shot_ = false; + } + } + if (auto_trigger_emergence) { + click_count_ = 0; + } + } + + if (shoot_stage_ == ShootStage::PRELOADING) { + + *bullet_feeder_control_torque_ = bullet_feeder_velocity_pid_.update( + low_latency_velocity_ - *bullet_feeder_velocity_); // Velocity loop. + + update_locked_detection(); + + // This includes the photoelectric-sensor logic: if triggered, switch to + // preloaded; otherwise reverse briefly and continue rotating. + } + + if (shoot_stage_ == ShootStage::SHOOTING) { + // Firing state: detect whether the bullet has been fired. + if (*bullet_fired_ + || *putter_angle_ - putter_start_point_ >= putter_stroke_) { + shot_fired_ = true; + } + + update_putter_jam_detection(); + + if (shot_fired_) { + // Bullet fired: return the putter. + const auto angle_err = putter_start_point_ - *putter_angle_; + if (angle_err > -0.1) { + *putter_control_torque_ = 0.; + set_preloading(); + shot_fired_ = false; + } else { + *putter_control_torque_ = + putter_return_velocity_pid_.update(-80. - *putter_velocity_); + putter_timeout_detection(); + } + } else { + // Bullet not fired yet: continue advancing. + *putter_control_torque_ = + putter_return_velocity_pid_.update(60. - *putter_velocity_); + } + } + } else { + // Friction wheels not ready: stop the bullet feeder. + *bullet_feeder_control_torque_ = 0.; + } + + // Non-firing state: apply a small holding force to the putter. + if (shoot_stage_ != ShootStage::SHOOTING) + *putter_control_torque_ = -0.02; + } + } else { + // Putter not initialized: perform the reset procedure. + *putter_control_torque_ = putter_return_velocity_pid_.update(-80. - *putter_velocity_); + update_putter_jam_detection(); + } + + // Save the current state for the next comparison. + 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(); + + bullet_feeder_velocity_pid_.reset(); + *bullet_feeder_control_torque_ = nan_; + + shoot_stage_ = ShootStage::PRELOADED; + + putter_is_initialized_ = false; + putter_start_point_ = nan_; + putter_return_velocity_pid_.reset(); + putter_velocity_pid_.reset(); + putter_return_angle_pid.reset(); + *putter_control_torque_ = nan_; + + bullet_feeder_faulty_count_ = 0; + + *shoot_delay_ms_ = nan_; + } + + void set_preloading() { shoot_stage_ = ShootStage::PRELOADING; } + + void set_preloaded() { shoot_stage_ = ShootStage::PRELOADED; } + + void set_shooting() { shoot_stage_ = ShootStage::SHOOTING; } + + void update_locked_detection() { + // If feeder speed is near zero and the photoelectric sensor is triggered, + // treat it as locked and start reversing. + + if (*bullet_feeder_velocity_ < 0.5 && *bullet_feeder_control_torque_ > 0.1) { + locked_detect_count_++; + } else { + locked_detect_count_ = 0; + } + + if (locked_detect_count_ > 300) { + if (*photoelectric_sensor_status_) { + set_preloaded(); + } + // If the photoelectric sensor was not triggered, treat it as a simple jam, + // reverse briefly, then continue until stall. + locked_detect_count_ = 0; + 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; + } + + // Accumulate a fault count when the torque is abnormal. + if (putter_faulty_count_ < 500) + ++putter_faulty_count_; + else { + putter_faulty_count_ = 0; + if (shoot_stage_ != ShootStage::SHOOTING) { + // Stall detected outside the firing state: the putter is in position, + // so mark it initialized. + putter_is_initialized_ = true; + putter_start_point_ = *putter_angle_; + } else { + // Stall detected during firing: treat the bullet as fired. + shot_fired_ = true; + } + } + } + + void putter_timeout_detection() { + // If the putter stays in the firing state too long without extending, + // treat it as finished and move to the next state. + if (shoot_stage_ == ShootStage::SHOOTING) { + if (shot_fired_) { + if (putter_timeout_count_ < 1600) + ++putter_timeout_count_; + else { + putter_timeout_count_ = 0; + set_preloading(); + shot_fired_ = false; + } + } + } + } + + void enter_jam_protection() { + // Set the target angle to 60 degrees behind the current angle. + locked_detect_count_ = 0; + bullet_feeder_faulty_count_ = 0; + bullet_feeder_reverse_end_ = 800; + bullet_feeder_velocity_pid_.reset(); + } + + static constexpr double nan_ = + std::numeric_limits::quiet_NaN(); ///< Not-a-number constant. + static constexpr double inf_ = std::numeric_limits::infinity(); ///< Infinity constant. + + static constexpr double putter_stroke_ = 11.5; ///< Putter stroke length. + + 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 low_latency_velocity_ = 5.0; + + InputInterface photoelectric_sensor_status_; + InputInterface grayscale_sensor_status_; + InputInterface bullet_fired_; + bool shot_fired_{false}; + bool first_shot_{true}; + + 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(); + + InputInterface bullet_feeder_angle_; + InputInterface bullet_feeder_velocity_; + + InputInterface control_bullet_allowance_limited_by_heat_; + + bool putter_is_initialized_ = false; + int putter_faulty_count_ = 0; + int putter_timeout_count_ = 0; + double putter_start_point_ = nan_; + 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; + + pid::PidCalculator bullet_feeder_velocity_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; + + OutputInterface shoot_delay_ms_; + + InputInterface fire_control_; + std::chrono::steady_clock::time_point last_fire_time_{}; + std::chrono::steady_clock::time_point last_click_time_{}; + int click_count_ = 0; + + int locked_detect_count_ = 0; + int bullet_feeder_reverse_end_ = 0; + + InputInterface bullet_feeder_torque; + InputInterface putter_torque; + + OutputInterface shoot_mode_; + OutputInterface shoot_condiction_; +}; + +} // 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/controller/shooting/shooting_recorder.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/shooting_recorder.cpp index 1cb9de963..6cefe2753 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,27 +21,10 @@ 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++) - register_input(topic[i], friction_wheels_velocity_[i]); - } using namespace std::chrono; auto now = high_resolution_clock::now(); @@ -46,12 +32,15 @@ class ShootingRecorder auto file = fmt::format("/robot_shoot/{}.log", ms); log_stream_.open(file); + + std::ofstream out_file("shoot_recorder"); + RCLCPP_INFO(get_logger(), "ShootingRecorder initialized, log file: %s", file.c_str()); } ~ShootingRecorder() { log_stream_.close(); } void update() override { - + // Evaluate friction-wheel quality. switch (log_mode_) { case LogMode::TRIGGER: // It will be triggered by shooting action @@ -64,38 +53,52 @@ 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_ == 2) { - log_text = fmt::format( - "{},{},{},{},{}", timestamp, *initial_speed_, // - *friction_wheels_velocity_[0], *friction_wheels_velocity_[1], - *fractional_temperature_); - } else if (friction_wheel_count_ == 4) { + 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()); + RCLCPP_INFO( + get_logger(), "%s", + log_text.c_str()); // Record each shot's initial speed and statistics. + + log_velocity = fmt::format("{:.3f}", *initial_speed_); + std::ofstream out_file("shoot_recorder", std::ios::app); + if (out_file.is_open()) { + out_file << log_velocity << std::endl; + out_file.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,9 +106,26 @@ 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); @@ -122,10 +142,117 @@ 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 #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::shooting::ShootingRecorder, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::shooting::ShootingRecorder, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp index 7ab8a38f5..1bc0ef8a8 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp @@ -43,6 +43,7 @@ class LkMotor { rmcs_executor::Component& status_component, rmcs_executor::Component& command_component, const std::string& name_prefix) { status_component.register_output(name_prefix + "/angle", angle_output_, 0.0); + status_component.register_output(name_prefix + "/raw_angle", raw_angle_output_, 0); status_component.register_output(name_prefix + "/velocity", velocity_output_, 0.0); status_component.register_output(name_prefix + "/torque", torque_output_, 0.0); status_component.register_output(name_prefix + "/temperature", temperature_output_, 0.0); @@ -75,7 +76,7 @@ class LkMotor { switch (config.motor_type) { case Type::kMG5010Ei10: raw_angle_modulus_ = 1 << 16; - torque_constant = 0.90909; + torque_constant = 0.1; reduction_ratio = 10.0; // Note: max_torque_ should represent the ACTUAL maximum torque of the motor. @@ -93,7 +94,7 @@ class LkMotor { break; case Type::kMG6012Ei8: raw_angle_modulus_ = 1 << 16; - torque_constant = 1.09; + torque_constant = 1.09 / 8.0; reduction_ratio = 8.0; max_torque_ = 16.0; break; @@ -193,6 +194,7 @@ class LkMotor { torque_ = status_current_to_torque_coefficient_ * static_cast(feedback.current); *angle_output_ = angle(); + *raw_angle_output_ = last_raw_angle(); *velocity_output_ = velocity(); *torque_output_ = torque(); *temperature_output_ = temperature(); @@ -521,6 +523,7 @@ class LkMotor { double temperature_; rmcs_executor::Component::OutputInterface angle_output_; + rmcs_executor::Component::OutputInterface raw_angle_output_; rmcs_executor::Component::OutputInterface velocity_output_; rmcs_executor::Component::OutputInterface torque_output_; rmcs_executor::Component::OutputInterface temperature_output_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little.cpp new file mode 100644 index 000000000..40150e605 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little.cpp @@ -0,0 +1,865 @@ +#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" +#include "hardware/device/supercap.hpp" + +namespace rmcs_core::hardware { + +class CanReceiveRateCounter { +public: + explicit CanReceiveRateCounter(rclcpp::Logger logger, std::string_view channel_name) + : logger_(std::move(logger)) + , channel_name_(channel_name) {} + + void record(std::uint32_t can_id) { + const auto now = Clock::now(); + + std::lock_guard lock{mutex_}; + auto& status = statuses_[can_id]; + ++status.receive_count; + status.last_receive_time = now; + + report_if_due(now); + } + + void report_if_due() { + const auto now = Clock::now(); + + std::lock_guard lock{mutex_}; + report_if_due(now); + } + +private: + using Clock = std::chrono::steady_clock; + + struct Status { + std::size_t receive_count{0}; + Clock::time_point last_receive_time{}; + }; + + void report_if_due(Clock::time_point now) { + if (statuses_.empty()) + return; + + if (last_report_time_ == Clock::time_point{}) { + last_report_time_ = now; + return; + } + + const auto elapsed = now - last_report_time_; + if (elapsed < kReportInterval) + return; + + const auto elapsed_seconds = std::chrono::duration(elapsed).count(); + for (auto& [can_id, status] : statuses_) { + const bool attached = now - status.last_receive_time <= kMissTimeout; + RCLCPP_INFO( + logger_, "[can rx] %.*s id=0x%03X rate=%.1fHz status=%s", + static_cast(channel_name_.size()), channel_name_.data(), + static_cast(can_id), + static_cast(status.receive_count) / elapsed_seconds, + attached ? "attach" : "miss"); + status.receive_count = 0; + } + + last_report_time_ = now; + } + + static constexpr std::chrono::milliseconds kReportInterval{1000}; + static constexpr std::chrono::milliseconds kMissTimeout{1000}; + + rclcpp::Logger logger_; + std::string_view channel_name_; + std::mutex mutex_; + Clock::time_point last_report_time_{}; + std::map statuses_; +}; + +class SteeringHeroLittle + : public rmcs_executor::Component + , public rclcpp::Node { +public: + SteeringHeroLittle() + : 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)); + }); + + 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("serial_bottom_rmcs_board").as_string()); + + tf_->set_transform( + Eigen::Translation3d{0.06603, 0.0, 0.082}); + } + + SteeringHeroLittle(const SteeringHeroLittle&) = delete; + SteeringHeroLittle& operator=(const SteeringHeroLittle&) = delete; + SteeringHeroLittle(SteeringHeroLittle&&) = delete; + SteeringHeroLittle& operator=(SteeringHeroLittle&&) = delete; + + ~SteeringHeroLittle() override = default; + + void update() override { + top_board_->update(); + bottom_board_->update(); + + tf_->set_state( + bottom_board_->gimbal_bottom_yaw_motor_.angle() + + top_board_->gimbal_top_yaw_motor_.angle()); + tf_->set_state( + top_board_->gimbal_pitch_motor_.angle()); + } + + void command_update() { + top_board_->command_update(); + bottom_board_->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()); + 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 player viewer offset: %ld", + top_board_->gimbal_player_viewer_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 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_->chassis_steering_motors_[0].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[chassis calibration] right front steering offset: %d", + bottom_board_->chassis_steering_motors_[1].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[chassis calibration] left back steering offset: %d", + bottom_board_->chassis_steering_motors_[2].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[chassis calibration] right back steering offset: %d", + bottom_board_->chassis_steering_motors_[3].calibrate_zero_point()); + } + + class SteeringHeroLittleCommand : public rmcs_executor::Component { + public: + explicit SteeringHeroLittleCommand(SteeringHeroLittle& hero) + : hero_(hero) {} + + void update() override { hero_.command_update(); } + + SteeringHeroLittle& hero_; + }; + std::shared_ptr command_component_; + + class TopBoard final : private librmcs::agent::RmcsBoardLite { + public: + friend class SteeringHeroLittle; + explicit TopBoard( + SteeringHeroLittle& steering_hero, SteeringHeroLittleCommand& steering_hero_command, + std::string_view board_serial = {}) + : librmcs::agent::RmcsBoardLite(board_serial) + , logger_(steering_hero.get_logger()) + // , can0_receive_rate_counter_(logger_, "bottom/can0") + // , can1_receive_rate_counter_(logger_, "bottom/can1") + // , can2_receive_rate_counter_(logger_, "bottom/can2") + // , can3_receive_rate_counter_(logger_, "bottom/can3") + , tf_(steering_hero.tf_) + , imu_(1000, 0.2, 0.0) + , 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_( + {steering_hero, steering_hero_command, "/gimbal/first_right_friction"}, + {steering_hero, steering_hero_command, "/gimbal/first_left_friction"}, + {steering_hero, steering_hero_command, "/gimbal/second_right_friction"}, + {steering_hero, steering_hero_command, "/gimbal/second_left_friction"}) + , gimbal_bullet_feeder_(steering_hero, steering_hero_command, "/gimbal/bullet_feeder") + , putter_motor_(steering_hero, steering_hero_command, "/gimbal/putter") + , gimbal_scope_motor_(steering_hero, steering_hero_command, "/gimbal/scope") + , gimbal_player_viewer_motor_( + steering_hero, steering_hero_command, "/gimbal/player_viewer") { + + gimbal_top_yaw_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} + .enable_multi_turn_angle() + .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())) + .enable_multi_turn_angle()); + 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_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_reversed() + .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( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reduction_ratio(1.) + .enable_multi_turn_angle()); + gimbal_scope_motor_.configure(device::DjiMotor::Config{device::DjiMotor::Type::kM2006}); + gimbal_player_viewer_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("viewer_motor_zero_point").as_int())) + .set_reversed() + .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( + "/gimbal/grayscale_sensor", grayscale_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. + + return std::make_tuple(-y, x, z); + }); + } + + TopBoard(const TopBoard&) = delete; + TopBoard& operator=(const TopBoard&) = delete; + TopBoard(TopBoard&&) = delete; + TopBoard& operator=(TopBoard&&) = delete; + + ~TopBoard() final = default; + + void update() { + // can0_receive_rate_counter_.report_if_due(); + // can1_receive_rate_counter_.report_if_due(); + // can2_receive_rate_counter_.report_if_due(); + // can3_receive_rate_counter_.report_if_due(); + + imu_.update_status(); + 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_top_yaw_motor_.update_status(); + gimbal_pitch_motor_.update_status(); + tf_->set_state( + gimbal_pitch_motor_.angle()); + + for (auto& motor : gimbal_friction_wheels_) + motor.update_status(); + + gimbal_bullet_feeder_.update_status(); + putter_motor_.update_status(); + + gimbal_player_viewer_motor_.update_status(); + tf_->set_state( + gimbal_player_viewer_motor_.angle()); + + gimbal_scope_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_; + + *photoelectric_sensor_status_ = photoelectric_sensor_status_atomic.load(); + *grayscale_sensor_status_ = grayscale_sensor_status_atomic.load(); + } + + void command_update() { + auto builder = start_transmit(); + + if (std::isfinite(gimbal_pitch_motor_.control_angle())) + builder.can0_transmit({ + .can_id = 0x142, + .can_data = gimbal_pitch_motor_ + .generate_angle_command(gimbal_pitch_motor_.control_angle()) + .as_bytes(), + }); + else + builder.can0_transmit({ + .can_id = 0x142, + .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), + }); // Used to distinguish pitch encoder control from IMU control. + + builder.can0_transmit({ + .can_id = 0x141, + .can_data = gimbal_top_yaw_motor_.generate_command().as_bytes(), + }); + + 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(), + }); + + builder.can1_transmit({ + .can_id = 0x1FF, + .can_data = + device::CanPacket8{ + putter_motor_.generate_command(), + gimbal_scope_motor_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + + builder.can2_transmit({ + .can_id = 0x143, + .can_data = + gimbal_player_viewer_motor_ + .generate_velocity_command(gimbal_player_viewer_motor_.control_velocity()) + .as_bytes(), + }); + + builder.can3_transmit({ + .can_id = 0x142, + .can_data = gimbal_bullet_feeder_.generate_torque_command().as_bytes(), + }); + + builder.gpio_digital_read( + librmcs::spec::rmcs_board_lite::kGpioDescriptors[2], + { + .period_ms = 20, + .pull = librmcs::data::GpioPull::kUp, + }); + + builder.gpio_digital_read( + librmcs::spec::rmcs_board_lite::kGpioDescriptors[3], + { + .period_ms = 20, + .pull = librmcs::data::GpioPull::kUp, + }); + } + + 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; + // can0_receive_rate_counter_.record(can_id); + if (can_id == 0x141) { + gimbal_top_yaw_motor_.store_status(data.can_data); + } else if (can_id == 0x142) { + gimbal_pitch_motor_.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; + // can1_receive_rate_counter_.record(can_id); + if (can_id == 0x201) { + gimbal_friction_wheels_[0].store_status(data.can_data); + } else if (can_id == 0x202) { + gimbal_friction_wheels_[1].store_status(data.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); + } else if (can_id == 0x205) { + putter_motor_.store_status(data.can_data); + } else if (can_id == 0x206) { + gimbal_scope_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; + // can2_receive_rate_counter_.record(can_id); + if (can_id == 0x143) { + gimbal_player_viewer_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; + // can3_receive_rate_counter_.record(can_id); + if (can_id == 0x142) { + gimbal_bullet_feeder_.store_status(data.can_data); + } + } + + void gpio_digital_read_result_callback( + const librmcs::spec::rmcs_board_lite::GpioDescriptor& gpio, + const librmcs::data::GpioDigitalDataView& data) override { + if (gpio.channel_index == 2) { + photoelectric_sensor_status_atomic.store(data.high); + } else if (gpio.channel_index == 3) { + grayscale_sensor_status_atomic.store(!data.high); + } + } + + 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_; + // CanReceiveRateCounter can0_receive_rate_counter_; + // CanReceiveRateCounter can1_receive_rate_counter_; + // CanReceiveRateCounter can2_receive_rate_counter_; + // CanReceiveRateCounter can3_receive_rate_counter_; + OutputInterface& tf_; + + std::time_t last_camera_capturer_trigger_timestamp_{0}; + + device::Bmi088 imu_; + device::LkMotor gimbal_top_yaw_motor_; + device::LkMotor gimbal_pitch_motor_; + device::DjiMotor gimbal_friction_wheels_[4]; + device::LkMotor gimbal_bullet_feeder_; + device::DjiMotor putter_motor_; + device::DjiMotor gimbal_scope_motor_; + device::LkMotor gimbal_player_viewer_motor_; + + OutputInterface gimbal_yaw_velocity_imu_; + OutputInterface gimbal_pitch_velocity_imu_; + OutputInterface photoelectric_sensor_status_; + OutputInterface grayscale_sensor_status_; + OutputInterface camera_capturer_trigger_; + OutputInterface camera_capturer_trigger_timestamp_; + std::atomic photoelectric_sensor_status_atomic{false}; + std::atomic grayscale_sensor_status_atomic{false}; + }; + + class BottomBoard final : private librmcs::agent::RmcsBoardLite { + public: + friend class SteeringHeroLittle; + explicit BottomBoard( + SteeringHeroLittle& steering_hero, SteeringHeroLittleCommand& steering_hero_command, + std::string_view board_serial = {}) + : librmcs::agent::RmcsBoardLite( + board_serial, {.dangerously_skip_version_checks = false}) + , logger_(steering_hero.get_logger()) + // , can0_receive_rate_counter_(logger_, "bottom/can0") + // , can1_receive_rate_counter_(logger_, "bottom/can1") + // , can2_receive_rate_counter_(logger_, "bottom/can2") + // , can3_receive_rate_counter_(logger_, "bottom/can3") + , imu_(1000, 0.2, 0.0) + , dr16_(steering_hero) + , supercap_(steering_hero, steering_hero_command) + , chassis_steering_motors_( + {steering_hero, steering_hero_command, "/chassis/left_front_steering"}, + {steering_hero, steering_hero_command, "/chassis/right_front_steering"}, + {steering_hero, steering_hero_command, "/chassis/left_back_steering"}, + {steering_hero, steering_hero_command, "/chassis/right_back_steering"}) + , chassis_wheel_motors_( + {steering_hero, steering_hero_command, "/chassis/left_front_wheel"}, + {steering_hero, steering_hero_command, "/chassis/right_front_wheel"}, + {steering_hero, steering_hero_command, "/chassis/left_back_wheel"}, + {steering_hero, steering_hero_command, "/chassis/right_back_wheel"}) + , chassis_front_climber_motor_( + {steering_hero, steering_hero_command, "/chassis/climber/left_front_motor"}, + {steering_hero, steering_hero_command, "/chassis/climber/right_front_motor"}) + , chassis_back_climber_motor_( + {steering_hero, steering_hero_command, "/chassis/climber/left_back_motor"}, + {steering_hero, steering_hero_command, "/chassis/climber/right_back_motor"}) + , gimbal_bottom_yaw_motor_(steering_hero, steering_hero_command, "/gimbal/bottom_yaw") { + // + 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_steering_motors_[2].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_motors_[3].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_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.)); + + 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_); + 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().uart0_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.); + + 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(const BottomBoard&) = delete; + BottomBoard& operator=(const BottomBoard&) = delete; + BottomBoard(BottomBoard&&) = delete; + BottomBoard& operator=(BottomBoard&&) = delete; + + ~BottomBoard() final = default; + + void update() { + // can0_receive_rate_counter_.report_if_due(); + // can1_receive_rate_counter_.report_if_due(); + // can2_receive_rate_counter_.report_if_due(); + // can3_receive_rate_counter_.report_if_due(); + + 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())); + + 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(); + + gimbal_bottom_yaw_motor_.update_status(); + } + + void command_update() { + auto builder = start_transmit(); + + builder.can0_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[0].generate_command(), + chassis_wheel_motors_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + + builder.can0_transmit({ + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + chassis_steering_motors_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + chassis_steering_motors_[0].generate_command(), + } + .as_bytes(), + }); + + builder.can1_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + 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{}, + chassis_steering_motors_[3].generate_command(), + chassis_steering_motors_[2].generate_command(), + supercap_.generate_command(), + } + .as_bytes(), + }); + + builder.can3_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + chassis_back_climber_motor_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + chassis_back_climber_motor_[0].generate_command(), + } + .as_bytes(), + }); + + builder.can3_transmit({ + .can_id = 0x141, + .can_data = gimbal_bottom_yaw_motor_.generate_command().as_bytes(), + }); + + builder.can2_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_front_climber_motor_[0].generate_command(), + device::CanPacket8::PaddingQuarter{}, + chassis_front_climber_motor_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + } + .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; + // can0_receive_rate_counter_.record(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 == 0x205) { + chassis_steering_motors_[1].store_status(data.can_data); + } else if (can_id == 0x208) { + chassis_steering_motors_[0].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; + // can1_receive_rate_counter_.record(can_id); + 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 == 0x207) { + chassis_steering_motors_[2].store_status(data.can_data); + } else if (can_id == 0x206) { + 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) + return; + auto can_id = data.can_id; + // can2_receive_rate_counter_.record(can_id); + if (can_id == 0x201) { + chassis_front_climber_motor_[0].store_status(data.can_data); + } else if (can_id == 0x203) { + chassis_front_climber_motor_[1].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; + // can3_receive_rate_counter_.record(can_id); + if (can_id == 0x202) { + chassis_back_climber_motor_[1].store_status(data.can_data); + } else if (can_id == 0x204) { + chassis_back_climber_motor_[0].store_status(data.can_data); + } else if (can_id == 0x141) { + gimbal_bottom_yaw_motor_.store_status(data.can_data); + } + } + + void uart0_receive_callback(const librmcs::data::UartDataView& data) override { + const std::byte* ptr = data.uart_data.data(); + referee_ring_buffer_receive_.emplace_back_n( + [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, 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_; + // CanReceiveRateCounter can0_receive_rate_counter_; + // CanReceiveRateCounter can1_receive_rate_counter_; + // CanReceiveRateCounter can2_receive_rate_counter_; + // CanReceiveRateCounter can3_receive_rate_counter_; + + device::Bmi088 imu_; + device::Dr16 dr16_; + device::Supercap supercap_; + + device::DjiMotor chassis_steering_motors_[4]; + device::DjiMotor chassis_wheel_motors_[4]; + device::DjiMotor chassis_front_climber_motor_[2]; + device::DjiMotor chassis_back_climber_motor_[2]; + device::LkMotor gimbal_bottom_yaw_motor_; + + rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; + + OutputInterface referee_serial_; + OutputInterface powermeter_control_enabled_; + OutputInterface powermeter_charge_power_limit_; + OutputInterface chassis_yaw_velocity_imu_; + OutputInterface chassis_pitch_imu_; + }; + + OutputInterface tf_; + + rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; + + std::shared_ptr top_board_; + std::shared_ptr bottom_board_; +}; + +} // namespace rmcs_core::hardware + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringHeroLittle, rmcs_executor::Component) 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 2da9e0dd8..b0b811f0e 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp @@ -1,14 +1,15 @@ +#include #include #include #include #include #include #include -#include #include #include #include +#include #include #include #include @@ -18,10 +19,8 @@ #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" @@ -31,21 +30,89 @@ namespace rmcs_core::hardware { +class CanReceiveRateCounter { +public: + explicit CanReceiveRateCounter(rclcpp::Logger logger, std::string_view channel_name) + : logger_(std::move(logger)) + , channel_name_(channel_name) {} + + void record(std::uint32_t can_id) { + const auto now = Clock::now(); + + std::lock_guard lock{mutex_}; + auto& status = statuses_[can_id]; + ++status.receive_count; + status.last_receive_time = now; + + report_if_due(now); + } + + void report_if_due() { + const auto now = Clock::now(); + + std::lock_guard lock{mutex_}; + report_if_due(now); + } + +private: + using Clock = std::chrono::steady_clock; + + struct Status { + std::size_t receive_count{0}; + Clock::time_point last_receive_time{}; + }; + + void report_if_due(Clock::time_point now) { + if (statuses_.empty()) + return; + + if (last_report_time_ == Clock::time_point{}) { + last_report_time_ = now; + return; + } + + const auto elapsed = now - last_report_time_; + if (elapsed < kReportInterval) + return; + + const auto elapsed_seconds = std::chrono::duration(elapsed).count(); + for (auto& [can_id, status] : statuses_) { + const bool attached = now - status.last_receive_time <= kMissTimeout; + RCLCPP_INFO( + logger_, "[can rx] %.*s id=0x%03X rate=%.1fHz status=%s", + static_cast(channel_name_.size()), channel_name_.data(), + static_cast(can_id), + static_cast(status.receive_count) / elapsed_seconds, + attached ? "attach" : "miss"); + status.receive_count = 0; + } + + last_report_time_ = now; + } + + static constexpr std::chrono::milliseconds kReportInterval{1000}; + static constexpr std::chrono::milliseconds kMissTimeout{1000}; + + rclcpp::Logger logger_; + std::string_view channel_name_; + std::mutex mutex_; + Clock::time_point last_report_time_{}; + std::map statuses_; +}; + class SteeringHero : public rmcs_executor::Component , public rclcpp::Node { public: SteeringHero() - : Node{ + : Node( get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) , command_component_( create_partner_component( get_component_name() + "_command", *this)) { 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) { @@ -54,10 +121,15 @@ 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); + 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}); } SteeringHero(const SteeringHero&) = delete; @@ -69,30 +141,27 @@ class SteeringHero 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(); + + tf_->set_state( + bottom_board_two_->gimbal_bottom_yaw_motor_.angle() + + top_board_->gimbal_top_yaw_motor_.angle()); + tf_->set_state( + top_board_->gimbal_pitch_motor_.angle()); } 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()); @@ -100,20 +169,44 @@ class SteeringHero 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()); + 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_->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()); + bottom_board_two_->chassis_steering_motors2_[1].calibrate_zero_point()); RCLCPP_INFO( - get_logger(), "[chassis calibration] right front steering offset: %d", - bottom_board_->chassis_steering_motors_[3].calibrate_zero_point()); + get_logger(), "[chassis calibration] left front wheel offset: %d", + bottom_board_one_->chassis_wheel_motors_[0].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[chassis calibration] right front wheel offset: %d", + bottom_board_one_->chassis_wheel_motors_[1].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[chassis calibration] left back wheel offset: %d", + bottom_board_two_->chassis_wheel_motors2_[0].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[chassis calibration] right back wheel offset: %d", + bottom_board_two_->chassis_wheel_motors2_[1].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[booster calibration] left front friction wheel offset: %d", + top_board_->gimbal_friction_wheels_[0].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[booster calibration] right front friction wheel offset: %d", + top_board_->gimbal_friction_wheels_[1].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[booster calibration] left back friction wheel offset: %d", + top_board_->gimbal_friction_wheels_[2].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[booster calibration] right back friction wheel offset: %d", + top_board_->gimbal_friction_wheels_[3].calibrate_zero_point()); } class SteeringHeroCommand : public rmcs_executor::Component { @@ -123,7 +216,6 @@ class SteeringHero void update() override { hero_.command_update(); } - private: SteeringHero& hero_; }; std::shared_ptr command_component_; @@ -132,68 +224,88 @@ class SteeringHero public: friend class SteeringHero; explicit TopBoard( - SteeringHero& hero, SteeringHeroCommand& hero_command, + SteeringHero& steering_hero, SteeringHeroCommand& steering_hero_command, std::string_view board_serial = {}) : librmcs::agent::CBoard(board_serial) - , tf_(hero.tf_) + , logger_(steering_hero.get_logger()) + , tf_(steering_hero.tf_) + // , can1_receive_rate_counter_(logger_, "bottom/can1") + // , can2_receive_rate_counter_(logger_, "bottom/can2") , 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} - .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} - .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/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.)}, - {hero, hero_command, "/gimbal/first_right_friction", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(1.) - .set_reversed()}, - {hero, hero_command, "/gimbal/second_right_friction", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(1.) - .set_reversed()}) - , gimbal_bullet_feeder_( - hero, hero_command, "/gimbal/bullet_feeder", - device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} - .set_reversed() - .enable_multi_turn_angle()) - , gimbal_scope_motor_( - hero, hero_command, "/gimbal/scope", - device::DjiMotor::Config{device::DjiMotor::Type::kM2006}) + {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_scope_motor_(steering_hero, steering_hero_command, "/gimbal/scope") , 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()) { + steering_hero, steering_hero_command, "/gimbal/player_viewer") { + + 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())) + .enable_multi_turn_angle()); + 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_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_reversed() + .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( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reduction_ratio(1.) + .enable_multi_turn_angle()); + gimbal_scope_motor_.configure(device::DjiMotor::Config{device::DjiMotor::Type::kM2006}); + gimbal_player_viewer_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("viewer_motor_zero_point").as_int())) + .set_reversed() + .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( + "/gimbal/grayscale_sensor", grayscale_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. - - // 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}; - // std::cout << mapping << std::endl; + // The rotation angle must be an exact multiple of 90 degrees, otherwise + // use a matrix. - 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_); } TopBoard(const TopBoard&) = delete; @@ -205,32 +317,294 @@ class SteeringHero 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(); gimbal_top_yaw_motor_.update_status(); - gimbal_pitch_motor_.update_status(); tf_->set_state( gimbal_pitch_motor_.angle()); + for (auto& motor : gimbal_friction_wheels_) + motor.update_status(); + + gimbal_bullet_feeder_.update_status(); + putter_motor_.update_status(); + 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(); + if (last_camera_capturer_trigger_timestamp_ != *camera_capturer_trigger_timestamp_) + *camera_capturer_trigger_ = true; + last_camera_capturer_trigger_timestamp_ = *camera_capturer_trigger_timestamp_; - gimbal_bullet_feeder_.update_status(); + *photoelectric_sensor_status_ = photoelectric_sensor_status_atomic.load(); + *grayscale_sensor_status_ = grayscale_sensor_status_atomic.load(); + } + + void command_update() { + auto builder = start_transmit(); + + if (control_flag == 0) { + 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(), + }); + + builder.can2_transmit({ + .can_id = 0x143, + .can_data = gimbal_player_viewer_motor_ + .generate_velocity_command( + gimbal_player_viewer_motor_.control_velocity()) + .as_bytes(), + }); + + if (std::isfinite(gimbal_pitch_motor_.control_angle())) + builder.can2_transmit({ + .can_id = 0x142, + .can_data = gimbal_pitch_motor_ + .generate_angle_command(gimbal_pitch_motor_.control_angle()) + .as_bytes(), + }); + else + builder.can2_transmit({ + .can_id = 0x142, + .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), + }); // Used to distinguish pitch encoder control from IMU control. + + } else { + builder.can1_transmit({ + .can_id = 0x1FF, + .can_data = + device::CanPacket8{ + putter_motor_.generate_command(), + gimbal_scope_motor_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + + builder.can1_transmit({ + .can_id = 0x141, + .can_data = gimbal_bullet_feeder_.generate_torque_command().as_bytes(), + }); + + builder.can2_transmit({ + .can_id = 0x141, + .can_data = gimbal_top_yaw_motor_.generate_command().as_bytes(), + }); + } + + builder.gpio_digital_read( + librmcs::spec::c_board::kGpioDescriptors[6], + { + .period_ms = 20, + .pull = librmcs::data::GpioPull::kUp, + }); + + builder.gpio_digital_read( + librmcs::spec::c_board::kGpioDescriptors[4], + { + .period_ms = 20, + .pull = librmcs::data::GpioPull::kUp, + }); + + if (control_flag == 0) { + control_flag = 1; + } else { + control_flag = 0; + } + } + + 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; + // can1_receive_rate_counter_.record(can_id); + if (can_id == 0x204) { + + gimbal_friction_wheels_[0].store_status(data.can_data); + } else if (can_id == 0x202) { + gimbal_friction_wheels_[1].store_status(data.can_data); + } else if (can_id == 0x203) { + gimbal_friction_wheels_[2].store_status(data.can_data); + } else if (can_id == 0x201) { + gimbal_friction_wheels_[3].store_status(data.can_data); + } else if (can_id == 0x205) { + putter_motor_.store_status(data.can_data); + } else if (can_id == 0x141) { + gimbal_bullet_feeder_.store_status(data.can_data); + } else if (can_id == 0x206) { + gimbal_scope_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; + // can2_receive_rate_counter_.record(can_id); + if (can_id == 0x141) { + gimbal_top_yaw_motor_.store_status(data.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); + } + } + + void gpio_digital_read_result_callback( + const librmcs::spec::c_board::GpioDescriptor& gpio, + const librmcs::data::GpioDigitalDataView& data) override { + if (gpio.channel_index == 6) { + photoelectric_sensor_status_atomic.store(!data.high); + } else if (gpio.channel_index == 4) { + grayscale_sensor_status_atomic.store(!data.high); + } + } + 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); + } + + int control_flag = 0; + rclcpp::Logger logger_; + OutputInterface& tf_; + // CanReceiveRateCounter can1_receive_rate_counter_; + // CanReceiveRateCounter can2_receive_rate_counter_; + + std::time_t last_camera_capturer_trigger_timestamp_{0}; + + device::Bmi088 imu_; + device::LkMotor gimbal_top_yaw_motor_; + device::LkMotor gimbal_pitch_motor_; + device::DjiMotor gimbal_friction_wheels_[4]; + device::LkMotor gimbal_bullet_feeder_; + device::DjiMotor putter_motor_; + device::DjiMotor gimbal_scope_motor_; + device::LkMotor gimbal_player_viewer_motor_; + + OutputInterface gimbal_yaw_velocity_imu_; + OutputInterface gimbal_pitch_velocity_imu_; + OutputInterface photoelectric_sensor_status_; + OutputInterface grayscale_sensor_status_; + OutputInterface camera_capturer_trigger_; + OutputInterface camera_capturer_trigger_timestamp_; + std::atomic photoelectric_sensor_status_atomic{false}; + std::atomic grayscale_sensor_status_atomic{false}; + }; + + class BottomBoard_one final : private librmcs::agent::CBoard { + public: + friend class SteeringHero; + explicit BottomBoard_one( + SteeringHero& steering_hero, SteeringHeroCommand& steering_hero_command, + std::string_view board_serial = {}) + : librmcs::agent::CBoard(board_serial) + , logger_(steering_hero.get_logger()) + // , can1_receive_rate_counter_(logger_, "bottom/can1") + // , can2_receive_rate_counter_(logger_, "bottom/can2") + , imu_(1000, 0.2, 0.0) + , chassis_front_climber_motor_( + {steering_hero, steering_hero_command, "/chassis/climber/left_front_motor"}, + {steering_hero, steering_hero_command, "/chassis/climber/right_front_motor"}) + , chassis_back_climber_motor_( + {steering_hero, steering_hero_command, "/chassis/climber/left_back_motor"}, + {steering_hero, steering_hero_command, "/chassis/climber/right_back_motor"}) + , chassis_steering_motors_( + {steering_hero, steering_hero_command, "/chassis/left_front_steering"}, + {steering_hero, steering_hero_command, "/chassis/right_front_steering"}) + , chassis_wheel_motors_( + {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.)); + + 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); + 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; + BottomBoard_one(BottomBoard_one&&) = delete; + BottomBoard_one& operator=(BottomBoard_one&&) = delete; + + ~BottomBoard_one() final = default; + + void update() { + imu_.update_status(); + // can1_receive_rate_counter_.report_if_due(); + // can2_receive_rate_counter_.report_if_due(); + + *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(); + 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() { @@ -240,91 +614,72 @@ class SteeringHero .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(), + 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 = 0x141, - .can_data = gimbal_bullet_feeder_ - .generate_torque_command(gimbal_bullet_feeder_.control_torque()) - .as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x200, + .can_id = 0x1FE, .can_data = device::CanPacket8{ - gimbal_scope_motor_.generate_command(), - device::CanPacket8::PaddingQuarter{}, 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 = 0x143, + .can_id = 0x200, .can_data = - gimbal_player_viewer_motor_ - .generate_velocity_command(gimbal_player_viewer_motor_.control_velocity()) + 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 = 0x141, - .can_data = gimbal_top_yaw_motor_.generate_command().as_bytes(), - }); - - builder.can2_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; + // can1_receive_rate_counter_.record(can_id); if (can_id == 0x201) { - gimbal_friction_wheels_[0].store_status(data.can_data); + chassis_wheel_motors_[1].store_status(data.can_data); } else if (can_id == 0x202) { - gimbal_friction_wheels_[1].store_status(data.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); - } else if (can_id == 0x141) { - gimbal_bullet_feeder_.store_status(data.can_data); + chassis_wheel_motors_[0].store_status(data.can_data); + } 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); } } 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(data.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); + // can2_receive_rate_counter_.record(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) { - gimbal_scope_motor_.store_status(data.can_data); + 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 uart2_receive_callback(const librmcs::data::UartDataView& data) override { - benewake_.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); @@ -334,82 +689,69 @@ class SteeringHero imu_.store_gyroscope_status(data.x, data.y, data.z); } - OutputInterface& tf_; + rclcpp::Logger logger_; + // CanReceiveRateCounter can1_receive_rate_counter_; + // CanReceiveRateCounter can2_receive_rate_counter_; device::Bmi088 imu_; - device::Benewake benewake_; + 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]; + OutputInterface chassis_yaw_velocity_imu_; + OutputInterface chassis_pitch_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_; - - device::DjiMotor gimbal_scope_motor_; - device::LkMotor gimbal_player_viewer_motor_; }; - class BottomBoard final : private librmcs::agent::CBoard { + class BottomBoard_two final : private librmcs::agent::CBoard { public: friend class SteeringHero; - explicit BottomBoard( - SteeringHero& hero, SteeringHeroCommand& hero_command, + explicit BottomBoard_two( + SteeringHero& steering_hero, SteeringHeroCommand& steering_hero_command, std::string_view board_serial = {}) : librmcs::agent::CBoard(board_serial) + , logger_(steering_hero.get_logger()) + // , can1_receive_rate_counter_(logger_, "bottom/can1") + // , can2_receive_rate_counter_(logger_, "bottom/can2") , imu_(1000, 0.2, 0.0) - , tf_(hero.tf_) - , dr16_(hero) - , chassis_steering_motors_( - {hero, hero_command, "/chassis/left_front_steering", - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .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} - .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} - .set_reversed() - .set_reduction_ratio(2232. / 169.)}, - {hero, hero_command, "/chassis/left_back_wheel", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .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} - .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} - .set_reversed() - .set_encoder_zero_point( - static_cast( - hero.get_parameter("bottom_yaw_motor_zero_point").as_int()))) { - - hero.register_output("/referee/serial", referee_serial_); + , dr16_(steering_hero) + , supercap_(steering_hero, steering_hero_command) + , chassis_steering_motors2_( + {steering_hero, steering_hero_command, "/chassis/left_back_steering"}, + {steering_hero, steering_hero_command, "/chassis/right_back_steering"}) + , chassis_wheel_motors2_( + {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_); 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); @@ -419,36 +761,32 @@ class SteeringHero {.uart_data = std::span{buffer, size}}); return size; }; - - hero.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); - - hero.register_output( + 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.); } - BottomBoard(const BottomBoard&) = delete; - BottomBoard& operator=(const BottomBoard&) = delete; - BottomBoard(BottomBoard&&) = delete; - BottomBoard& operator=(BottomBoard&&) = 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() final = default; + ~BottomBoard_two() final = default; void update() { imu_.update_status(); dr16_.update_status(); supercap_.update_status(); + // can1_receive_rate_counter_.report_if_due(); + // can2_receive_rate_counter_.report_if_due(); - *chassis_yaw_velocity_imu_ = imu_.gz(); - - for (auto& motor : chassis_wheel_motors_) + 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() { @@ -458,10 +796,10 @@ class SteeringHero .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(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + chassis_wheel_motors2_[1].generate_command(), + chassis_wheel_motors2_[0].generate_command(), } .as_bytes(), }); @@ -470,26 +808,14 @@ class SteeringHero .can_id = 0x1FE, .can_data = device::CanPacket8{ + chassis_steering_motors2_[0].generate_command(), device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, + chassis_steering_motors2_[1].generate_command(), 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(), @@ -500,16 +826,17 @@ class SteeringHero 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); + // can1_receive_rate_counter_.record(can_id); + if (can_id == 0x203) { + chassis_wheel_motors2_[1].store_status(data.can_data); + } else if (can_id == 0x204) { - chassis_wheel_motors_[3].store_status(data.can_data); + chassis_wheel_motors2_[0].store_status(data.can_data); + } 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); } else if (can_id == 0x300) { supercap_.store_status(data.can_data); } @@ -518,21 +845,15 @@ class SteeringHero 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) { - 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) { + // can2_receive_rate_counter_.record(can_id); + if (can_id == 0x141) { gimbal_bottom_yaw_motor_.store_status(data.can_data); } } + rclcpp::Logger logger_; + 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( @@ -553,20 +874,18 @@ class SteeringHero imu_.store_gyroscope_status(data.x, data.y, data.z); } + // CanReceiveRateCounter can1_receive_rate_counter_; + // CanReceiveRateCounter can2_receive_rate_counter_; device::Bmi088 imu_; - OutputInterface& tf_; - - OutputInterface chassis_yaw_velocity_imu_; OutputInterface powermeter_control_enabled_; OutputInterface powermeter_charge_power_limit_; 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}; @@ -577,10 +896,9 @@ class SteeringHero rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; - std::unique_ptr top_board_; - std::unique_ptr bottom_board_; - - rmcs_utility::TickTimer temperature_logging_timer_; + std::shared_ptr top_board_; + std::shared_ptr bottom_board_one_; + std::shared_ptr bottom_board_two_; }; } // namespace rmcs_core::hardware diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/hero.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/hero.cpp index 905744990..0bac8649f 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/hero.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/hero.cpp @@ -1,12 +1,16 @@ #include #include #include +#include #include #include #include #include +#include +#include #include +#include #include #include "referee/app/ui/shape/shape.hpp" @@ -25,18 +29,50 @@ class Hero get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , status_ring_(26.5, 26.5, 600, 40) - , rangefinder_() - , chassis_direction_indicator_(Shape::Color::PINK, 8, x_center, y_center, 0, 0, 84, 84) - , time_reminder_(Shape::Color::PINK, 50, 5, x_center + 150, y_center + 65, 0, false) { + // , chassis_direction_indicator_(Shape::Color::PINK, 8, x_center, y_center, 0, 0, 84, 84) + , chassis_left_wheel_indicator_( + Shape::Color::WHITE, wheel_indicator_width, x_center, y_center, 0, 0, + wheel_indicator_radius, wheel_indicator_radius) + , chassis_right_wheel_indicator_( + Shape::Color::WHITE, wheel_indicator_width, x_center, y_center, 0, 0, + wheel_indicator_radius, wheel_indicator_radius) + , yaw_angle_number_(Shape::Color::YELLOW, 20, 5, x_center + 270, y_center + 95, 0.0, false) + , pitch_angle_number_( + Shape::Color::YELLOW, 20, 5, x_center + 270, y_center - 35, 0.0, false) + , bottom_yaw_angle_number_( + Shape::Color::YELLOW, 20, 5, x_center + 270, y_center - 65, 0.0, false) + , time_reminder_(Shape::Color::PINK, 50, 5, x_center + 150, y_center + 65, 0, false) + // , bullet_allowance_label_( + // Shape::Color::YELLOW, 18, 3, x_center - 300, y_center + 270, "bullet", false) + // , bullet_allowance_number_( + // Shape::Color::YELLOW, 20, 5, x_center - 170, y_center + 270, 0, false) + , bullet_allowance_number_( + Shape::Color::YELLOW, 20, 5, x_center - 220, y_center + 270, 0, false) + , friction_profile_number_( + Shape::Color::GREEN, friction_profile_number_font_size, 5, 0, 0, 12, false) + , friction_profile_indicator_{Line(Shape::Color::WHITE, friction_profile_box_line_width, 0, 0, 0, 0, false), Line(Shape::Color::WHITE, friction_profile_box_line_width, 0, 0, 0, 0, false), Line(Shape::Color::WHITE, friction_profile_box_line_width, 0, 0, 0, 0, false), Line(Shape::Color::WHITE, friction_profile_box_line_width, 0, 0, 0, 0, true)} + , center_green_line_( + Shape::Color::GREEN, green_line_width, x_center - green_line_half_length, + y_center - green_line_offset_y, x_center + green_line_half_length, + y_center - green_line_offset_y, true) + , tracking_pink_line_( + Shape::Color::PINK, pink_line_width, x_center - pink_line_half_length, + y_center + pink_line_offset_y, x_center + pink_line_half_length, + y_center + pink_line_offset_y, false) { chassis_control_direction_indicator_.set_x(x_center); chassis_control_direction_indicator_.set_y(y_center); + register_input("/gimbal/mode", gimbal_mode_); + register_input("/remote/keyboard", keyboard_); register_input("/chassis/control_mode", chassis_mode_); register_input("/chassis/angle", chassis_angle_); register_input("/chassis/control_angle", chassis_control_angle_); + register_input("/chassis/climber/left_front_motor/velocity", left_track_velocity_); + register_input("/chassis/climber/right_front_motor/velocity", right_track_velocity_); + register_input("/chassis/supercap/voltage", supercap_voltage_); // register_input("/chassis/supercap/control_enable", supercap_control_enabled_); @@ -44,56 +80,192 @@ class Hero 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("/referee/shooter/42mm_bullet_allowance", robot_bullet_allowance_); + register_input("/gimbal/control_bullet_allowance/limited_by_heat", robot_bullet_allowance_); register_input( "/gimbal/first_left_friction/control_velocity", left_friction_control_velocity_); register_input("/gimbal/first_left_friction/velocity", left_friction_velocity_); register_input("/gimbal/first_right_friction/velocity", right_friction_velocity_); + register_input("/gimbal/friction_profile_1_active", friction_profile_1_active_, false); + // register_input("/gimbal/yaw/angle", gimbal_yaw_angle_); + register_input("/gimbal/player_viewer/raw_angle", gimbal_player_viewer_raw_angle_); register_input("/gimbal/pitch/angle", gimbal_pitch_angle_); - register_input("/gimbal/auto_aim/laser_distance", laser_distance_); + register_input("/gimbal/pitch/raw_angle", gimbal_pitch_raw_angle_); + register_input("/gimbal/bottom_yaw/angle", bottom_yaw_angle_); + register_input("/gimbal/bottom_yaw/raw_angle", bottom_yaw_raw_angle_); + // register_input("/gimbal/auto_aim/laser_distance", laser_distance_); + + register_input("/gimbal/shooter/condiction", shoot_condiction_); register_input("/gimbal/shooter/mode", shoot_mode_); - register_input("/gimbal/scope/active", is_scope_active_); + + // register_input("/gimbal/scope/active", is_scope_active_); register_input("/remote/mouse", mouse_); register_input("/referee/game/stage", game_stage_); + + // register_input("/gimbal/auto_aim/fire_control", auto_aim_fire_control_, false); + // register_input("/gimbal/auto_aim/target_confidence", auto_aim_target_confidence_, false); } void update() override { update_normal_ui(); - update_sniper_ui(); - - if (*is_scope_active_) { - set_normal_ui_visible(false); - rangefinder_.set_visible(true); - } else { - set_normal_ui_visible(true); - rangefinder_.set_visible(false); - } + // update_bullet_allowance(); + // update_sniper_ui(); + // update_state_word(); + + // if (*is_scope_active_) { + // set_normal_ui_visible(false); + // rangefinder_.set_visible(true); + // } else { + set_normal_ui_visible(true); + // rangefinder_.set_visible(false); + // } } private: + static uint16_t count_digits(int32_t value) { + uint16_t digits = value <= 0 ? 1 : 0; + while (value != 0) { + value /= 10; + ++digits; + } + return digits; + } void set_normal_ui_visible(bool value) { status_ring_.set_visible(value); - chassis_direction_indicator_.set_visible(value); - chassis_control_direction_indicator_.set_visible(value); + // chassis_direction_indicator_.set_visible(value); + chassis_left_wheel_indicator_.set_visible(value); + chassis_right_wheel_indicator_.set_visible(value); + // chassis_control_direction_indicator_.set_visible(value); + yaw_angle_number_.set_visible(value); + pitch_angle_number_.set_visible(value); + bottom_yaw_angle_number_.set_visible(value); + // bullet_allowance_label_.set_visible(value); + bullet_allowance_number_.set_visible(value); + friction_profile_number_.set_visible(value); + center_green_line_.set_visible(value); + tracking_pink_line_.set_visible(value); + + const bool show_friction_profile_box = + value && friction_profile_1_active_.ready() && *friction_profile_1_active_; + for (auto& line : friction_profile_indicator_) + line.set_visible(show_friction_profile_box); + if (!value) + chassis_control_direction_indicator_.set_visible(false); } void update_normal_ui() { update_chassis_direction_indicator(); - - status_ring_.update_bullet_allowance(*robot_bullet_allowance_); + yaw_angle_number_.set_value(static_cast(*gimbal_player_viewer_raw_angle_)); + pitch_angle_number_.set_value(static_cast(*gimbal_pitch_raw_angle_)); + update_pitch_raw_angle_color(); + bottom_yaw_angle_number_.set_value(static_cast(*bottom_yaw_raw_angle_)); + update_bottom_yaw_tracking_lines(); + const int32_t bullet_allowance = + static_cast(std::max(0, *robot_bullet_allowance_)); + bullet_allowance_number_.set_value(bullet_allowance); + + const uint16_t yaw_right = + yaw_raw_angle_x + + count_digits(static_cast(*gimbal_player_viewer_raw_angle_)) + * raw_angle_font_size; + const uint16_t pitch_right = + pitch_raw_angle_x + + count_digits(static_cast(*gimbal_pitch_raw_angle_)) * raw_angle_font_size; + const uint16_t box_left = std::max(yaw_right, pitch_right) + friction_profile_box_gap; + const uint16_t box_right = box_left + friction_profile_box_visual_width; + + const uint16_t box_top = yaw_raw_angle_y + raw_angle_font_size; + const uint16_t box_bottom = pitch_raw_angle_y; + + const bool friction_profile_1_active = + friction_profile_1_active_.ready() && *friction_profile_1_active_; + const uint16_t box_center_x = (box_left + box_right) / 2; + const uint16_t profile_number_y = box_top + friction_profile_number_gap; + + for (auto& line : friction_profile_indicator_) + line.set_color(Shape::Color::GREEN); + + friction_profile_number_.set_value(friction_profile_1_active ? 16 : 12); + friction_profile_number_.set_color( + friction_profile_1_active ? Shape::Color::PINK : Shape::Color::GREEN); + friction_profile_number_.set_font_size(friction_profile_number_font_size); + friction_profile_number_.set_xy(box_left, profile_number_y); + friction_profile_number_.set_center_x(box_center_x); + + friction_profile_indicator_[0].set_x(box_left); + friction_profile_indicator_[0].set_y(box_top); + friction_profile_indicator_[0].set_x2(box_right); + friction_profile_indicator_[0].set_y2(box_top); + + friction_profile_indicator_[1].set_x(box_right); + friction_profile_indicator_[1].set_y(box_top); + friction_profile_indicator_[1].set_x2(box_right); + friction_profile_indicator_[1].set_y2(box_bottom); + + friction_profile_indicator_[2].set_x(box_left); + friction_profile_indicator_[2].set_y(box_bottom); + friction_profile_indicator_[2].set_x2(box_right); + friction_profile_indicator_[2].set_y2(box_bottom); + + friction_profile_indicator_[3].set_x(box_left); + friction_profile_indicator_[3].set_y(box_top); + friction_profile_indicator_[3].set_x2(box_left); + friction_profile_indicator_[3].set_y2(box_bottom); 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_, true); status_ring_.update_battery_power(*chassis_voltage_); - update_static_status_ring(); + // const bool auto_aim_locked = auto_aim_fire_control_.ready() && *auto_aim_fire_control_; + // const double target_confidence_value = + // auto_aim_target_confidence_.ready() ? *auto_aim_target_confidence_ : 0.0; + + // status_ring_.update_auto_aim_feedback(auto_aim_locked, target_confidence_value); + // update_static_status_ring(); + last_keyboard_ = *keyboard_; + } + + void update_pitch_raw_angle_color() { + if (keyboard_.ready()) { + if (!last_keyboard_.e && keyboard_->e) { + last_e_triggered_with_ctrl_ = keyboard_->ctrl; + } + last_keyboard_ = *keyboard_; + } + + if (!gimbal_mode_.ready()) { + pitch_angle_number_.set_color(Shape::Color::YELLOW); + return; + } + + const bool is_encoder = *gimbal_mode_ == rmcs_msgs::GimbalMode::ENCODER; + const bool entering_encoder = + last_gimbal_mode_ != rmcs_msgs::GimbalMode::ENCODER && is_encoder; + const bool leaving_encoder = + last_gimbal_mode_ == rmcs_msgs::GimbalMode::ENCODER && !is_encoder; + + if (entering_encoder) { + pitch_encoder_by_ctrl_e_ = last_e_triggered_with_ctrl_; + } + + if (leaving_encoder) { + pitch_encoder_by_ctrl_e_ = false; + } + + if (!is_encoder) { + pitch_angle_number_.set_color(Shape::Color::YELLOW); + } else if (pitch_encoder_by_ctrl_e_) { + pitch_angle_number_.set_color(Shape::Color::PINK); + } else { + pitch_angle_number_.set_color(Shape::Color::GREEN); + } + + last_gimbal_mode_ = *gimbal_mode_; } void update_sniper_ui() { @@ -101,7 +273,7 @@ class Hero ? *gimbal_pitch_angle_ - 2 * std::numbers::pi : *gimbal_pitch_angle_; - rangefinder_.update_pitch_angle(-display_angle); + rangefinder_.update_pitch_angle(static_cast(*gimbal_pitch_raw_angle_)); double raw_height = -display_angle / 0.7 * static_cast(height_max); raw_height = std::clamp(raw_height, 0.0, static_cast(height_max)); @@ -109,6 +281,35 @@ class Hero lift_height = std::clamp(lift_height, height_min, height_max); rangefinder_.update_vertical_rangefinder(lift_height); + pitch_angle_number_.set_value(static_cast(*gimbal_pitch_raw_angle_)); + } + + void update_bottom_yaw_tracking_lines() { + const bool ctrl_e_pressed = keyboard_->ctrl && keyboard_->e; + if (ctrl_e_pressed) { + bottom_yaw_tracking_enabled_ = !bottom_yaw_tracking_enabled_; + bottom_yaw_anchor_angle_rad_ = *bottom_yaw_angle_; + } + + const double delta_bottom_yaw_rad = *bottom_yaw_angle_ - bottom_yaw_anchor_angle_rad_; + + const int min_center_x = static_cast(pink_line_half_length); + const int max_center_x = + static_cast(screen_width) - static_cast(pink_line_half_length); + + const int pink_center_x = std::clamp( + static_cast(std::lround( + static_cast(x_center) + + delta_bottom_yaw_rad * pink_line_pixels_per_radian)), + min_center_x, max_center_x); + + tracking_pink_line_.set_x( + static_cast(pink_center_x - static_cast(pink_line_half_length))); + tracking_pink_line_.set_y(y_center + pink_line_offset_y); + tracking_pink_line_.set_x2( + static_cast(pink_center_x + static_cast(pink_line_half_length))); + tracking_pink_line_.set_y2(y_center + pink_line_offset_y); + tracking_pink_line_.set_visible(true); } void update_time_reminder() { @@ -123,17 +324,78 @@ class Hero status_ring_.update_static_parts({auto_aim_enable, precise_enable}); } + // void update_bullet_allowance() { + + // std::string text = "BULLET : " + std::to_string(max(0,*robot_bullet_allowance_)); + // char* allow = text.data(); + // auto color = Shape::Color::YELLOW; + + // bullet_allowance_number_.set_value(allow); + // bullet_allowance_number_.set_font_size(14); + // bullet_allowance_number_.set_color(color); + // bullet_allowance_number_.set_visible(true); + // bullet_allowance_number_.set_xy(x_center - 240, y_center + 288); + // } + + void update_state_word() { + + const char* text = "OK"; + auto color = Shape::Color::GREEN; + + if (*shoot_condiction_ == rmcs_msgs::ShootCondiction::FRICTION_WAITING) { + text = " WAITING "; + } else if (*shoot_condiction_ == rmcs_msgs::ShootCondiction::SHOOT) { + text = " SHOOT "; + } else if (*shoot_condiction_ == rmcs_msgs::ShootCondiction::FIRED) { + text = " FIRED "; + } else if (*shoot_condiction_ == rmcs_msgs::ShootCondiction::JAM) { + text = " JAM "; + } else if (*shoot_condiction_ == rmcs_msgs::ShootCondiction::PRELOADING) { + text = "PRELOADING"; + } + + state_word_.set_value(text); + state_word_.set_font_size(30); + state_word_.set_color(color); + state_word_.set_visible(true); + state_word_.set_xy(x_center - 800, y_center + 200); + } + 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)); + // return static_cast( + // std::round((2 * std::numbers::pi - angle) / std::numbers::pi * 180)); + int degrees = static_cast( + std::lround((2.0 * std::numbers::pi - angle) / std::numbers::pi * 180.0)); + degrees %= 360; + if (degrees < 0) + degrees += 360; + return static_cast(degrees); }; - 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); + // 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); + const bool left_track_active = + std::abs(*left_track_velocity_) > track_velocity_active_threshold; + const bool right_track_active = + std::abs(*right_track_velocity_) > track_velocity_active_threshold; + + chassis_left_wheel_indicator_.set_color( + left_track_active ? Shape::Color::GREEN : Shape::Color::WHITE); + chassis_right_wheel_indicator_.set_color( + right_track_active ? Shape::Color::GREEN : Shape::Color::WHITE); + + const double wheel_offset = wheel_indicator_offset_deg * std::numbers::pi / 180.0; + const double left_wheel_angle = *chassis_angle_ + wheel_offset; + const double right_wheel_angle = *chassis_angle_ - wheel_offset; + + chassis_left_wheel_indicator_.set_angle( + to_referee_angle(left_wheel_angle), wheel_indicator_half_angle); + chassis_right_wheel_indicator_.set_angle( + to_referee_angle(right_wheel_angle), wheel_indicator_half_angle); bool chassis_control_direction_indicator_visible = false; if (!std::isnan(*chassis_control_angle_)) { @@ -160,10 +422,48 @@ class Hero static constexpr uint16_t screen_width = 1920, screen_height = 1080; static constexpr uint16_t x_center = screen_width / 2, y_center = screen_height / 2; + static constexpr uint16_t raw_angle_font_size = 20; + + static constexpr uint16_t yaw_raw_angle_x = x_center + 270; + static constexpr uint16_t yaw_raw_angle_y = y_center + 65; + + static constexpr uint16_t pitch_raw_angle_x = x_center + 270; + static constexpr uint16_t pitch_raw_angle_y = y_center - 65; + + static constexpr uint16_t friction_profile_box_gap = 18; + static constexpr uint16_t friction_profile_box_visual_width = 105; + static constexpr uint16_t friction_profile_box_line_width = 6; + static constexpr uint16_t friction_profile_number_font_size = 24; + static constexpr uint16_t friction_profile_number_gap = 12; + static constexpr uint16_t height_min = 0, height_max = 500; + static constexpr uint16_t wheel_indicator_radius = 110; + static constexpr uint16_t wheel_indicator_width = 12; + static constexpr uint16_t wheel_indicator_half_angle = 10; + static constexpr double wheel_indicator_offset_deg = 28.0; + static constexpr double track_velocity_active_threshold = 1.0; + static constexpr uint16_t green_line_half_length = 8; + static constexpr uint16_t green_line_width = 40; + static constexpr uint16_t green_line_offset_y = 0; + + static constexpr uint16_t pink_line_half_length = 40; + static constexpr uint16_t pink_line_width = 8; + static constexpr uint16_t pink_line_offset_y = 0; + + static constexpr double pink_line_pixels_per_radian = 400.0; + + InputInterface gimbal_mode_; + InputInterface keyboard_; + + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + rmcs_msgs::GimbalMode last_gimbal_mode_ = rmcs_msgs::GimbalMode::IMU; + + bool last_e_triggered_with_ctrl_ = false; + bool pitch_encoder_by_ctrl_e_ = false; InputInterface chassis_mode_; InputInterface chassis_angle_, chassis_control_angle_; + InputInterface left_track_velocity_, right_track_velocity_; InputInterface supercap_voltage_; InputInterface supercap_control_enabled_; @@ -173,33 +473,60 @@ class Hero InputInterface chassis_control_power_limit_; InputInterface supercap_charge_power_limit_; - InputInterface robot_bullet_allowance_; + InputInterface robot_bullet_allowance_; InputInterface left_friction_control_velocity_; InputInterface left_friction_velocity_; InputInterface right_friction_velocity_; + InputInterface friction_profile_1_active_; InputInterface mouse_; InputInterface game_stage_; + InputInterface gimbal_yaw_angle_; InputInterface gimbal_pitch_angle_; InputInterface gimbal_player_viewer_angle_; - InputInterface laser_distance_; + InputInterface gimbal_player_viewer_raw_angle_; + InputInterface gimbal_pitch_raw_angle_; + InputInterface bottom_yaw_raw_angle_; + InputInterface bottom_yaw_angle_; + // InputInterface laser_distance_; InputInterface shoot_mode_; - InputInterface is_scope_active_; + InputInterface shoot_condiction_; + // InputInterface is_scope_active_; StatusRing status_ring_; Rangefinder rangefinder_; - Arc chassis_direction_indicator_, chassis_control_direction_indicator_; + Arc chassis_left_wheel_indicator_; + Arc chassis_right_wheel_indicator_; + Arc chassis_control_direction_indicator_; + + Float yaw_angle_number_; + Float pitch_angle_number_; + Float bottom_yaw_angle_number_; + Text state_word_; Integer time_reminder_; + + // Text bullet_allowance_label_; + Integer bullet_allowance_number_; + Integer friction_profile_number_; + Line friction_profile_indicator_[4]; + Line center_green_line_; + Line tracking_pink_line_; + + bool bottom_yaw_tracking_enabled_ = false; + double bottom_yaw_anchor_angle_rad_ = 0.0; + + // InputInterface auto_aim_fire_control_; + // InputInterface auto_aim_target_confidence_; }; } // namespace rmcs_core::referee::app::ui #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::app::ui::Hero, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::app::ui::Hero, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp index 990d2ccf7..37be26aeb 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp @@ -87,6 +87,14 @@ class Shape set_modified(); } + void set_xy(uint16_t x, uint16_t y) { + if (part2_.x == x && part2_.y == y) + return; + part2_.x = x; + part2_.y = y; + set_modified(); + } + bool is_text_shape() const { return is_text_shape_; } enum class Operation : uint8_t { @@ -139,6 +147,9 @@ class Shape }; protected: + explicit Shape(bool is_text_shape = false) + : is_text_shape_(is_text_shape) {} + enum class ShapeType : uint8_t { LINE = 0, RECTANGLE = 1, @@ -173,7 +184,7 @@ class Shape }; void set_modified() { - // Optimization: Assume the modification not exist when invisible. + // Optimization: Assume the modification does not exist when invisible. if (!visible_) return; @@ -200,7 +211,7 @@ class Shape // Re-enter the update queue to try to get a new id. set_modified(); } else { - // Leave run_queue when shape was hidden. + // Leave run_queue when the shape is hidden. leave_run_queue(); } } @@ -210,10 +221,9 @@ class Shape // Called by CfsScheduler. if (!has_id() && !try_assign_id()) { - // TODO: Print error message. sync_confidence_ = max_update_times; visible_ = false; - // Do nothing when failed + // Do nothing when the update fails. return no_operation_description(); } @@ -224,8 +234,8 @@ class Shape command::Field field; - // Optimization1: Stop adding when shape is invisible. - // Optimization2: Prevent continuous modification. + // Optimization 1: Stop adding when the shape is invisible. + // Optimization 2: Prevent continuous modification. if (visible_ && (existence_confidence() <= sync_confidence_ || (last_time_modified_ && existence_confidence() < max_update_times))) { @@ -497,8 +507,7 @@ class Arc : public Shape { public: Arc() = default; Arc(Color color, uint16_t width, uint16_t x, uint16_t y, uint16_t angle_start, - uint16_t angle_end, uint16_t rx, uint16_t ry, bool visible = true) - : Arc() { + uint16_t angle_end, uint16_t rx, uint16_t ry, bool visible = true) { angle_start_ = angle_start; angle_end_ = angle_end; @@ -604,8 +613,7 @@ class Integer : public Shape { Integer() = default; Integer( Color color, uint16_t font_size, uint16_t width, uint16_t x, uint16_t y, int32_t value, - bool visible = true) - : Integer() { + bool visible = true) { color_ = color; font_size_ = font_size; @@ -714,7 +722,8 @@ class Float : public Integer { class Text : public Shape { public: - Text() { value_ = nullptr; }; + Text() + : Shape(true) {} Text( Color color, uint16_t font_size, uint16_t width, uint16_t x, uint16_t y, const char* value, bool visible = true) @@ -775,8 +784,8 @@ class Text : public Shape { uint16_t font_size_; Color color_; - const char* value_; + const char* value_ = nullptr; }; } // namespace app::ui -} // namespace rmcs_core::referee \ No newline at end of file +} // namespace rmcs_core::referee diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/shoot_condiction.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/shoot_condiction.hpp new file mode 100644 index 000000000..95dd40907 --- /dev/null +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/shoot_condiction.hpp @@ -0,0 +1,14 @@ +#pragma once + +#include + +namespace rmcs_msgs { + +enum class ShootCondiction : uint8_t { + FRICTION_WAITING = 0, + PRELOADING = 1, + SHOOT = 2, + JAM = 3, + FIRED = 4, +}; +} From 75e0c30f6094890fa1f7766ec32c4d3adaea2b6b Mon Sep 17 00:00:00 2001 From: dwx5 <1591215786@qq.com> Date: Thu, 4 Jun 2026 21:08:50 +0800 Subject: [PATCH 2/3] update after merge main --- .../config/steering-hero-little.yaml | 24 ++-- .../controller/gimbal/dual_yaw_controller.cpp | 69 ++++++------ .../gimbal/hero_gimbal_controller.cpp | 67 +++++++++++- .../src/controller/gimbal/player_viewer.cpp | 14 +-- .../controller/shooting/heat_controller.cpp | 19 +++- .../controller/shooting/putter_controller.cpp | 103 ++++++++++-------- .../src/hardware/steering-hero-little.cpp | 26 ++++- .../src/rmcs_core/src/referee/app/ui/hero.cpp | 31 ++++-- 8 files changed, 233 insertions(+), 120 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little.yaml index 539707670..822f9d969 100644 --- a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little.yaml @@ -36,7 +36,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 # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster # - sp_vision_25::bridge::HeroAutoAimBridge -> hero_auto_aim_bridge @@ -56,10 +56,10 @@ hero_hardware: viewer_motor_zero_point: 31940 external_imu_port: /dev/ttyUSB0 bullet_feeder_motor_zero_point: 60480 #39045 - left_front_zero_point: 5799 - right_front_zero_point: 3700 - left_back_zero_point: 7862 - right_back_zero_point: 1608 + left_front_zero_point: 5818 + right_front_zero_point: 3792 + left_back_zero_point: 7830 + right_back_zero_point: 1790 value_broadcaster: ros__parameters: @@ -102,14 +102,14 @@ value_broadcaster: # - /chassis/climber/front/power_demand_estimate # - /chassis/climber/front/actual_power_estimate # - /chassis/steering_wheel/actual_power_estimate - - /gimbal/bullet_feeder/torque - - /gimbal/bullet_feeder/control_torque + # - /gimbal/bullet_feeder/torque + # - /gimbal/bullet_feeder/control_torque - /gimbal/putter/angle - /gimbal/putter/velocity - /gimbal/putter/torque - /gimbal/putter/control_torque - - /gimbal/bullet_feeder/velocity - - /gimbal/bullet_feeder/angle + # - /gimbal/bullet_feeder/velocity + # - /gimbal/bullet_feeder/angle climber_controller: @@ -120,7 +120,7 @@ climber_controller: auto_climb_support_retract_velocity_slow: 20.0 auto_climb_approach_chassis_velocity: 2.0 auto_climb_support_deploy_chassis_velocity: 0.3 - auto_climb_support_retract_chassis_velocity: 0.3 + auto_climb_support_retract_chassis_velocity: 0.15 auto_climb_dash_chassis_velocity: 3.0 first_stair_dash_leveled_pitch_threshold: 0.05 second_stair_dash_leveled_pitch_threshold: -0.09 @@ -209,7 +209,7 @@ bullet_feeder_controller: bullet_feeder_angle_kp: 6.0 bullet_feeder_angle_ki: 0.0 bullet_feeder_angle_kd: 1.6 - putter_return_velocity_kp: 0.0025 + putter_return_velocity_kp: 0.003 putter_return_velocity_ki: 0.00005 putter_return_velocity_kd: 0.0 putter_return_velocity_integral_min: -0.03 @@ -291,7 +291,7 @@ steering_wheel_controller: ros__parameters: mess: 22.0 moment_of_inertia: 1.08 - vehicle_radius: 0.318198 + vehicle_radius: 0.286378 wheel_radius: 0.055 friction_coefficient: 0.6 k1: 2.958580e+00 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 0d49184e8..56596d598 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 @@ -49,11 +49,8 @@ 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_); - register_output("/gimbal/top_yaw/control_angle_shift", top_yaw_control_angle_shift_, nan_); register_output("/gimbal/bottom_yaw/control_angle", bottom_yaw_control_angle_, nan_); + register_output("/gimbal/top_yaw/control_angle_shift", top_yaw_control_angle_shift_, nan_); status_component_ = create_partner_component(get_component_name() + "_status"); @@ -68,34 +65,39 @@ class DualYawController } void update() override { - const auto mode = *gimbal_mode_; + if (mode == rmcs_msgs::GimbalMode::ENCODER) { const bool entering_encoder = last_gimbal_mode_ != rmcs_msgs::GimbalMode::ENCODER; + if (entering_encoder) { - if (std::isfinite(*top_yaw_angle_)) { - top_yaw_encoder_locked_ = true; + if (std::isfinite(*bottom_yaw_angle_)) { + bottom_yaw_encoder_angle_ = *bottom_yaw_angle_; + bottom_yaw_encoder_locked_ = true; + bottom_yaw_angle_pid_.reset(); + bottom_yaw_velocity_pid_.reset(); } else { - top_yaw_encoder_locked_ = false; + bottom_yaw_encoder_angle_ = nan_; + bottom_yaw_encoder_locked_ = false; } } - *bottom_yaw_control_angle_shift_ = *control_angle_shift_; - if (top_yaw_encoder_locked_) - *top_yaw_control_angle_ = top_yaw_encoder_angle_; - else - *top_yaw_control_angle_ = nan_; - } else { - *top_yaw_control_angle_ = nan_; - *bottom_yaw_control_angle_shift_ = nan_; - top_yaw_encoder_angle_ = *top_yaw_angle_; - top_yaw_encoder_locked_ = false; - } - - last_gimbal_mode_ = mode; - if (std::isnan(*control_angle_error_)) { *top_yaw_control_torque_ = nan_; - *bottom_yaw_control_torque_ = nan_; + *bottom_yaw_control_angle_ = nan_; + *top_yaw_control_angle_shift_ = *control_angle_shift_; + + if (bottom_yaw_encoder_locked_) { + double err = std::remainder( + bottom_yaw_encoder_angle_ - *bottom_yaw_angle_, 2.0 * std::numbers::pi); + + double target_velocity = bottom_yaw_angle_pid_.update(err); + *bottom_yaw_control_torque_ = + bottom_yaw_velocity_pid_.update(target_velocity - bottom_yaw_velocity_imu()); + } else { + *bottom_yaw_control_torque_ = nan_; + } + // *bottom_yaw_control_torque_ = nan_; + } else { *top_yaw_control_torque_ = top_yaw_velocity_pid_.update( top_yaw_angle_pid_.update(*control_angle_error_) - *gimbal_yaw_velocity_imu_); @@ -103,20 +105,18 @@ class DualYawController *bottom_yaw_control_torque_ = bottom_yaw_velocity_pid_.update( bottom_yaw_angle_pid_.update(bottom_yaw_control_error()) - bottom_yaw_velocity_imu()); + + *bottom_yaw_control_angle_ = nan_; + *top_yaw_control_angle_shift_ = nan_; + bottom_yaw_encoder_locked_ = false; } + + last_gimbal_mode_ = mode; } private: static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - double wrap_angle(double angle) { - while (angle > 0) - angle += 2 * std::numbers::pi; - while (angle >= 2 * std::numbers::pi) - angle -= 2 * std::numbers::pi; - return angle; - } - double bottom_yaw_control_error() { if (!std::isfinite(*top_yaw_angle_) || !std::isfinite(*control_angle_error_)) return nan_; @@ -147,15 +147,12 @@ class DualYawController OutputInterface top_yaw_control_torque_; OutputInterface bottom_yaw_control_torque_; - OutputInterface top_yaw_control_angle_; - OutputInterface bottom_yaw_control_angle_shift_; - OutputInterface bottom_yaw_control_angle_; OutputInterface top_yaw_control_angle_shift_; rmcs_msgs::GimbalMode last_gimbal_mode_ = rmcs_msgs::GimbalMode::IMU; - bool top_yaw_encoder_locked_ = false; - double top_yaw_encoder_angle_ = nan_; + bool bottom_yaw_encoder_locked_ = false; + double bottom_yaw_encoder_angle_ = nan_; class DualYawStatus : public rmcs_executor::Component { public: 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 939f17f6d..2cd9be969 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 @@ -40,6 +40,11 @@ class HeroGimbalController register_input("/gimbal/auto_aim/control_direction", auto_aim_control_direction_, false); register_input("/gimbal/pitch/angle", gimbal_pitch_angle_); register_input("/gimbal/pitch/raw_angle", gimbal_pitch_raw_angle_); + register_input("/gimbal/yaw_brake/angle", gimbal_yaw_brake_angle_); + register_input("/gimbal/yaw_brake/torque", gimbal_yaw_brake_torque_); + register_input("/gimbal/yaw_brake/velocity", gimbal_yaw_brake_velocity_); + // 锁住时复方向 + register_input("/tf", tf_); register_output("/gimbal/mode", gimbal_mode_, rmcs_msgs::GimbalMode::IMU); @@ -47,6 +52,7 @@ class HeroGimbalController register_output("/gimbal/pitch/control_angle_error", pitch_angle_error_, nan_); register_output("/gimbal/yaw/control_angle_shift", yaw_control_angle_shift_, nan_); register_output("/gimbal/pitch/control_angle", pitch_control_angle_, nan_); + register_output("/gimbal/yaw_brake/control_torque", yaw_brake_control_torque_, nan_); } void update() override { @@ -61,6 +67,7 @@ class HeroGimbalController break; } + // RCLCPP_INFO(get_logger(), "yaw_brake_torque: %f", *gimbal_yaw_brake_torque_); if (!last_keyboard_.e && keyboard_->e) { if (gimbal_mode_keyboard_ == GimbalMode::IMU) { encoder_init_pitch_ = keyboard_->ctrl ? kCtrlEInitPitch : kEInitPitch; @@ -70,11 +77,19 @@ class HeroGimbalController } } + bool switch_encoder_to_imu_by_c = false; + + if (!last_keyboard_.c && keyboard_->c && gimbal_mode_keyboard_ == GimbalMode::ENCODER) { + gimbal_mode_keyboard_ = GimbalMode::IMU; + switch_encoder_to_imu_by_c = true; + } + *gimbal_mode_ = gimbal_mode_keyboard_; - //*gimbal_mode_ = switch_right == Switch::UP ? GimbalMode::ENCODER : GimbalMode::IMU; + *gimbal_mode_ = switch_right == Switch::UP ? GimbalMode::ENCODER : GimbalMode::IMU; if (*gimbal_mode_ == GimbalMode::IMU) { - auto angle_error = update_imu_control(); + auto angle_error = switch_encoder_to_imu_by_c ? enter_imu_hold_current_pose() + : update_imu_control(); *yaw_angle_error_ = angle_error.yaw_angle_error; *pitch_angle_error_ = angle_error.pitch_angle_error; @@ -107,6 +122,25 @@ class HeroGimbalController *pitch_angle_error_ = nan_; *yaw_control_angle_shift_ = nan_; *pitch_control_angle_ = nan_; + *yaw_brake_control_torque_ = nan_; + + yaw_brake_count_ = 0; + yaw_brake_locked_ = false; + } + + void yaw_brake_update() { + if (std::abs(*yaw_brake_control_torque_) > 0.01 + && std::abs(*gimbal_yaw_brake_velocity_) < 0.1) { + yaw_brake_count_++; + } else { + yaw_brake_count_ = 0; + } + + if (yaw_brake_count_ > 50) { + yaw_brake_count_ = 0; + yaw_brake_locked_ = true; + *yaw_brake_control_torque_ = nan_; + } } TwoAxisGimbalSolver::AngleError update_imu_control() { @@ -133,6 +167,17 @@ class HeroGimbalController TwoAxisGimbalSolver::SetControlShift{yaw_shift, pitch_shift}); } + TwoAxisGimbalSolver::AngleError enter_imu_hold_current_pose() { + if (!tf_.ready()) + return update_imu_control(); + + auto current_direction = + fast_tf::cast(PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); + + return imu_gimbal_solver.update( + TwoAxisGimbalSolver::SetControlDirection{OdomImu::DirectionVector{*current_direction}}); + } + PreciseTwoAxisGimbalSolver::ControlAngle update_encoder_control() { if (!encoder_gimbal_solver.enabled()) { return encoder_gimbal_solver.update( @@ -148,6 +193,12 @@ class HeroGimbalController double pitch_shift = -joystick_sensitivity * joystick_left_->x() + mouse_pitch_sensitivity * mouse_velocity_->x(); + if (!yaw_brake_locked_) { + *yaw_brake_control_torque_ = -0.3; + } + + yaw_brake_update(); + return encoder_gimbal_solver.update( PreciseTwoAxisGimbalSolver::SetControlShift{yaw_shift, pitch_shift}); } @@ -155,8 +206,8 @@ class HeroGimbalController private: static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - static constexpr double kEInitPitch = -0.476972; // Initial angle for standalone E. - static constexpr double kCtrlEInitPitch = -0.541591; // Initial angle for Ctrl+E. + static constexpr double kEInitPitch = -0.346584; // Initial angle for standalone E. + static constexpr double kCtrlEInitPitch = -0.471795; // Initial angle for Ctrl+E. double encoder_init_pitch_ = kEInitPitch; InputInterface joystick_left_; InputInterface switch_right_; @@ -170,6 +221,10 @@ class HeroGimbalController InputInterface auto_aim_control_direction_; InputInterface gimbal_pitch_angle_; InputInterface gimbal_pitch_raw_angle_; + InputInterface gimbal_yaw_brake_angle_; + InputInterface gimbal_yaw_brake_torque_; + InputInterface gimbal_yaw_brake_velocity_; + InputInterface tf_; rmcs_msgs::GimbalMode gimbal_mode_keyboard_ = rmcs_msgs::GimbalMode::IMU; OutputInterface gimbal_mode_; @@ -180,6 +235,10 @@ class HeroGimbalController OutputInterface yaw_angle_error_, pitch_angle_error_; OutputInterface yaw_control_angle_shift_, pitch_control_angle_; + OutputInterface yaw_brake_control_torque_; + + int yaw_brake_count_ = 0; + bool yaw_brake_locked_ = false; }; } // namespace rmcs_core::controller::gimbal diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/player_viewer.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/player_viewer.cpp index d4d98715f..862f6369a 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/player_viewer.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/player_viewer.cpp @@ -54,16 +54,16 @@ class PlayerViewer || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { reset_all_controls(); } else { - if (!last_keyboard_.q && keyboard.q) { - scope_active_ = !scope_active_; - *is_scope_active_ = scope_active_; - scope_viewer_reset_ = scope_active_; - } if (!last_keyboard_.e && keyboard.e) { viewer_init_angle_ = keyboard.ctrl ? kCtrlInitViewerAngle : kEInitViewerAngle; viewer_reset_ = true; scope_viewer_reset_ = false; } + if (!last_keyboard_.q && keyboard.q) { + scope_active_ = !scope_active_; + *is_scope_active_ = scope_active_; + scope_viewer_reset_ = scope_active_; + } update_viewer_control(); }; @@ -106,7 +106,7 @@ class PlayerViewer *viewer_control_angle_ += *viewer_delta_angle_by_mouse_wheel_; } } - *viewer_control_angle_ = std::clamp(*viewer_control_angle_, upper_limit_, lower_limit_); + *viewer_control_angle_ = std::clamp(*viewer_control_angle_, lower_limit_, upper_limit_); auto norm_angle = [](double angle) { return (angle > pi_) ? angle - 2 * pi_ : angle; }; @@ -130,7 +130,7 @@ class PlayerViewer static constexpr double pi_ = std::numbers::pi; // The steering-hero viewer angle limit range is [0.68, 1.17]. - static constexpr double kEInitViewerAngle = 0.38905; // Move here when E is pressed. + static constexpr double kEInitViewerAngle = -0.065769; // Move here when E is pressed. static constexpr double kCtrlInitViewerAngle = 0.38905; // Move here when Ctrl is pressed. bool scope_viewer_reset_{false}; 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 3de0960ae..6036d592c 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 @@ -28,11 +28,21 @@ class HeatController void update() override { shooter_heat_ = std::max(0, shooter_heat_ - *shooter_cooling_); - if (*bullet_fired_) - shooter_heat_ += heat_per_shot + 10; + if (*bullet_fired_ && !bullet_fired_false_) { + shooter_heat_ += heat_per_shot; + } + bullet_fired_false_ = *bullet_fired_; + + if (++cooling_settlement_tick_ >= kCoolingSettlementTicks) { + cooling_settlement_tick_ = 0; + shooter_heat_ = std::max( + 0, shooter_heat_ - *shooter_cooling_ * kCoolingPerSettlementScale); + } *control_bullet_allowance_ = std::max( 0, (*shooter_heat_limit_ - shooter_heat_ - reserved_heat) / heat_per_shot); + + *shooting_heat_ = static_cast(shooter_heat_); } private: @@ -44,7 +54,12 @@ class HeatController const int64_t heat_per_shot; const int64_t reserved_heat; + int cooling_settlement_tick_ = 0; + static constexpr int kCoolingSettlementTicks = 100; + static constexpr int kCoolingPerSettlementScale = 100; + bool bullet_fired_false_ = false; int64_t shooter_heat_ = 0; + OutputInterface shooting_heat_; OutputInterface control_bullet_allowance_; }; 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 0b0c63755..5874a49a0 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 @@ -68,16 +68,6 @@ class PutterController set_pid_parameter(bullet_feeder_velocity_pid_, "bullet_feeder_velocity"); set_pid_parameter(putter_return_velocity_pid_, "putter_return_velocity"); - 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_); @@ -89,6 +79,7 @@ class PutterController register_output("/gimbal/shooter/mode", shoot_mode_, rmcs_msgs::ShootMode::SINGLE); register_output("/gimbal/shooter/condiction", shoot_condiction_); + register_output("/gimbal/shooter/preloaded_ready", preloaded_ready_, false); } ~PutterController() {} @@ -108,13 +99,13 @@ class PutterController } // Normal control flow after the putter has been initialized. - if (putter_is_initialized_) { + if (putter_initialized) { // Handling during bullet-feeder jam-protection cooldown. if (bullet_feeder_reverse_end_ > 0) { bullet_feeder_reverse_end_--; // Early cooldown stage: reverse the feeder to clear the jam. - if (bullet_feeder_reverse_end_ > 500) + if (bullet_feeder_reverse_end_ > 300) *bullet_feeder_control_torque_ = bullet_feeder_velocity_pid_.update( -low_latency_velocity_ / 2 - *bullet_feeder_velocity_); else { @@ -123,6 +114,14 @@ class PutterController *bullet_feeder_control_torque_ = 0.0; } + if (!bullet_feeder_reverse_end_ && shoot_stage_ == ShootStage::PRELOADED) + // RCLCPP_INFO(get_logger(), "Reverse finished"); + { + *preloaded_ready_ = true; + } else { + *preloaded_ready_ = false; + } + } else { // Normal operating mode: only fire when the friction wheels are ready. if (*friction_ready_) { @@ -145,6 +144,8 @@ class PutterController || (last_switch_left_ == rmcs_msgs::Switch::MIDDLE && switch_left == rmcs_msgs::Switch::DOWN); + // const bool auto_fire_now = (switch_right == Switch::UP) && + // (*fire_control_); const bool auto_fire_now = (switch_right == Switch::UP || (mouse.right && mouse.left)) && (*fire_control_); @@ -157,10 +158,10 @@ class PutterController if (manual_trigger || auto_trigger || auto_trigger_emergence) { if (*control_bullet_allowance_limited_by_heat_ > 0 - && (shoot_stage_ == ShootStage::PRELOADED || first_shot_)) { + && (shoot_stage_ == ShootStage::PRELOADED || shoot_first)) { set_shooting(); last_fire_time_ = now; - first_shot_ = false; + shoot_first = false; } } if (auto_trigger_emergence) { @@ -174,27 +175,31 @@ class PutterController low_latency_velocity_ - *bullet_feeder_velocity_); // Velocity loop. update_locked_detection(); - // This includes the photoelectric-sensor logic: if triggered, switch to // preloaded; otherwise reverse briefly and continue rotating. } if (shoot_stage_ == ShootStage::SHOOTING) { // Firing state: detect whether the bullet has been fired. - if (*bullet_fired_ - || *putter_angle_ - putter_start_point_ >= putter_stroke_) { - shot_fired_ = true; + if (*bullet_fired_ && !shooted) { + RCLCPP_INFO(get_logger(), "DETECT: Bullet fired!"); + shooted = true; + } + + if (*putter_angle_ - putter_startpoint >= putter_stroke_ && !shooted) { + RCLCPP_INFO(get_logger(), "DETECT: Putter stroke completed!"); + shooted = true; } update_putter_jam_detection(); - if (shot_fired_) { + if (shooted) { // Bullet fired: return the putter. - const auto angle_err = putter_start_point_ - *putter_angle_; + const auto angle_err = putter_startpoint - *putter_angle_; if (angle_err > -0.1) { *putter_control_torque_ = 0.; set_preloading(); - shot_fired_ = false; + shooted = false; } else { *putter_control_torque_ = putter_return_velocity_pid_.update(-80. - *putter_velocity_); @@ -240,11 +245,9 @@ class PutterController shoot_stage_ = ShootStage::PRELOADED; - putter_is_initialized_ = false; - putter_start_point_ = nan_; + putter_initialized = false; + putter_startpoint = nan_; putter_return_velocity_pid_.reset(); - putter_velocity_pid_.reset(); - putter_return_angle_pid.reset(); *putter_control_torque_ = nan_; bullet_feeder_faulty_count_ = 0; @@ -252,23 +255,31 @@ class PutterController *shoot_delay_ms_ = nan_; } - void set_preloading() { shoot_stage_ = ShootStage::PRELOADING; } + void set_preloading() { + RCLCPP_INFO(get_logger(), "PRELOADING"); + shoot_stage_ = ShootStage::PRELOADING; + } - void set_preloaded() { shoot_stage_ = ShootStage::PRELOADED; } + void set_preloaded() { + RCLCPP_INFO(get_logger(), "PRELOADED"); + shoot_stage_ = ShootStage::PRELOADED; + } - void set_shooting() { shoot_stage_ = ShootStage::SHOOTING; } + void set_shooting() { + RCLCPP_INFO(get_logger(), "SHOOTING"); + shoot_stage_ = ShootStage::SHOOTING; + } void update_locked_detection() { // If feeder speed is near zero and the photoelectric sensor is triggered, // treat it as locked and start reversing. - if (*bullet_feeder_velocity_ < 0.5 && *bullet_feeder_control_torque_ > 0.1) { locked_detect_count_++; } else { locked_detect_count_ = 0; } - if (locked_detect_count_ > 300) { + if (locked_detect_count_ > 150) { if (*photoelectric_sensor_status_) { set_preloaded(); } @@ -295,11 +306,12 @@ class PutterController if (shoot_stage_ != ShootStage::SHOOTING) { // Stall detected outside the firing state: the putter is in position, // so mark it initialized. - putter_is_initialized_ = true; - putter_start_point_ = *putter_angle_; + putter_initialized = true; + putter_startpoint = *putter_angle_; } else { // Stall detected during firing: treat the bullet as fired. - shot_fired_ = true; + RCLCPP_INFO(get_logger(), "DETECT: Putter jammed"); + shooted = true; } } } @@ -308,31 +320,32 @@ class PutterController // If the putter stays in the firing state too long without extending, // treat it as finished and move to the next state. if (shoot_stage_ == ShootStage::SHOOTING) { - if (shot_fired_) { + if (shooted) { if (putter_timeout_count_ < 1600) ++putter_timeout_count_; else { putter_timeout_count_ = 0; + RCLCPP_INFO(get_logger(), "PUTTER TIMEOUT"); set_preloading(); - shot_fired_ = false; + shooted = false; } } } } void enter_jam_protection() { - // Set the target angle to 60 degrees behind the current angle. locked_detect_count_ = 0; bullet_feeder_faulty_count_ = 0; - bullet_feeder_reverse_end_ = 800; + bullet_feeder_reverse_end_ = 400; bullet_feeder_velocity_pid_.reset(); + // RCLCPP_INFO(get_logger(), "Jammed!"); } static constexpr double nan_ = std::numeric_limits::quiet_NaN(); ///< Not-a-number constant. static constexpr double inf_ = std::numeric_limits::infinity(); ///< Infinity constant. - static constexpr double putter_stroke_ = 11.5; ///< Putter stroke length. + static constexpr double putter_stroke_ = 12.0; ///< Putter stroke length. static constexpr double max_bullet_feeder_control_torque_ = 0.1; static constexpr double bullet_feeder_angle_per_bullet_ = 2 * std::numbers::pi / 6; @@ -341,8 +354,8 @@ class PutterController InputInterface photoelectric_sensor_status_; InputInterface grayscale_sensor_status_; InputInterface bullet_fired_; - bool shot_fired_{false}; - bool first_shot_{true}; + bool shooted{false}; + bool shoot_first{true}; InputInterface friction_ready_; @@ -361,15 +374,13 @@ class PutterController InputInterface control_bullet_allowance_limited_by_heat_; - bool putter_is_initialized_ = false; + bool putter_initialized = false; int putter_faulty_count_ = 0; int putter_timeout_count_ = 0; - double putter_start_point_ = nan_; + double putter_startpoint = nan_; 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; @@ -378,7 +389,6 @@ class PutterController OutputInterface bullet_feeder_control_torque_; InputInterface putter_angle_; - pid::PidCalculator putter_return_angle_pid; OutputInterface putter_control_torque_; int bullet_feeder_faulty_count_ = 0; @@ -398,10 +408,11 @@ class PutterController OutputInterface shoot_mode_; OutputInterface shoot_condiction_; + OutputInterface preloaded_ready_; }; } // namespace rmcs_core::controller::shooting #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::shooting::PutterController, rmcs_executor::Component) +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/hardware/steering-hero-little.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little.cpp index 40150e605..698fa16bc 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little.cpp @@ -544,6 +544,7 @@ class SteeringHeroLittle , chassis_back_climber_motor_( {steering_hero, steering_hero_command, "/chassis/climber/left_back_motor"}, {steering_hero, steering_hero_command, "/chassis/climber/right_back_motor"}) + , yaw_brake_motor_(steering_hero, steering_hero_command, "/gimbal/yaw_brake") , gimbal_bottom_yaw_motor_(steering_hero, steering_hero_command, "/gimbal/bottom_yaw") { // chassis_steering_motors_[0].configure( @@ -603,6 +604,8 @@ class SteeringHeroLittle .enable_multi_turn_angle() .set_reduction_ratio(19.)); + yaw_brake_motor_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.set_reduction_ratio(1.)); gimbal_bottom_yaw_motor_.configure( device::LkMotor::Config{device::LkMotor::Type::kMG6012Ei8} .set_reversed() @@ -661,7 +664,18 @@ class SteeringHeroLittle for (auto& motor : chassis_steering_motors_) motor.update_status(); + yaw_brake_motor_.update_status(); gimbal_bottom_yaw_motor_.update_status(); + + if (++count_ == 500) { + for (int i = 0; i < 8; ++i) { + if (check[i] == 0) { + // RCLCPP_WARN(logger_, "can id 0x%03X missing", i + 0x201); + } + } + std::fill_n(check, 8, 0); + count_ = 0; + } } void command_update() { @@ -721,7 +735,7 @@ class SteeringHeroLittle device::CanPacket8{ device::CanPacket8::PaddingQuarter{}, chassis_back_climber_motor_[1].generate_command(), - device::CanPacket8::PaddingQuarter{}, + yaw_brake_motor_.generate_command(), chassis_back_climber_motor_[0].generate_command(), } .as_bytes(), @@ -751,6 +765,7 @@ class SteeringHeroLittle return; auto can_id = data.can_id; // can0_receive_rate_counter_.record(can_id); + check[can_id - 0x201] = 1; if (can_id == 0x201) { chassis_wheel_motors_[0].store_status(data.can_data); } else if (can_id == 0x202) { @@ -767,6 +782,9 @@ class SteeringHeroLittle return; auto can_id = data.can_id; // can1_receive_rate_counter_.record(can_id); + if (can_id != 0x300) { + check[can_id - 0x201] = 1; + } if (can_id == 0x203) { chassis_wheel_motors_[2].store_status(data.can_data); } else if (can_id == 0x204) { @@ -801,6 +819,8 @@ class SteeringHeroLittle chassis_back_climber_motor_[1].store_status(data.can_data); } else if (can_id == 0x204) { chassis_back_climber_motor_[0].store_status(data.can_data); + } else if (can_id == 0x203) { + yaw_brake_motor_.store_status(data.can_data); } else if (can_id == 0x141) { gimbal_bottom_yaw_motor_.store_status(data.can_data); } @@ -831,6 +851,9 @@ class SteeringHeroLittle // CanReceiveRateCounter can2_receive_rate_counter_; // CanReceiveRateCounter can3_receive_rate_counter_; + int count_ = 0; + int check[10] = {0}; + device::Bmi088 imu_; device::Dr16 dr16_; device::Supercap supercap_; @@ -839,6 +862,7 @@ class SteeringHeroLittle device::DjiMotor chassis_wheel_motors_[4]; device::DjiMotor chassis_front_climber_motor_[2]; device::DjiMotor chassis_back_climber_motor_[2]; + device::DjiMotor yaw_brake_motor_; device::LkMotor gimbal_bottom_yaw_motor_; rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/hero.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/hero.cpp index 0bac8649f..2ab4bd450 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/hero.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/hero.cpp @@ -51,14 +51,15 @@ class Hero , friction_profile_number_( Shape::Color::GREEN, friction_profile_number_font_size, 5, 0, 0, 12, false) , friction_profile_indicator_{Line(Shape::Color::WHITE, friction_profile_box_line_width, 0, 0, 0, 0, false), Line(Shape::Color::WHITE, friction_profile_box_line_width, 0, 0, 0, 0, false), Line(Shape::Color::WHITE, friction_profile_box_line_width, 0, 0, 0, 0, false), Line(Shape::Color::WHITE, friction_profile_box_line_width, 0, 0, 0, 0, true)} - , center_green_line_( - Shape::Color::GREEN, green_line_width, x_center - green_line_half_length, - y_center - green_line_offset_y, x_center + green_line_half_length, - y_center - green_line_offset_y, true) - , tracking_pink_line_( - Shape::Color::PINK, pink_line_width, x_center - pink_line_half_length, - y_center + pink_line_offset_y, x_center + pink_line_half_length, - y_center + pink_line_offset_y, false) { + // , center_green_line_( + // Shape::Color::GREEN, green_line_width, x_center - green_line_half_length, + // y_center - green_line_offset_y, x_center + green_line_half_length, + // y_center - green_line_offset_y, true) + // , tracking_pink_line_( + // Shape::Color::PINK, pink_line_width, x_center - pink_line_half_length, + // y_center + pink_line_offset_y, x_center + pink_line_half_length, + // y_center + pink_line_offset_y, false) + { chassis_control_direction_indicator_.set_x(x_center); chassis_control_direction_indicator_.set_y(y_center); @@ -99,6 +100,7 @@ class Hero register_input("/gimbal/shooter/condiction", shoot_condiction_); register_input("/gimbal/shooter/mode", shoot_mode_); + register_input("/gimbal/shooter/preloaded_ready", shooter_preloaded_ready_, false); // register_input("/gimbal/scope/active", is_scope_active_); @@ -147,8 +149,8 @@ class Hero // bullet_allowance_label_.set_visible(value); bullet_allowance_number_.set_visible(value); friction_profile_number_.set_visible(value); - center_green_line_.set_visible(value); - tracking_pink_line_.set_visible(value); + // center_green_line_.set_visible(value); + // tracking_pink_line_.set_visible(value); const bool show_friction_profile_box = value && friction_profile_1_active_.ready() && *friction_profile_1_active_; @@ -164,10 +166,14 @@ class Hero pitch_angle_number_.set_value(static_cast(*gimbal_pitch_raw_angle_)); update_pitch_raw_angle_color(); bottom_yaw_angle_number_.set_value(static_cast(*bottom_yaw_raw_angle_)); - update_bottom_yaw_tracking_lines(); + // update_bottom_yaw_tracking_lines(); const int32_t bullet_allowance = static_cast(std::max(0, *robot_bullet_allowance_)); bullet_allowance_number_.set_value(bullet_allowance); + const bool shooter_preloaded_ready = + shooter_preloaded_ready_.ready() && *shooter_preloaded_ready_; + bullet_allowance_number_.set_color( + shooter_preloaded_ready ? Shape::Color::GREEN : Shape::Color::PINK); const uint16_t yaw_right = yaw_raw_angle_x @@ -495,6 +501,7 @@ class Hero InputInterface shoot_mode_; InputInterface shoot_condiction_; + InputInterface shooter_preloaded_ready_; // InputInterface is_scope_active_; StatusRing status_ring_; @@ -529,4 +536,4 @@ class Hero #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::app::ui::Hero, rmcs_executor::Component) +PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::app::ui::Hero, rmcs_executor::Component) \ No newline at end of file From 178cc51ab665bf83f2354a075f78d546fc38e8eb Mon Sep 17 00:00:00 2001 From: dwx5 <1591215786@qq.com> Date: Sat, 6 Jun 2026 20:11:57 +0800 Subject: [PATCH 3/3] add yaw brake --- .../controller/gimbal/dual_yaw_controller.cpp | 248 ++++++++++++++++-- .../gimbal/hero_gimbal_controller.cpp | 39 +-- 2 files changed, 224 insertions(+), 63 deletions(-) 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 56596d598..84f8c0862 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 @@ -1,5 +1,6 @@ #include +#include #include #include @@ -39,18 +40,22 @@ class DualYawController register_input("/gimbal/top_yaw/velocity", top_yaw_velocity_); register_input("/gimbal/bottom_yaw/angle", bottom_yaw_angle_); register_input("/gimbal/bottom_yaw/velocity", bottom_yaw_velocity_); + register_input("/gimbal/bottom_yaw/raw_angle", bottom_yaw_raw_angle_); register_input("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); register_input("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_); + register_input("/gimbal/yaw_brake/velocity", yaw_brake_velocity_); register_input("/gimbal/mode", gimbal_mode_); register_input("/gimbal/yaw/control_angle_error", control_angle_error_); register_input("/gimbal/yaw/control_angle_shift", control_angle_shift_, false); + register_input("/gimbal/bottom_yaw/torque", bottom_yaw_torque_); 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/bottom_yaw/control_angle", bottom_yaw_control_angle_, nan_); register_output("/gimbal/top_yaw/control_angle_shift", top_yaw_control_angle_shift_, nan_); + register_output("/gimbal/yaw_brake/control_torque", yaw_brake_control_torque_, nan_); status_component_ = create_partner_component(get_component_name() + "_status"); @@ -67,38 +72,203 @@ class DualYawController void update() override { const auto mode = *gimbal_mode_; - if (mode == rmcs_msgs::GimbalMode::ENCODER) { - const bool entering_encoder = last_gimbal_mode_ != rmcs_msgs::GimbalMode::ENCODER; + const bool entering_encoder = mode == rmcs_msgs::GimbalMode::ENCODER + && last_gimbal_mode_ != rmcs_msgs::GimbalMode::ENCODER; - if (entering_encoder) { - if (std::isfinite(*bottom_yaw_angle_)) { - bottom_yaw_encoder_angle_ = *bottom_yaw_angle_; - bottom_yaw_encoder_locked_ = true; - bottom_yaw_angle_pid_.reset(); - bottom_yaw_velocity_pid_.reset(); - } else { - bottom_yaw_encoder_angle_ = nan_; - bottom_yaw_encoder_locked_ = false; - } + const bool leaving_encoder = mode != rmcs_msgs::GimbalMode::ENCODER + && last_gimbal_mode_ == rmcs_msgs::GimbalMode::ENCODER; + + if (entering_encoder) { + top_yaw_angle_pid_.reset(); + top_yaw_velocity_pid_.reset(); + bottom_yaw_angle_pid_.reset(); + bottom_yaw_velocity_pid_.reset(); + + yaw_brake_engage_count_ = 0; + yaw_brake_release_elapsed_count_ = 0; + yaw_brake_release_stop_count_ = 0; + yaw_brake_release_seen_motion_ = false; + encoder_state_ = EncoderState::AlignTargetRawAngle; + } + + if (leaving_encoder) { + top_yaw_angle_pid_.reset(); + top_yaw_velocity_pid_.reset(); + bottom_yaw_angle_pid_.reset(); + bottom_yaw_velocity_pid_.reset(); + + if (encoder_state_ == EncoderState::EngageYawBrake + || encoder_state_ == EncoderState::BrakeLocked) { + yaw_brake_release_elapsed_count_ = 0; + yaw_brake_release_stop_count_ = 0; + yaw_brake_release_seen_motion_ = false; + encoder_state_ = EncoderState::ReleaseYawBrake; + } else { + yaw_brake_engage_count_ = 0; + yaw_brake_release_elapsed_count_ = 0; + yaw_brake_release_stop_count_ = 0; + yaw_brake_release_seen_motion_ = false; + *yaw_brake_control_torque_ = nan_; + encoder_state_ = EncoderState::Idle; } + } + if (mode == rmcs_msgs::GimbalMode::ENCODER && encoder_state_ == EncoderState::Idle) { + top_yaw_angle_pid_.reset(); + top_yaw_velocity_pid_.reset(); + bottom_yaw_angle_pid_.reset(); + bottom_yaw_velocity_pid_.reset(); + + yaw_brake_engage_count_ = 0; + yaw_brake_release_elapsed_count_ = 0; + yaw_brake_release_stop_count_ = 0; + yaw_brake_release_seen_motion_ = false; + encoder_state_ = EncoderState::AlignTargetRawAngle; + } + // RCLCPP_INFO(get_logger(), "bottom_yaw_raw_angle: %ld", *bottom_yaw_raw_angle_); + // RCLCPP_INFO(get_logger(), "bottom_yaw_control_torque: %f", *bottom_yaw_control_torque_); + // RCLCPP_INFO(get_logger(), "bottom_yaw_torque: %f", *bottom_yaw_torque_); + // RCLCPP_INFO( + // get_logger(), "encoder_state: %s", + // encoder_state_ == EncoderState::Idle ? "Idle" + // : encoder_state_ == EncoderState::AlignTargetRawAngle ? "AlignTargetRawAngle" + // : encoder_state_ == EncoderState::EngageYawBrake ? "EngageYawBrake" + // : encoder_state_ == EncoderState::BrakeLocked ? "BrakeLocked" + // : "ReleaseYawBrake"); + const bool hold_encoder_for_brake_release = encoder_state_ == EncoderState::ReleaseYawBrake; + + if (mode == rmcs_msgs::GimbalMode::ENCODER || hold_encoder_for_brake_release) { *top_yaw_control_torque_ = nan_; *bottom_yaw_control_angle_ = nan_; - *top_yaw_control_angle_shift_ = *control_angle_shift_; - if (bottom_yaw_encoder_locked_) { - double err = std::remainder( - bottom_yaw_encoder_angle_ - *bottom_yaw_angle_, 2.0 * std::numbers::pi); + auto wrap_raw_delta = [](int64_t diff) -> int64_t { + diff %= kBottomYawRawAngleModulus; + if (diff <= -(kBottomYawRawAngleModulus / 2)) + diff += kBottomYawRawAngleModulus; + else if (diff > (kBottomYawRawAngleModulus / 2)) + diff -= kBottomYawRawAngleModulus; + return diff; + }; + + if (encoder_state_ == EncoderState::AlignTargetRawAngle) { + *top_yaw_control_angle_shift_ = 0.0; + *yaw_brake_control_torque_ = nan_; + + if (!bottom_yaw_raw_angle_.ready()) { + *bottom_yaw_control_torque_ = nan_; + } else { + const int64_t raw_error_count = + wrap_raw_delta(kEncoderBottomYawTargetRawAngle - *bottom_yaw_raw_angle_); + const int64_t raw_error_abs = + raw_error_count >= 0 ? raw_error_count : -raw_error_count; + + const bool bottom_yaw_aligned = + raw_error_abs <= kEncoderBottomYawTargetToleranceRawAngle + && std::abs(*bottom_yaw_velocity_) + <= kEncoderBottomYawLockVelocityThreshold; + + if (bottom_yaw_aligned) { + *bottom_yaw_control_torque_ = nan_; + bottom_yaw_angle_pid_.reset(); + bottom_yaw_velocity_pid_.reset(); + yaw_brake_engage_count_ = 0; + encoder_state_ = EncoderState::EngageYawBrake; + } else { + const double raw_error_angle = + kEncoderBottomYawRawAngleErrorSign + * static_cast(raw_error_count) + / static_cast(kBottomYawRawAngleModulus) * 2.0 + * std::numbers::pi; + + const double target_velocity = + bottom_yaw_angle_pid_.update(raw_error_angle); + const double velocity_error = target_velocity - *bottom_yaw_velocity_; + double control_torque = bottom_yaw_velocity_pid_.update(velocity_error); + + constexpr double kBreakawayTorque = 2.5; + constexpr double kBreakawayVelocityThreshold = 0.10; + constexpr int64_t kBreakawayErrorThreshold = 80; + + if (std::abs(*bottom_yaw_velocity_) < kBreakawayVelocityThreshold + && std::abs(raw_error_count) > kBreakawayErrorThreshold + && std::abs(control_torque) < kBreakawayTorque) { + control_torque = + velocity_error >= 0.0 ? kBreakawayTorque : -kBreakawayTorque; + } + + *bottom_yaw_control_torque_ = control_torque; + } + } + + } else if (encoder_state_ == EncoderState::EngageYawBrake) { + *top_yaw_control_angle_shift_ = 0.0; + *bottom_yaw_control_torque_ = nan_; + *yaw_brake_control_torque_ = kYawBrakeEngageTorque; + + if (std::abs(*yaw_brake_velocity_) < kYawBrakeEngageVelocityThreshold) { + ++yaw_brake_engage_count_; + } else { + yaw_brake_engage_count_ = 0; + } + + if (yaw_brake_engage_count_ >= kYawBrakeEngageConfirmCount) { + yaw_brake_engage_count_ = 0; + *yaw_brake_control_torque_ = nan_; + encoder_state_ = EncoderState::BrakeLocked; + } + + } else if (encoder_state_ == EncoderState::BrakeLocked) { + constexpr double kBrakeLockedHoldTorque = 0.6; + *bottom_yaw_control_torque_ = kBrakeLockedHoldTorque; + *yaw_brake_control_torque_ = nan_; + *top_yaw_control_angle_shift_ = *control_angle_shift_; + + } else if (encoder_state_ == EncoderState::ReleaseYawBrake) { + *top_yaw_control_angle_shift_ = 0.0; + *bottom_yaw_control_torque_ = nan_; + *yaw_brake_control_torque_ = kYawBrakeReleaseTorque; + + ++yaw_brake_release_elapsed_count_; + + if (std::abs(*yaw_brake_velocity_) > kYawBrakeReleaseMotionVelocityThreshold) { + yaw_brake_release_seen_motion_ = true; + } + + if (yaw_brake_release_seen_motion_ + && std::abs(*yaw_brake_velocity_) < kYawBrakeReleaseStopVelocityThreshold) { + ++yaw_brake_release_stop_count_; + } else { + yaw_brake_release_stop_count_ = 0; + } + + if (yaw_brake_release_stop_count_ >= kYawBrakeReleaseStopConfirmCount) { + yaw_brake_engage_count_ = 0; + yaw_brake_release_elapsed_count_ = 0; + yaw_brake_release_stop_count_ = 0; + yaw_brake_release_seen_motion_ = false; + *yaw_brake_control_torque_ = nan_; + encoder_state_ = EncoderState::Idle; + } else if (yaw_brake_release_elapsed_count_ >= kYawBrakeReleaseTimeoutCount) { + RCLCPP_WARN( + get_logger(), "Yaw brake release timed out, stop applying reverse torque " + "for protection."); + yaw_brake_engage_count_ = 0; + yaw_brake_release_elapsed_count_ = 0; + yaw_brake_release_stop_count_ = 0; + yaw_brake_release_seen_motion_ = false; + *yaw_brake_control_torque_ = nan_; + encoder_state_ = EncoderState::Idle; + } - double target_velocity = bottom_yaw_angle_pid_.update(err); - *bottom_yaw_control_torque_ = - bottom_yaw_velocity_pid_.update(target_velocity - bottom_yaw_velocity_imu()); } else { + *top_yaw_control_angle_shift_ = 0.0; *bottom_yaw_control_torque_ = nan_; + *yaw_brake_control_torque_ = nan_; } - // *bottom_yaw_control_torque_ = nan_; } else { + *yaw_brake_control_torque_ = nan_; + *top_yaw_control_torque_ = top_yaw_velocity_pid_.update( top_yaw_angle_pid_.update(*control_angle_error_) - *gimbal_yaw_velocity_imu_); @@ -108,20 +278,41 @@ class DualYawController *bottom_yaw_control_angle_ = nan_; *top_yaw_control_angle_shift_ = nan_; - bottom_yaw_encoder_locked_ = false; } last_gimbal_mode_ = mode; } private: + enum class EncoderState { + Idle, + AlignTargetRawAngle, + EngageYawBrake, + BrakeLocked, + ReleaseYawBrake + }; static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + static constexpr int64_t kBottomYawRawAngleModulus = 1 << 16; + static constexpr int64_t kEncoderBottomYawTargetRawAngle = 2434; + static constexpr int64_t kEncoderBottomYawTargetToleranceRawAngle = 80; + static constexpr double kEncoderBottomYawLockVelocityThreshold = 0.15; + static constexpr double kEncoderBottomYawRawAngleErrorSign = -1.0; + + static constexpr double kYawBrakeEngageTorque = -0.3; + static constexpr double kYawBrakeEngageVelocityThreshold = 0.1; + static constexpr int kYawBrakeEngageConfirmCount = 50; + + static constexpr double kYawBrakeReleaseTorque = 0.3; + static constexpr double kYawBrakeReleaseMotionVelocityThreshold = 0.2; + static constexpr double kYawBrakeReleaseStopVelocityThreshold = 0.08; + static constexpr int kYawBrakeReleaseStopConfirmCount = 20; + static constexpr int kYawBrakeReleaseTimeoutCount = 400; + double bottom_yaw_control_error() { if (!std::isfinite(*top_yaw_angle_) || !std::isfinite(*control_angle_error_)) return nan_; - // Avoid relying on top_yaw_angle in [0, 2pi) and control_angle_error in [-pi, pi]. constexpr double alignment = 2 * std::numbers::pi; double err = std::fmod(*top_yaw_angle_ + *control_angle_error_ + std::numbers::pi, alignment); @@ -135,24 +326,31 @@ class DualYawController InputInterface top_yaw_angle_, top_yaw_velocity_; InputInterface bottom_yaw_angle_, bottom_yaw_velocity_; + InputInterface bottom_yaw_raw_angle_; InputInterface gimbal_yaw_velocity_imu_, chassis_yaw_velocity_imu_; + InputInterface yaw_brake_velocity_; InputInterface gimbal_mode_; InputInterface control_angle_error_, control_angle_shift_; + InputInterface bottom_yaw_torque_; pid::PidCalculator top_yaw_angle_pid_, top_yaw_velocity_pid_; pid::PidCalculator bottom_yaw_angle_pid_, bottom_yaw_velocity_pid_; OutputInterface top_yaw_control_torque_; OutputInterface bottom_yaw_control_torque_; - OutputInterface bottom_yaw_control_angle_; OutputInterface top_yaw_control_angle_shift_; + OutputInterface yaw_brake_control_torque_; rmcs_msgs::GimbalMode last_gimbal_mode_ = rmcs_msgs::GimbalMode::IMU; - bool bottom_yaw_encoder_locked_ = false; - double bottom_yaw_encoder_angle_ = nan_; + EncoderState encoder_state_ = EncoderState::Idle; + + int yaw_brake_engage_count_ = 0; + int yaw_brake_release_elapsed_count_ = 0; + int yaw_brake_release_stop_count_ = 0; + bool yaw_brake_release_seen_motion_ = false; class DualYawStatus : public rmcs_executor::Component { public: 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 2cd9be969..6dcffe1c8 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 @@ -40,10 +40,6 @@ class HeroGimbalController register_input("/gimbal/auto_aim/control_direction", auto_aim_control_direction_, false); register_input("/gimbal/pitch/angle", gimbal_pitch_angle_); register_input("/gimbal/pitch/raw_angle", gimbal_pitch_raw_angle_); - register_input("/gimbal/yaw_brake/angle", gimbal_yaw_brake_angle_); - register_input("/gimbal/yaw_brake/torque", gimbal_yaw_brake_torque_); - register_input("/gimbal/yaw_brake/velocity", gimbal_yaw_brake_velocity_); - // 锁住时复方向 register_input("/tf", tf_); register_output("/gimbal/mode", gimbal_mode_, rmcs_msgs::GimbalMode::IMU); @@ -52,7 +48,6 @@ class HeroGimbalController register_output("/gimbal/pitch/control_angle_error", pitch_angle_error_, nan_); register_output("/gimbal/yaw/control_angle_shift", yaw_control_angle_shift_, nan_); register_output("/gimbal/pitch/control_angle", pitch_control_angle_, nan_); - register_output("/gimbal/yaw_brake/control_torque", yaw_brake_control_torque_, nan_); } void update() override { @@ -67,7 +62,6 @@ class HeroGimbalController break; } - // RCLCPP_INFO(get_logger(), "yaw_brake_torque: %f", *gimbal_yaw_brake_torque_); if (!last_keyboard_.e && keyboard_->e) { if (gimbal_mode_keyboard_ == GimbalMode::IMU) { encoder_init_pitch_ = keyboard_->ctrl ? kCtrlEInitPitch : kEInitPitch; @@ -122,25 +116,6 @@ class HeroGimbalController *pitch_angle_error_ = nan_; *yaw_control_angle_shift_ = nan_; *pitch_control_angle_ = nan_; - *yaw_brake_control_torque_ = nan_; - - yaw_brake_count_ = 0; - yaw_brake_locked_ = false; - } - - void yaw_brake_update() { - if (std::abs(*yaw_brake_control_torque_) > 0.01 - && std::abs(*gimbal_yaw_brake_velocity_) < 0.1) { - yaw_brake_count_++; - } else { - yaw_brake_count_ = 0; - } - - if (yaw_brake_count_ > 50) { - yaw_brake_count_ = 0; - yaw_brake_locked_ = true; - *yaw_brake_control_torque_ = nan_; - } } TwoAxisGimbalSolver::AngleError update_imu_control() { @@ -193,12 +168,6 @@ class HeroGimbalController double pitch_shift = -joystick_sensitivity * joystick_left_->x() + mouse_pitch_sensitivity * mouse_velocity_->x(); - if (!yaw_brake_locked_) { - *yaw_brake_control_torque_ = -0.3; - } - - yaw_brake_update(); - return encoder_gimbal_solver.update( PreciseTwoAxisGimbalSolver::SetControlShift{yaw_shift, pitch_shift}); } @@ -208,6 +177,7 @@ class HeroGimbalController static constexpr double kEInitPitch = -0.346584; // Initial angle for standalone E. static constexpr double kCtrlEInitPitch = -0.471795; // Initial angle for Ctrl+E. + double encoder_init_pitch_ = kEInitPitch; InputInterface joystick_left_; InputInterface switch_right_; @@ -221,9 +191,6 @@ class HeroGimbalController InputInterface auto_aim_control_direction_; InputInterface gimbal_pitch_angle_; InputInterface gimbal_pitch_raw_angle_; - InputInterface gimbal_yaw_brake_angle_; - InputInterface gimbal_yaw_brake_torque_; - InputInterface gimbal_yaw_brake_velocity_; InputInterface tf_; rmcs_msgs::GimbalMode gimbal_mode_keyboard_ = rmcs_msgs::GimbalMode::IMU; @@ -235,10 +202,6 @@ class HeroGimbalController OutputInterface yaw_angle_error_, pitch_angle_error_; OutputInterface yaw_control_angle_shift_, pitch_control_angle_; - OutputInterface yaw_brake_control_torque_; - - int yaw_brake_count_ = 0; - bool yaw_brake_locked_ = false; }; } // namespace rmcs_core::controller::gimbal