From 482954861d36b5a268c9fa93c11a0ea13c8dbe8d Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Thu, 13 Aug 2026 15:04:28 +0800 Subject: [PATCH] merge/deformable-infantry --- rmcs_ws/src/hikcamera | 1 + .../rmcs_bringup/config/auto_aim_test.yaml | 11 +- .../config/deformable-infantry-omni-b.yaml | 109 ++- .../config/deformable-infantry-omni-c.yaml | 369 +++++++++ .../config/deformable-infantry-omni.yaml | 93 ++- rmcs_ws/src/rmcs_bringup/config/sentry.yaml | 2 +- rmcs_ws/src/rmcs_core/plugins.xml | 1 + .../controller/chassis/chassis_controller.cpp | 1 + .../controller/chassis/deformable_chassis.cpp | 176 +++- .../controller/chassis/deformable_mode.hpp | 179 +++- .../chassis/deformable_suspension.cpp | 6 +- .../chassis/hero_chassis_controller.cpp | 1 + .../deformable_infantry_gimbal_controller.cpp | 75 +- .../gimbal/two_axis_gimbal_solver.hpp | 10 + .../src/controller/pid/pid_calculator.hpp | 1 + .../shooting/friction_wheel_controller.cpp | 100 ++- .../hardware/deformable-infantry-omni-b.cpp | 152 +--- .../hardware/deformable-infantry-omni-c.cpp | 773 ++++++++++++++++++ .../src/hardware/deformable-infantry-omni.cpp | 519 +++++------- .../src/hardware/device/bmi088_ekf.hpp | 2 +- .../static_torque_test_controller.cpp | 76 +- .../referee/app/ui/deformable_infantry_ui.cpp | 1 + rmcs_ws/src/rmcs_core/src/referee/status.cpp | 31 + .../rmcs_core/src/referee/status/field.hpp | 7 + .../include/rmcs_msgs/chassis_mode.hpp | 1 + .../rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp | 1 + 26 files changed, 2047 insertions(+), 651 deletions(-) create mode 160000 rmcs_ws/src/hikcamera create mode 100644 rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-c.cpp diff --git a/rmcs_ws/src/hikcamera b/rmcs_ws/src/hikcamera new file mode 160000 index 000000000..f0077f034 --- /dev/null +++ b/rmcs_ws/src/hikcamera @@ -0,0 +1 @@ +Subproject commit f0077f034800bcd0dde4fffeff270b733772a57e diff --git a/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml b/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml index 7f812fa52..db9e59e70 100644 --- a/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml @@ -2,14 +2,14 @@ rmcs_executor: ros__parameters: update_rate: 1000.0 components: - # - rmcs::AutoAimPlayerComponent -> auto_aim_player - - rmcs::AutoAimVideoPlayerComponent -> auto_aim_video_player + - rmcs::AutoAimPlayerComponent -> auto_aim_player + # - rmcs::AutoAimVideoPlayerComponent -> auto_aim_video_player # - rmcs::AutoAimRecorderComponent -> auto_aim_recorder - rmcs::AutoAimComponent -> auto_aim_component auto_aim_player: ros__parameters: - input_path: "/workspaces/data/autoaim/robot/blue_fast_track/" + input_path: "/workspaces/RMCS/develop_ws/record/26uc-train/2026-08-03_20-51-11" loop_play: true auto_aim_video_player: @@ -29,10 +29,10 @@ auto_aim_recorder: auto_aim_component: ros__parameters: - dangerous_fallback: "red" + dangerous_fallback: "blue" manual_shoot: false enable_rune: true - track_ids: [HERO, ENGINEER, INFANTRY_3, INFANTRY_4, SENTRY, OUTPOST] + track_ids: [HERO, ENGINEER, INFANTRY_3, INFANTRY_4, SENTRY, OUTPOST, RUNE] camera_translation: [0., 0., 0.] fire_control: @@ -50,3 +50,4 @@ auto_aim_component: pitch_tolerance: 0.04 rune_idle_duration: 0.4 rune_shoot_duration: 0.2 + # is_lazy_gimbal: false diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index aa952607c..6da631fc8 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -30,10 +30,13 @@ rmcs_executor: - rmcs_core::controller::chassis::DeformableJointController -> rb_joint_controller - rmcs_core::controller::chassis::DeformableJointController -> rf_joint_controller + - rmcs::AutoAimRecorderComponent -> auto_aim_recorder - rmcs::AutoAimComponent -> auto_aim_component - rmcs::AutoAimCapturerComponent -> auto_aim_capturer - rmcs_core::referee::app::ui::AutoAimUi -> auto_aim_ui + # - rmcs_core::debug::GimbalValueCollector -> gimbal_value_collector + # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster # - rmcs_core::debug::ValueCollector -> value_collector @@ -50,17 +53,32 @@ value_collector: write_interval: 5 flush_interval: 1000 +gimbal_value_collector: + ros__parameters: + csv_path: "/tmp/gimbal_ff_.csv" + write_interval: 1 + flush_interval: 1000 + value_broadcaster: ros__parameters: forward_list: - - /gimbal/pitch/angle - - /gimbal/pitch/velocity + - /gimbal/yaw/angle + - /gimbal/yaw/velocity + +auto_aim_recorder: + ros__parameters: + output_path: "/autoaim/recoder" + queue_depth: 16 + flush_every_n_frames: 64 + max_duration_seconds: 0 + max_videos_size_gb: 200.0 + auto_record: false auto_aim_capturer: ros__parameters: camera_name: "" - exposure_us: 4000.0 - gain: 8.0 + exposure_us: 2000.0 + gain: 10.0 framerate: 120.0 invert_image: false rls_tau_sec: 10.0 @@ -73,23 +91,23 @@ auto_aim_component: # 将强行绑定为对应阵营哨兵，仅供调试，严禁比赛启用。 # 留空或填 unknow 表示禁用。 dangerous_fallback: "" - manual_shoot: true + manual_shoot: false enable_rune: true camera_translation: [0.058, -0.08, 0.0] fire_control: bullet_speed: 22.5 - shoot_delay: 0.04 - offset_yaw: +2.5 - offset_pitch: +0.5 - attack_window: 80.0 + shoot_delay: 0.07 + offset_yaw: +2.6 + offset_pitch: -0.3 + attack_window: 120.0 degraded_angle_speed: 12.0 - window_redundancy: 0.8 + window_redundancy: 0.6 window_hysteresis: 0.2 attack_preaim: false require_stable_command: false - yaw_tolerance: 0.07 - pitch_tolerance: 0.04 - rune_idle_duration: 0.4 + yaw_tolerance: 0.14 + pitch_tolerance: 0.08 + rune_idle_duration: 0.6 rune_shoot_duration: 0.2 auto_aim_ui: @@ -106,26 +124,23 @@ deformable_infantry: rod_length: 0.140 yaw_motor_zero_point: 57900 pitch_motor_zero_point: 56354 - debug_log_supercap: false - debug_log_wheel_motor: false - debug_log_deformable_joint_motor: false chassis_controller: ros__parameters: - # Deploy geometry / chassis-owned joint intent - min_angle: 5.0 + min_angle: 8.0 max_angle: 59.0 active_suspension_enable: true - spin_ratio: 1.0 + wireless_charging_offset_deg: 225.0 + wireless_charging_speed_limit: 0.6 + wireless_charging_angular_velocity_limit: 10.0 deformable_suspension: ros__parameters: - # IMU attitude correction at min-angle stance. active_suspension_pitch_outer_kp: 12.0 active_suspension_pitch_outer_ki: 0.02 active_suspension_pitch_outer_kd: 0.0 - active_suspension_pitch_outer_integral_min: -2.0 - active_suspension_pitch_outer_integral_max: 2.0 + active_suspension_pitch_outer_integral_min: -1.0 + active_suspension_pitch_outer_integral_max: 1.0 active_suspension_pitch_outer_output_min: -3.0 active_suspension_pitch_outer_output_max: 3.0 @@ -140,8 +155,8 @@ deformable_suspension: active_suspension_roll_outer_kp: 12.0 active_suspension_roll_outer_ki: 0.02 active_suspension_roll_outer_kd: 0.0 - active_suspension_roll_outer_integral_min: -2.0 - active_suspension_roll_outer_integral_max: 2.0 + active_suspension_roll_outer_integral_min: -1.0 + active_suspension_roll_outer_integral_max: 1.0 active_suspension_roll_outer_output_min: -3.0 active_suspension_roll_outer_output_max: 3.0 @@ -153,42 +168,53 @@ deformable_suspension: active_suspension_roll_inner_output_min: -0.785 active_suspension_roll_inner_output_max: 0.785 - # Chassis-owned joint intent trajectory limits while attitude correction is active. - active_suspension_target_velocity_limit_deg: 80.0 - active_suspension_target_acceleration_limit_deg: 360.0 + active_suspension_target_velocity_limit_deg: 150.0 + active_suspension_target_acceleration_limit_deg: 600.0 active_suspension_correction_velocity_limit_deg: 720.0 active_suspension_correction_acceleration_limit_deg: 3600.0 active_suspension_rate_lpf_cutoff_hz: 10.0 - # Automatic IMU mounting-error calibration. - # When all four requested joint targets stay equal for 2s, average pitch/roll from 2s to 5s. + active_suspension_target_pitch_deg: 0.0 + chassis_imu_calibration_wait_s: 2.0 chassis_imu_calibration_sample_s: 3.0 gimbal_controller: ros__parameters: - upper_limit: -0.47123 # -27 deg - lower_limit: 0.10 # 8 deg + upper_limit: -0.60 + lower_limit: 0.10 ctrl_hold_pitch_target_angle: 0.0 - yaw_angle_kp: 15.0 + yaw_angle_kp: 10.0 yaw_angle_ki: 0.0 yaw_angle_kd: 0.0 - yaw_velocity_kp: 15.0 - yaw_velocity_ki: 0.0 + yaw_velocity_kp: 13.0 + yaw_velocity_ki: 0.02 yaw_velocity_kd: 0.0 + yaw_velocity_integral_min: -5.0 + yaw_velocity_integral_max: 5.0 pitch_angle_kp: 35.0 pitch_angle_ki: 0.02 - pitch_angle_kd: 0.3 + pitch_angle_kd: 0.0 + pitch_angle_integral_min: -0.5 + pitch_angle_integral_max: 0.5 - pitch_velocity_kp: 2.0 + pitch_velocity_kp: 1.8 pitch_velocity_ki: 0.0 pitch_velocity_kd: 0.0 - pitch_gravity_ff_gain: 4.302 - pitch_gravity_ff_phase: 0.589 + yaw_ref_velocity_gain: 1.0 + pitch_ref_velocity_gain: 1.0 + + yaw_velocity_ff_gain: 0.13 + yaw_acceleration_ff_gain: 0.18 + pitch_velocity_ff_gain: 0.378 + pitch_acceleration_ff_gain: 0.0396 + + pitch_gravity_ff_gain: 2.438 + pitch_gravity_ff_phase: 1.7254 pitch_torque_control: true @@ -200,6 +226,9 @@ friction_wheel_controller: friction_velocities: - 580.0 - 580.0 + friction_velocities_low_mode: + - 530.0 + - 530.0 friction_soft_start_stop_time: 1.0 heat_controller: @@ -210,7 +239,7 @@ heat_controller: bullet_feeder_controller: ros__parameters: bullets_per_feeder_turn: 8.0 - shot_frequency: 30.0 + shot_frequency: 15.0 safe_shot_frequency: 10.0 eject_frequency: 10.0 eject_time: 0.05 @@ -241,7 +270,7 @@ bullet_feeder_velocity_pid_controller: measurement: /gimbal/bullet_feeder/velocity setpoint: /gimbal/bullet_feeder/control_velocity control: /gimbal/bullet_feeder/control_torque - kp: 1.5 + kp: 0.9 ki: 0.0 kd: 0.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml new file mode 100644 index 000000000..a0f5dbcd3 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-c.yaml @@ -0,0 +1,369 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::DeformableInfantryOmniC -> deformable_infantry + + - 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::DeformableInfantry -> referee_ui_infantry + + - rmcs_core::controller::gimbal::DeformableInfantryGimbalController -> gimbal_controller + + - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller + - rmcs_core::controller::shooting::HeatController -> heat_controller + - rmcs_core::controller::shooting::BulletFeederController17mm -> bullet_feeder_controller + - rmcs_core::controller::pid::PidController -> left_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> right_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller + + - rmcs_core::controller::chassis::DeformableChassis -> chassis_controller + - rmcs_core::controller::chassis::DeformableSuspension -> deformable_suspension + - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller + - rmcs_core::controller::chassis::DeformableOmniWheelController -> deformable_chassis_controller + + - rmcs_core::controller::chassis::DeformableJointController -> lf_joint_controller + - rmcs_core::controller::chassis::DeformableJointController -> lb_joint_controller + - rmcs_core::controller::chassis::DeformableJointController -> rb_joint_controller + - rmcs_core::controller::chassis::DeformableJointController -> rf_joint_controller + + - rmcs::AutoAimRecorderComponent -> auto_aim_recorder + - rmcs::AutoAimComponent -> auto_aim_component + - rmcs::AutoAimCapturerComponent -> auto_aim_capturer + - rmcs_core::referee::app::ui::AutoAimUi -> auto_aim_ui + + # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + # - rmcs_core::debug::ValueCollector -> value_collector + +value_collector: + ros__parameters: + csv_path: "/tmp/pitch_.csv" + signals: + - /gimbal/pitch/angle + - /gimbal/pitch/velocity + - /gimbal/pitch/velocity_imu + - /gimbal/pitch/angle_error + - /gimbal/pitch/control_torque + - /gimbal/pitch/control_velocity + write_interval: 5 + flush_interval: 1000 + +value_broadcaster: + ros__parameters: + forward_list: + - /gimbal/pitch/angle + - /gimbal/pitch/velocity + +auto_aim_recorder: + ros__parameters: + output_path: "/autoaim/recoder" + queue_depth: 16 + flush_every_n_frames: 64 + max_duration_seconds: 0 + max_videos_size_gb: 200.0 + auto_record: false + +auto_aim_capturer: + ros__parameters: + camera_name: "" + exposure_us: 2000.0 + gain: 10.0 + framerate: 120.0 + invert_image: false + rls_tau_sec: 10.0 + use_hardware_sync: false + delay_ms: 6.5 + +auto_aim_component: + ros__parameters: + # WARN: 危险！可选 red | blue，生效后裁判系统 ID 缺席时 + # 将强行绑定为对应阵营哨兵，仅供调试，严禁比赛启用。 + # 留空或填 unknow 表示禁用。 + dangerous_fallback: "" + manual_shoot: false + enable_rune: true + camera_translation: [0.058, -0.08, 0.0] + fire_control: + bullet_speed: 22.5 + shoot_delay: 0.07 + offset_yaw: +0.8 + offset_pitch: -1.4 + attack_window: 120.0 + degraded_angle_speed: 12.0 + window_redundancy: 0.6 + window_hysteresis: 0.2 + attack_preaim: false + require_stable_command: false + yaw_tolerance: 0.21 + pitch_tolerance: 0.12 + rune_idle_duration: 0.6 + rune_shoot_duration: 0.2 + +auto_aim_ui: + ros__parameters: + offset_x: 0.0 + offset_y: -0.08 + offset_z: 0.0 + +deformable_infantry: + ros__parameters: + serial_filter_bottom_board: "AF-C1C3-DFE8-40A8-FC4B-B853-6ED7-AC9F-1DED" + serial_filter_top_board: "AF-ABAC-786D-1B53-99F6-00A2-42A6-AA95-9D69" + chassis_radius: 0.2341741 + rod_length: 0.140 + yaw_motor_zero_point: 38910 + pitch_motor_zero_point: 32214 + +chassis_controller: + ros__parameters: + min_angle: 8.0 + max_angle: 59.0 + active_suspension_enable: true + wireless_charging_offset_deg: 135.0 + wireless_charging_speed_limit: 0.6 + wireless_charging_angular_velocity_limit: 10.0 + +deformable_suspension: + ros__parameters: + active_suspension_pitch_outer_kp: 12.0 + active_suspension_pitch_outer_ki: 0.02 + active_suspension_pitch_outer_kd: 0.0 + active_suspension_pitch_outer_integral_min: -1.0 + active_suspension_pitch_outer_integral_max: 1.0 + active_suspension_pitch_outer_output_min: -3.0 + active_suspension_pitch_outer_output_max: 3.0 + + active_suspension_pitch_inner_kp: 0.45 + active_suspension_pitch_inner_ki: 0.0 + active_suspension_pitch_inner_kd: 0.0 + active_suspension_pitch_inner_integral_min: -1.0 + active_suspension_pitch_inner_integral_max: 1.0 + active_suspension_pitch_inner_output_min: -0.785 + active_suspension_pitch_inner_output_max: 0.785 + + active_suspension_roll_outer_kp: 12.0 + active_suspension_roll_outer_ki: 0.02 + active_suspension_roll_outer_kd: 0.0 + active_suspension_roll_outer_integral_min: -1.0 + active_suspension_roll_outer_integral_max: 1.0 + active_suspension_roll_outer_output_min: -3.0 + active_suspension_roll_outer_output_max: 3.0 + + active_suspension_roll_inner_kp: 0.45 + active_suspension_roll_inner_ki: 0.0 + active_suspension_roll_inner_kd: 0.0 + active_suspension_roll_inner_integral_min: -1.0 + active_suspension_roll_inner_integral_max: 1.0 + active_suspension_roll_inner_output_min: -0.785 + active_suspension_roll_inner_output_max: 0.785 + + active_suspension_target_velocity_limit_deg: 150.0 + active_suspension_target_acceleration_limit_deg: 600.0 + active_suspension_correction_velocity_limit_deg: 720.0 + active_suspension_correction_acceleration_limit_deg: 3600.0 + active_suspension_rate_lpf_cutoff_hz: 10.0 + + active_suspension_target_pitch_deg: 0.0 + + chassis_imu_calibration_wait_s: 2.0 + chassis_imu_calibration_sample_s: 3.0 + +gimbal_controller: + ros__parameters: + upper_limit: -0.60 # -27 deg + lower_limit: 0.10 # 8 deg + ctrl_hold_pitch_target_angle: 0.0 + + yaw_angle_kp: 10.0 + yaw_angle_ki: 0.0 + yaw_angle_kd: 0.0 + + yaw_velocity_kp: 13.0 + yaw_velocity_ki: 0.02 + yaw_velocity_kd: 0.0 + yaw_velocity_integral_min: -5.0 + yaw_velocity_integral_max: 5.0 + + pitch_angle_kp: 35.0 + pitch_angle_ki: 0.02 + pitch_angle_kd: 0.3 + pitch_angle_integral_min: -0.5 + pitch_angle_integral_max: 0.5 + + pitch_velocity_kp: 2.0 + pitch_velocity_ki: 0.0 + pitch_velocity_kd: 0.0 + + yaw_ref_velocity_gain: 1.0 + pitch_ref_velocity_gain: 1.0 + + yaw_velocity_ff_gain: 0.13 + yaw_acceleration_ff_gain: 0.18 + pitch_velocity_ff_gain: 0.378 + pitch_acceleration_ff_gain: 0.0396 + + pitch_gravity_ff_gain: 2.575 + pitch_gravity_ff_phase: 1.784 + + pitch_torque_control: true + +friction_wheel_controller: + ros__parameters: + friction_wheels: + - /gimbal/left_friction + - /gimbal/right_friction + friction_velocities: + - 580.0 + - 580.0 + friction_velocities_low_mode: + - 530.0 + - 530.0 + friction_soft_start_stop_time: 1.0 + +heat_controller: + ros__parameters: + heat_per_shot: 10000 + reserved_heat: 15000 + +bullet_feeder_controller: + ros__parameters: + bullets_per_feeder_turn: 8.0 + shot_frequency: 15.0 + safe_shot_frequency: 10.0 + eject_frequency: 10.0 + eject_time: 0.05 + deep_eject_frequency: 5.0 + deep_eject_time: 0.2 + single_shot_max_stop_delay: 2.0 + +left_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/left_friction/velocity + setpoint: /gimbal/left_friction/control_velocity + control: /gimbal/left_friction/control_torque + kp: 0.003436926 + ki: 0.00 + kd: 0.009373434 + +right_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/right_friction/velocity + setpoint: /gimbal/right_friction/control_velocity + control: /gimbal/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: 0.9 + ki: 0.0 + kd: 0.0 + +deformable_chassis_controller: + ros__parameters: + mass: 25.5 + moment_of_inertia: 1.0 + wheel_radius: 0.075 + friction_coefficient: 6.6 + k1: 2.958580e+00 + k2: 3.082190e-03 + no_load_power: 11.37 + +lf_joint_controller: + ros__parameters: + measurement_angle: /chassis/left_front_joint/physical_angle + setpoint_angle: /chassis/left_front_joint/target_physical_angle + setpoint_velocity: /chassis/left_front_joint/target_physical_velocity + control: /chassis/left_front_joint/control_torque + dt: 0.001 + b0: -1.0 + kt: 1.0 + td_h: 0.001 + td_r: 50.0 + eso_w0: 250.0 + eso_auto_beta: true + k1: 30.0 + k2: 17.0 + alpha1: 0.75 + alpha2: 0.7 + delta: 0.02 + u_min: -200.0 + u_max: 200.0 + output_min: -200.0 + output_max: 200.0 + +lb_joint_controller: + ros__parameters: + measurement_angle: /chassis/left_back_joint/physical_angle + setpoint_angle: /chassis/left_back_joint/target_physical_angle + setpoint_velocity: /chassis/left_back_joint/target_physical_velocity + control: /chassis/left_back_joint/control_torque + dt: 0.001 + b0: -1.0 + kt: 1.0 + td_h: 0.001 + td_r: 50.0 + eso_w0: 250.0 + eso_auto_beta: true + k1: 30.0 + k2: 17.0 + alpha1: 0.75 + alpha2: 0.7 + delta: 0.02 + u_min: -200.0 + u_max: 200.0 + output_min: -200.0 + output_max: 200.0 + +rb_joint_controller: + ros__parameters: + measurement_angle: /chassis/right_back_joint/physical_angle + setpoint_angle: /chassis/right_back_joint/target_physical_angle + setpoint_velocity: /chassis/right_back_joint/target_physical_velocity + control: /chassis/right_back_joint/control_torque + dt: 0.001 + b0: -1.0 + kt: 1.0 + td_h: 0.001 + td_r: 50.0 + eso_w0: 250.0 + eso_auto_beta: true + k1: 30.0 + k2: 17.0 + alpha1: 0.75 + alpha2: 0.7 + delta: 0.02 + u_min: -200.0 + u_max: 200.0 + output_min: -200.0 + output_max: 200.0 + +rf_joint_controller: + ros__parameters: + measurement_angle: /chassis/right_front_joint/physical_angle + setpoint_angle: /chassis/right_front_joint/target_physical_angle + setpoint_velocity: /chassis/right_front_joint/target_physical_velocity + control: /chassis/right_front_joint/control_torque + dt: 0.001 + b0: -1.0 + kt: 1.0 + td_h: 0.001 + td_r: 50.0 + eso_w0: 250.0 + eso_auto_beta: true + k1: 30.0 + k2: 17.0 + alpha1: 0.75 + alpha2: 0.7 + delta: 0.02 + u_min: -200.0 + u_max: 200.0 + output_min: -200.0 + output_max: 200.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 272d4f091..8ebccfd87 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -30,6 +30,7 @@ rmcs_executor: - rmcs_core::controller::chassis::DeformableJointController -> rb_joint_controller - rmcs_core::controller::chassis::DeformableJointController -> rf_joint_controller + - rmcs::AutoAimRecorderComponent -> auto_aim_recorder - rmcs::AutoAimComponent -> auto_aim_component - rmcs::AutoAimCapturerComponent -> auto_aim_capturer - rmcs_core::referee::app::ui::AutoAimUi -> auto_aim_ui @@ -56,15 +57,24 @@ value_broadcaster: - /gimbal/pitch/angle - /gimbal/pitch/velocity +auto_aim_recorder: + ros__parameters: + output_path: "/autoaim/recoder" + queue_depth: 16 + flush_every_n_frames: 64 + max_duration_seconds: 0 + max_videos_size_gb: 200.0 + auto_record: false + auto_aim_capturer: ros__parameters: camera_name: "" - exposure_us: 4000.0 - gain: 8.0 + exposure_us: 2000.0 + gain: 10.0 framerate: 120.0 invert_image: false rls_tau_sec: 10.0 - use_hardware_sync: true + use_hardware_sync: false delay_ms: 6.5 auto_aim_component: @@ -73,23 +83,23 @@ auto_aim_component: # 将强行绑定为对应阵营哨兵，仅供调试，严禁比赛启用。 # 留空或填 unknow 表示禁用。 dangerous_fallback: "" - manual_shoot: true + manual_shoot: false enable_rune: true camera_translation: [0.058, -0.08, 0.0] fire_control: bullet_speed: 22.5 - shoot_delay: 0.04 - offset_yaw: +0.1 - offset_pitch: -0.4 - attack_window: 80.0 + shoot_delay: 0.07 + offset_yaw: +0.2 + offset_pitch: -1.2 + attack_window: 120.0 degraded_angle_speed: 12.0 - window_redundancy: 0.8 + window_redundancy: 0.6 window_hysteresis: 0.2 attack_preaim: false require_stable_command: false - yaw_tolerance: 0.07 - pitch_tolerance: 0.04 - rune_idle_duration: 0.4 + yaw_tolerance: 0.14 + pitch_tolerance: 0.08 + rune_idle_duration: 0.7 rune_shoot_duration: 0.2 auto_aim_ui: @@ -106,26 +116,23 @@ deformable_infantry: rod_length: 0.140 yaw_motor_zero_point: 43365 pitch_motor_zero_point: 6432 - debug_log_supercap: false - debug_log_wheel_motor: false - debug_log_deformable_joint_motor: false chassis_controller: ros__parameters: - # Deploy geometry / chassis-owned joint intent - min_angle: 5.0 + min_angle: 8.0 max_angle: 59.0 active_suspension_enable: true - spin_ratio: 1.0 + wireless_charging_offset_deg: 45.0 + wireless_charging_speed_limit: 0.6 + wireless_charging_angular_velocity_limit: 10.0 deformable_suspension: ros__parameters: - # IMU attitude correction at min-angle stance. active_suspension_pitch_outer_kp: 12.0 active_suspension_pitch_outer_ki: 0.02 active_suspension_pitch_outer_kd: 0.0 - active_suspension_pitch_outer_integral_min: -2.0 - active_suspension_pitch_outer_integral_max: 2.0 + active_suspension_pitch_outer_integral_min: -1.0 + active_suspension_pitch_outer_integral_max: 1.0 active_suspension_pitch_outer_output_min: -3.0 active_suspension_pitch_outer_output_max: 3.0 @@ -140,8 +147,8 @@ deformable_suspension: active_suspension_roll_outer_kp: 12.0 active_suspension_roll_outer_ki: 0.02 active_suspension_roll_outer_kd: 0.0 - active_suspension_roll_outer_integral_min: -2.0 - active_suspension_roll_outer_integral_max: 2.0 + active_suspension_roll_outer_integral_min: -1.0 + active_suspension_roll_outer_integral_max: 1.0 active_suspension_roll_outer_output_min: -3.0 active_suspension_roll_outer_output_max: 3.0 @@ -153,21 +160,20 @@ deformable_suspension: active_suspension_roll_inner_output_min: -0.785 active_suspension_roll_inner_output_max: 0.785 - # Chassis-owned joint intent trajectory limits while attitude correction is active. - active_suspension_target_velocity_limit_deg: 80.0 - active_suspension_target_acceleration_limit_deg: 360.0 + active_suspension_target_velocity_limit_deg: 150.0 + active_suspension_target_acceleration_limit_deg: 600.0 active_suspension_correction_velocity_limit_deg: 720.0 active_suspension_correction_acceleration_limit_deg: 3600.0 active_suspension_rate_lpf_cutoff_hz: 10.0 - # Automatic IMU mounting-error calibration. - # When all four requested joint targets stay equal for 2s, average pitch/roll from 2s to 5s. + active_suspension_target_pitch_deg: 0.0 + chassis_imu_calibration_wait_s: 2.0 chassis_imu_calibration_sample_s: 3.0 gimbal_controller: ros__parameters: - upper_limit: -0.47123 # -27 deg + upper_limit: -0.60 # -27 deg lower_limit: 0.10 # 8 deg ctrl_hold_pitch_target_angle: 0.0 @@ -175,20 +181,32 @@ gimbal_controller: yaw_angle_ki: 0.0 yaw_angle_kd: 0.0 - yaw_velocity_kp: 10.0 - yaw_velocity_ki: 0.0 + yaw_velocity_kp: 13.0 + yaw_velocity_ki: 0.02 yaw_velocity_kd: 0.0 + yaw_velocity_integral_min: -5.0 + yaw_velocity_integral_max: 5.0 pitch_angle_kp: 35.0 - pitch_angle_ki: 0.02 - pitch_angle_kd: 0.3 + pitch_angle_ki: 0.03 + pitch_angle_kd: 0.2 + pitch_angle_integral_min: -0.5 + pitch_angle_integral_max: 0.5 pitch_velocity_kp: 2.0 pitch_velocity_ki: 0.0 pitch_velocity_kd: 0.0 - pitch_gravity_ff_gain: 4.302 - pitch_gravity_ff_phase: 0.589 + yaw_ref_velocity_gain: 1.0 + pitch_ref_velocity_gain: 1.0 + + yaw_velocity_ff_gain: 0.20 + yaw_acceleration_ff_gain: 0.17 + pitch_velocity_ff_gain: 0.356 + pitch_acceleration_ff_gain: 0.041 + + pitch_gravity_ff_gain: 2.575 + pitch_gravity_ff_phase: 1.7814 pitch_torque_control: true @@ -200,6 +218,9 @@ friction_wheel_controller: friction_velocities: - 580.0 - 580.0 + friction_velocities_low_mode: + - 530.0 + - 530.0 friction_soft_start_stop_time: 1.0 heat_controller: @@ -210,7 +231,7 @@ heat_controller: bullet_feeder_controller: ros__parameters: bullets_per_feeder_turn: 8.0 - shot_frequency: 30.0 + shot_frequency: 15.0 safe_shot_frequency: 10.0 eject_frequency: 10.0 eject_time: 0.05 diff --git a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml index c4c0bd3aa..3f8d323e9 100644 --- a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml @@ -62,7 +62,7 @@ auto_aim_component: manual_shoot: true enable_rune: true # track_ids: [OUTPOST, BASE] - track_ids: [HERO, ENGINEER, INFANTRY_3, INFANTRY_4, SENTRY, OUTPOST, BASE] + track_ids: [HERO, ENGINEER, INFANTRY_3, INFANTRY_4, SENTRY, OUTPOST, BASE, RUNE] camera_translation: [0.07128, 0.0, 0.0481] fire_control: bullet_speed: 22.5 diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index a066759ae..ac41a1087 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -4,6 +4,7 @@ + diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp index 81f6bbaf3..c4e220d01 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp @@ -259,6 +259,7 @@ class ChassisController // @NOTE: Align With 4 Sides case ChassisMode::ALIGNMENT_POWERED: [[fallthrough]]; + case ChassisMode::WIRELESS_CHARGING: [[fallthrough]]; case ChassisMode::ALIGNMENT: { const auto speed = chassis_control_velocity_->vector.head<2>(); const auto line1 = Eigen::Vector2d{speed.x(), 0}; diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index 397400cfe..a1af4e889 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -30,7 +30,9 @@ class DeformableChassis get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) , following_velocity_controller_(10.0, 0.0, 0.0) - , spin_ratio_(std::clamp(get_parameter_or("spin_ratio", 0.6), 0.0, 1.0)) + , wireless_charging_speed_limit_(get_parameter_or("wireless_charging_speed_limit", 0.2)) + , wireless_charging_angular_velocity_limit_( + get_parameter_or("wireless_charging_angular_velocity_limit", 3.0)) , joint_mode_mgr_(*this) { following_velocity_controller_.output_max = angular_velocity_max_; @@ -46,6 +48,10 @@ class DeformableChassis register_input("/gimbal/yaw/angle", gimbal_yaw_angle_, false); register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, false); + register_input("/auto_aim/single_shoot", auto_aim_single_shoot_, false); + register_input("/auto_aim/robot_center", auto_aim_robot_center_, false); + register_input("/tf", tf_, false); + register_output("/chassis/angle", chassis_angle_, nan_); register_output("/chassis/control_angle", chassis_control_angle_, nan_); register_output("/chassis/control_mode", mode_); @@ -109,7 +115,8 @@ class DeformableChassis double rotary_knob = rotary_knob_.ready() ? *rotary_knob_ : 0.0; - joint_mode_mgr_.update(switch_left, switch_right, keyboard, rotary_knob, update_dt()); + joint_mode_mgr_.update( + switch_left, switch_right, keyboard, rotary_knob, update_dt(), *gimbal_yaw_angle_); *mode_ = joint_mode_mgr_.mode(); *pitch_lock_active_ = joint_mode_mgr_.pitch_lock_active(); @@ -122,6 +129,10 @@ class DeformableChassis *suspension_reference_angle_deg_ = joint_mode_mgr_.suspension_reference_angle_deg(); publish_joint_posture_targets_(); + update_auto_aim_override_state_(); + if (auto_aim_posture_override_) + apply_auto_aim_posture_override_(); + update_velocity_control(); } while (false); } @@ -153,6 +164,36 @@ class DeformableChassis *chassis_control_angle_ = nan_; } + void update_auto_aim_override_state_() { + auto_aim_posture_override_ = auto_aim_single_shoot_.ready() && *auto_aim_single_shoot_; + auto_aim_vector_follow_ = // + auto_aim_posture_override_ && tf_.ready() && auto_aim_robot_center_.ready() + && auto_aim_robot_center_->allFinite() && !auto_aim_robot_center_->isZero(); + } + + void apply_auto_aim_posture_override_() { + const double front_rad = deg_to_rad(joint_mode_mgr_.max_angle()); + const double back_rad = deg_to_rad(joint_mode_mgr_.min_angle()); + *joint_posture_target_angle_rad_[kLeftFront] = front_rad; + *joint_posture_target_angle_rad_[kRightFront] = front_rad; + *joint_posture_target_angle_rad_[kLeftBack] = back_rad; + *joint_posture_target_angle_rad_[kRightBack] = back_rad; + + *symmetric_posture_target_ = false; + *low_prone_active_ = false; + *active_suspension_active_ = false; + + const double min_deg = joint_mode_mgr_.min_angle(); + const double max_deg = joint_mode_mgr_.max_angle(); + const double reference_deg = (min_deg + max_deg) / 2.0; + *suspension_reference_angle_deg_ = reference_deg; + // Always true: reference_deg == (min_deg + max_deg) / 2.0 + // > (min_deg - 5.0 + max_deg) / 2.0 == reference_deg - 2.5. + // Under auto-aim low-prone override the correction direction is + // intentionally always inverted. + *correction_inverted_ = reference_deg > (min_deg - 5.0 + max_deg) / 2.0; + } + double update_dt() const { if (update_rate_.ready() && std::isfinite(*update_rate_) && *update_rate_ > 1e-6) return 1.0 / *update_rate_; @@ -175,7 +216,10 @@ class DeformableChassis if (translational_velocity.norm() > 1.0) translational_velocity.normalize(); - translational_velocity *= translational_velocity_max_; + const double max_speed = *mode_ == rmcs_msgs::ChassisMode::WIRELESS_CHARGING + ? wireless_charging_speed_limit_ + : translational_velocity_max_; + translational_velocity *= max_speed; return translational_velocity; } @@ -183,33 +227,54 @@ class DeformableChassis double angular_velocity = 0.0; double chassis_control_angle = nan_; - switch (*mode_) { - case rmcs_msgs::ChassisMode::AUTO: break; - - case rmcs_msgs::ChassisMode::SPIN_FAST: { - bool forward = joint_mode_mgr_.spinning_forward(); - angular_velocity = - spin_ratio_ * (forward ? angular_velocity_max_ : -angular_velocity_max_); - angular_velocity = - std::clamp(angular_velocity, -angular_velocity_max_, angular_velocity_max_); - } break; - - case rmcs_msgs::ChassisMode::STEP_DOWN: { - double chassis_angle_error = - calculate_unsigned_chassis_angle_error(chassis_control_angle); - - constexpr double alignment = std::numbers::pi; - while (chassis_angle_error > alignment / 2) { - chassis_control_angle -= alignment; - if (chassis_control_angle < 0) - chassis_control_angle += 2 * std::numbers::pi; - chassis_angle_error -= alignment; + if (auto_aim_posture_override_) { + angular_velocity = update_auto_aim_override_angular_velocity_(chassis_control_angle); + } else { + switch (*mode_) { + case rmcs_msgs::ChassisMode::AUTO: break; + + case rmcs_msgs::ChassisMode::SPIN_FAST: { + bool forward = joint_mode_mgr_.spinning_forward(); + angular_velocity = forward ? angular_velocity_max_ : -angular_velocity_max_; + angular_velocity = + std::clamp(angular_velocity, -angular_velocity_max_, angular_velocity_max_); + } break; + + case rmcs_msgs::ChassisMode::STEP_DOWN: { + double chassis_angle_error = + calculate_unsigned_chassis_angle_error(chassis_control_angle); + + constexpr double alignment = std::numbers::pi; + while (chassis_angle_error > alignment / 2) { + chassis_control_angle -= alignment; + if (chassis_control_angle < 0) + chassis_control_angle += 2 * std::numbers::pi; + chassis_angle_error -= alignment; + } + + angular_velocity = following_velocity_controller_.update(chassis_angle_error); + } break; + + case rmcs_msgs::ChassisMode::WIRELESS_CHARGING: { + const double wireless_charging_offset_rad = + joint_mode_mgr_.wireless_charging_offset_rad(); + double chassis_angle_error = + calculate_unsigned_chassis_angle_error(chassis_control_angle); + + chassis_control_angle = + normalize_positive_angle(chassis_control_angle - wireless_charging_offset_rad); + chassis_angle_error = + normalize_positive_angle(chassis_angle_error - wireless_charging_offset_rad); + chassis_angle_error = normalize_signed_angle(chassis_angle_error); + + angular_velocity = following_velocity_controller_.update(chassis_angle_error); + angular_velocity = std::clamp( + angular_velocity, -wireless_charging_angular_velocity_limit_, + wireless_charging_angular_velocity_limit_); + } break; + + default: break; } - - angular_velocity = following_velocity_controller_.update(chassis_angle_error); - } break; - - default: break; } *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; @@ -218,6 +283,26 @@ class DeformableChassis return angular_velocity; } + double update_auto_aim_override_angular_velocity_(double& chassis_control_angle) { + double chassis_angle_error; + if (auto_aim_vector_follow_) { + const auto target_in_base = fast_tf::cast( + rmcs_description::OdomImu::Position{*auto_aim_robot_center_}, *tf_); + chassis_angle_error = target_in_base->head<2>().norm() > 1e-6 + ? std::atan2(target_in_base->y(), target_in_base->x()) + : 0.0; + chassis_control_angle = normalize_positive_angle( + 2 * std::numbers::pi - *gimbal_yaw_angle_ + chassis_angle_error); + } else { + chassis_angle_error = normalize_signed_angle( + calculate_unsigned_chassis_angle_error(chassis_control_angle)); + } + + return std::clamp( + following_velocity_controller_.update(chassis_angle_error), -angular_velocity_max_, + angular_velocity_max_); + } + double calculate_unsigned_chassis_angle_error(double& chassis_control_angle) { chassis_control_angle = *gimbal_yaw_angle_error_; if (chassis_control_angle < 0) @@ -232,6 +317,22 @@ class DeformableChassis static double deg_to_rad(double deg) { return deg * std::numbers::pi / 180.0; } + 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; + } + void publish_joint_posture_targets_() { std::array targets_deg{}; joint_mode_mgr_.copy_joint_posture_target_deg(targets_deg); @@ -246,6 +347,10 @@ class DeformableChassis "right_back", "right_front", }; + static constexpr size_t kLeftFront = 0; + static constexpr size_t kLeftBack = 1; + static constexpr size_t kRightBack = 2; + static constexpr size_t kRightFront = 3; InputInterface joystick_right_; InputInterface switch_right_; @@ -257,6 +362,12 @@ class DeformableChassis InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_; OutputInterface chassis_angle_, chassis_control_angle_; + InputInterface auto_aim_single_shoot_; + InputInterface auto_aim_robot_center_; + InputInterface tf_; + bool auto_aim_posture_override_ = false; + bool auto_aim_vector_follow_ = false; + OutputInterface mode_; OutputInterface chassis_control_velocity_; OutputInterface pitch_lock_active_; @@ -271,7 +382,9 @@ class DeformableChassis std::array, kJointCount> joint_posture_target_angle_rad_; pid::PidCalculator following_velocity_controller_; - const double spin_ratio_; + + double wireless_charging_speed_limit_; + double wireless_charging_angular_velocity_limit_; DeformableChassisModeManager joint_mode_mgr_; }; @@ -280,4 +393,5 @@ class DeformableChassis #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::chassis::DeformableChassis, rmcs_executor::Component) +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::DeformableChassis, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp index dac11d423..7e3af28cc 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp @@ -40,7 +40,10 @@ class DeformableChassisModeManager { std::clamp( node.get_parameter_or("active_suspension_base_angle", max_angle_), min_angle_ - 5.0, max_angle_)) - , suspension_enable_(node.get_parameter_or("active_suspension_enable", false)) { + , suspension_enable_(node.get_parameter_or("active_suspension_enable", false)) + , wireless_charging_offset_deg_(normalize_ccw_deg_( + node.get_parameter("wireless_charging_offset_deg").as_double())) + , wireless_charging_offset_rad_(deg_to_rad_(wireless_charging_offset_deg_)) { current_target_angle_ = max_angle_; joint_current_target_angle_.fill(max_angle_); update_joint_posture_state_(false); @@ -64,17 +67,20 @@ class DeformableChassisModeManager { apply_symmetric_target_ = true; suspension_enabled_by_toggle_ = false; low_prone_enabled_by_toggle_ = false; + step_down_combo_active_ = false; last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; last_keyboard_ = rmcs_msgs::Keyboard::zero(); last_rotary_knob_ = 0.0; + suspension_was_active_ = false; + posture_state_saved_ = false; update_joint_posture_state_(false); } void update( rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, - const rmcs_msgs::Keyboard& keyboard, double rotary_knob, double dt) { + const rmcs_msgs::Keyboard& keyboard, double rotary_knob, double dt, double gimbal_yaw_rad) { update_mode_from_inputs_(switch_left, switch_right, keyboard); update_low_prone_toggle_from_inputs_(switch_left, switch_right); @@ -82,10 +88,13 @@ class DeformableChassisModeManager { joint_posture_state_.ctrl_low_prone_active = keyboard.ctrl; joint_posture_state_.low_prone_active = joint_posture_state_.ctrl_low_prone_active || low_prone_enabled_by_toggle_; - joint_posture_state_.pitch_lock_active = joint_posture_state_.ctrl_low_prone_active; update_suspension_mode_from_inputs_(switch_left, switch_right, keyboard, rotary_knob); - update_posture_target_from_inputs_(switch_left, switch_right, keyboard, rotary_knob, dt); + update_posture_target_from_inputs_( + switch_left, switch_right, keyboard, rotary_knob, dt, gimbal_yaw_rad); + update_step_down_combo_from_inputs_(keyboard); + joint_posture_state_.pitch_lock_active = + joint_posture_state_.ctrl_low_prone_active || step_down_combo_active_; update_joint_posture_state_(joint_posture_state_.low_prone_active); last_switch_right_ = switch_right; @@ -105,13 +114,9 @@ class DeformableChassisModeManager { out = joint_posture_state_.joint_posture_target_deg; } - const JointPostureState& joint_posture_state() const { return joint_posture_state_; } - double min_angle() const { return min_angle_; } double max_angle() const { return max_angle_; } - double max_angle_rad() const { return deg_to_rad_(max_angle_); } - - double active_suspension_min_angle_rad() const { return deg_to_rad_(min_angle_ - 5.0); } + double wireless_charging_offset_rad() const { return wireless_charging_offset_rad_; } bool correction_inverted() const { double midpoint = (min_angle_ - 5.0 + max_angle_) / 2.0; @@ -127,6 +132,20 @@ class DeformableChassisModeManager { static double deg_to_rad_(double deg) { return deg * std::numbers::pi / 180.0; } + static double normalize_ccw_deg_(double deg) { + double normalized = std::fmod(deg, 360.0); + if (normalized < 0.0) + normalized += 360.0; + return normalized; + } + + static bool gimbal_faces_physical_front_(double gimbal_yaw_rad) { + if (!std::isfinite(gimbal_yaw_rad)) + return true; + const double signed_yaw = std::remainder(gimbal_yaw_rad, 2 * std::numbers::pi); + return std::abs(signed_yaw) <= std::numbers::pi / 2; + } + static bool symmetric_joint_target_requested_(const std::array& joint_target_deg) { constexpr double epsilon = 1e-6; @@ -135,15 +154,31 @@ class DeformableChassisModeManager { }); } + void save_posture_state_() { + saved_current_target_angle_ = current_target_angle_; + saved_active_suspension_base_angle_ = active_suspension_base_angle_; + saved_joint_current_target_angle_ = joint_current_target_angle_; + saved_apply_symmetric_target_ = apply_symmetric_target_; + posture_state_saved_ = true; + } + + void restore_posture_state_() { + if (!posture_state_saved_) + return; + current_target_angle_ = saved_current_target_angle_; + active_suspension_base_angle_ = saved_active_suspension_base_angle_; + joint_current_target_angle_ = saved_joint_current_target_angle_; + apply_symmetric_target_ = saved_apply_symmetric_target_; + posture_state_saved_ = false; + } + void update_mode_from_inputs_( rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, const rmcs_msgs::Keyboard& keyboard) { auto next_mode = joint_posture_state_.mode; - if (switch_left == rmcs_msgs::Switch::DOWN) { - joint_posture_state_.mode = next_mode; + if (switch_left == rmcs_msgs::Switch::DOWN) return; - } if (last_switch_right_ == rmcs_msgs::Switch::MIDDLE && switch_right == rmcs_msgs::Switch::DOWN) { @@ -164,6 +199,25 @@ class DeformableChassisModeManager { next_mode = next_mode == rmcs_msgs::ChassisMode::STEP_DOWN ? rmcs_msgs::ChassisMode::AUTO : rmcs_msgs::ChassisMode::STEP_DOWN; + } else if (!last_keyboard_.x && keyboard.x) { + if (next_mode == rmcs_msgs::ChassisMode::WIRELESS_CHARGING) { + next_mode = rmcs_msgs::ChassisMode::AUTO; + } else { + next_mode = rmcs_msgs::ChassisMode::WIRELESS_CHARGING; + save_posture_state_(); + // Entering wireless charging: force min-angle posture once, regardless of + // the current max/min posture. Q can still toggle freely afterwards. + current_target_angle_ = min_angle_; + active_suspension_base_angle_ = min_angle_; + apply_symmetric_target_ = true; + joint_current_target_angle_.fill(min_angle_); + } + } + + if (posture_state_saved_ + && joint_posture_state_.mode == rmcs_msgs::ChassisMode::WIRELESS_CHARGING + && next_mode != rmcs_msgs::ChassisMode::WIRELESS_CHARGING) { + restore_posture_state_(); } joint_posture_state_.mode = next_mode; @@ -200,13 +254,11 @@ class DeformableChassisModeManager { const bool remote_active_toggle_requested = remote_suspension_rotary_mode && rotary_knob_down_edge_(rotary_knob); - const bool keyboard_active_suspension_toggle_requested = !last_keyboard_.e && keyboard.e; + const bool keyboard_active_suspension_toggle_requested = !last_keyboard_.g && keyboard.g; if (keyboard_active_suspension_toggle_requested || remote_active_toggle_requested) suspension_enabled_by_toggle_ = !suspension_enabled_by_toggle_; - const bool active_requested = - suspension_enable_ - && (joint_posture_state_.low_prone_active || suspension_enabled_by_toggle_); + const bool active_requested = suspension_enable_ && suspension_enabled_by_toggle_; joint_posture_state_.suspension_mode = SuspensionMode::OFF; if (active_requested) @@ -214,6 +266,13 @@ class DeformableChassisModeManager { joint_posture_state_.suspension_active = joint_posture_state_.suspension_mode == SuspensionMode::ACTIVE; + + if (joint_posture_state_.suspension_active && !suspension_was_active_) { + active_suspension_base_angle_ = current_target_angle_; + } else if (!joint_posture_state_.suspension_active && suspension_was_active_) { + current_target_angle_ = active_suspension_base_angle_; + } + suspension_was_active_ = joint_posture_state_.suspension_active; } void update_low_prone_toggle_from_inputs_( @@ -226,7 +285,8 @@ class DeformableChassisModeManager { void update_posture_target_from_inputs_( rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, - const rmcs_msgs::Keyboard& keyboard, double rotary_knob, double /*dt*/) { + const rmcs_msgs::Keyboard& keyboard, double rotary_knob, double /*dt*/, + double gimbal_yaw_rad) { const bool remote_joint_posture_rotary_mode = switch_left == rmcs_msgs::Switch::MIDDLE && switch_right == rmcs_msgs::Switch::MIDDLE; @@ -236,14 +296,12 @@ class DeformableChassisModeManager { const bool remote_front_back_posture_toggle_condition = remote_joint_posture_rotary_mode && rotary_knob_up_edge_(rotary_knob); const bool front_high_rear_low = !last_keyboard_.b && keyboard.b; - const bool front_low_rear_high = !last_keyboard_.g && keyboard.g; + const bool posture_toggle_requested = + remote_posture_toggle_condition || keyboard_posture_toggle_condition; if (apply_symmetric_target_) joint_current_target_angle_.fill(current_target_angle_); - const bool posture_toggle_requested = - remote_posture_toggle_condition || keyboard_posture_toggle_condition; - if (posture_toggle_requested) { if (joint_posture_state_.suspension_active) { active_suspension_base_angle_ = @@ -261,14 +319,70 @@ class DeformableChassisModeManager { } else if (remote_front_back_posture_toggle_condition) { toggle_front_back_posture_target_(); } else if (front_high_rear_low) { - apply_front_high_rear_low_target_(); - } else if (front_low_rear_high) { - apply_front_low_rear_high_target_(); + if (gimbal_faces_physical_front_(gimbal_yaw_rad)) + apply_front_high_rear_low_target_(); + else + apply_front_low_rear_high_target_(); } last_rotary_knob_ = rotary_knob; } + void update_step_down_combo_from_inputs_(const rmcs_msgs::Keyboard& keyboard) { + const bool e_held = keyboard.e; + + if (e_held && !step_down_combo_active_) { + saved_combo_mode_ = joint_posture_state_.mode; + saved_combo_suspension_enabled_ = suspension_enabled_by_toggle_; + saved_combo_current_target_angle_ = current_target_angle_; + saved_combo_active_suspension_base_angle_ = active_suspension_base_angle_; + saved_combo_joint_target_ = joint_current_target_angle_; + saved_combo_apply_symmetric_ = apply_symmetric_target_; + + step_down_combo_active_ = true; + joint_posture_state_.mode = rmcs_msgs::ChassisMode::STEP_DOWN; + joint_posture_state_.suspension_mode = SuspensionMode::ACTIVE; + joint_posture_state_.suspension_active = true; + suspension_enabled_by_toggle_ = true; + suspension_was_active_ = true; + current_target_angle_ = min_angle_ - 5.0; + active_suspension_base_angle_ = min_angle_ - 5.0; + apply_symmetric_target_ = true; + joint_current_target_angle_.fill(min_angle_ - 5.0); + update_joint_posture_state_(joint_posture_state_.low_prone_active); + return; + } + + if (!e_held && step_down_combo_active_) { + joint_posture_state_.mode = saved_combo_mode_; + suspension_enabled_by_toggle_ = saved_combo_suspension_enabled_; + suspension_was_active_ = saved_combo_suspension_enabled_ && suspension_enable_; + current_target_angle_ = saved_combo_current_target_angle_; + active_suspension_base_angle_ = saved_combo_active_suspension_base_angle_; + joint_current_target_angle_ = saved_combo_joint_target_; + apply_symmetric_target_ = saved_combo_apply_symmetric_; + joint_posture_state_.suspension_active = + suspension_enable_ && suspension_enabled_by_toggle_; + joint_posture_state_.suspension_mode = + joint_posture_state_.suspension_active ? SuspensionMode::ACTIVE + : SuspensionMode::OFF; + step_down_combo_active_ = false; + update_joint_posture_state_(joint_posture_state_.low_prone_active); + return; + } + + if (step_down_combo_active_) { + joint_posture_state_.mode = rmcs_msgs::ChassisMode::STEP_DOWN; + joint_posture_state_.suspension_mode = SuspensionMode::ACTIVE; + joint_posture_state_.suspension_active = true; + current_target_angle_ = min_angle_ - 5.0; + active_suspension_base_angle_ = min_angle_ - 5.0; + apply_symmetric_target_ = true; + joint_current_target_angle_.fill(min_angle_ - 5.0); + update_joint_posture_state_(joint_posture_state_.low_prone_active); + } + } + bool rotary_knob_down_edge_(double rotary_knob) const { constexpr double rotary_knob_edge_threshold = 0.7; return last_rotary_knob_ < rotary_knob_edge_threshold @@ -316,12 +430,29 @@ class DeformableChassisModeManager { double max_angle_; double active_suspension_base_angle_; bool suspension_enable_; + double wireless_charging_offset_deg_; + double wireless_charging_offset_rad_; double current_target_angle_; std::array joint_current_target_angle_; bool apply_symmetric_target_ = true; bool suspension_enabled_by_toggle_ = false; bool low_prone_enabled_by_toggle_ = false; + bool suspension_was_active_ = false; + + bool step_down_combo_active_ = false; + rmcs_msgs::ChassisMode saved_combo_mode_ = rmcs_msgs::ChassisMode::AUTO; + bool saved_combo_suspension_enabled_ = false; + double saved_combo_current_target_angle_ = 0.0; + double saved_combo_active_suspension_base_angle_ = 0.0; + std::array saved_combo_joint_target_{}; + bool saved_combo_apply_symmetric_ = true; + + bool posture_state_saved_ = false; + double saved_current_target_angle_ = 0.0; + double saved_active_suspension_base_angle_ = 0.0; + std::array saved_joint_current_target_angle_{}; + bool saved_apply_symmetric_target_ = true; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp index 6da008098..ee31f2205 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp @@ -211,6 +211,8 @@ class DeformableSuspension 1e-6); active_rate_lpf_cutoff_hz_ = std::max(get_parameter_or("active_suspension_rate_lpf_cutoff_hz", 10.0), 1e-6); + active_target_pitch_rad_ = + deg_to_rad_(get_parameter_or("active_suspension_target_pitch_deg", 0.0)); calibration_wait_time_ = std::max(get_parameter_or("chassis_imu_calibration_wait_s", 2.0), 0.0); @@ -451,7 +453,8 @@ class DeformableSuspension const double clamped_pitch = std::clamp(pitch, -max_attitude, max_attitude); const double clamped_roll = std::clamp(roll, -max_attitude, max_attitude); - const double pitch_outer = pitch_outer_pid_.update(-clamped_pitch); + const double pitch_outer = + pitch_outer_pid_.update(active_target_pitch_rad_ - clamped_pitch); const double roll_outer = roll_outer_pid_.update(clamped_roll); const double pitch_diff = pitch_inner_pid_.update(pitch_outer - pitch_rate); const double roll_diff = roll_inner_pid_.update(roll_outer + roll_rate); @@ -589,6 +592,7 @@ class DeformableSuspension double active_correction_vel_limit_ = 40.0; double active_correction_acc_limit_ = 200.0; double active_rate_lpf_cutoff_hz_ = 10.0; + double active_target_pitch_rad_ = 0.0; double active_rate_filter_sampling_hz_ = 0.0; double calibration_wait_time_ = 2.0; 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 index 9291886ed..7e22197c0 100644 --- 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 @@ -203,6 +203,7 @@ class HeroChassisController } break; case rmcs_msgs::ChassisMode::ALIGNMENT: [[fallthrough]]; case rmcs_msgs::ChassisMode::ALIGNMENT_POWERED: [[fallthrough]]; + case rmcs_msgs::ChassisMode::WIRELESS_CHARGING: [[fallthrough]]; case rmcs_msgs::ChassisMode::CLIMB: [[fallthrough]]; case rmcs_msgs::ChassisMode::LAUNCH_RAMP: { double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp index 9396df1d1..6690e8841 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp @@ -35,6 +35,12 @@ class DeformableInfantryGimbalController get_parameter_or("pitch_gravity_ff_gain", pitch_gravity_ff_gain_, 0.0); get_parameter_or("pitch_gravity_ff_phase", pitch_gravity_ff_phase_, 0.0); + get_parameter_or("yaw_ref_velocity_gain", yaw_ref_velocity_gain_, 1.0); + get_parameter_or("pitch_ref_velocity_gain", pitch_ref_velocity_gain_, 1.0); + get_parameter_or("yaw_velocity_ff_gain", yaw_velocity_ff_gain_, 0.0); + get_parameter_or("yaw_acceleration_ff_gain", yaw_acceleration_ff_gain_, 0.0); + get_parameter_or("pitch_velocity_ff_gain", pitch_velocity_ff_gain_, 0.0); + get_parameter_or("pitch_acceleration_ff_gain", pitch_acceleration_ff_gain_, 0.0); get_parameter_or("ctrl_hold_pitch_target_angle", ctrl_hold_pitch_target_angle_, 0.0); } @@ -70,16 +76,18 @@ class DeformableInfantryGimbalController if (!ctrl_hold_active_) *output_.pitch_angle_error = angle_error.pitch_angle_error; + const auto trajectory_ff = trajectory_feedforward(auto_aim_active); + if (!std::isfinite(angle_error.yaw_angle_error)) { yaw_angle_pid_.reset(); yaw_velocity_pid_.reset(); *output_.yaw_control_torque = kNaN; - } - - if (std::isfinite(angle_error.yaw_angle_error)) { - const auto yaw_velocity_ref = yaw_angle_pid_.update(angle_error.yaw_angle_error); + } else { + const auto yaw_velocity_ref = yaw_angle_pid_.update(angle_error.yaw_angle_error) + + trajectory_ff.yaw_ref_velocity; *output_.yaw_control_torque = - yaw_velocity_pid_.update(yaw_velocity_ref - *input_.yaw_velocity_imu); + yaw_velocity_pid_.update(yaw_velocity_ref - *input_.yaw_velocity_imu) + + trajectory_ff.yaw_velocity + trajectory_ff.yaw_acceleration; } if (!ctrl_hold_active_) { @@ -91,13 +99,15 @@ class DeformableInfantryGimbalController } else { const auto pitch_gravity_ff = pitch_gravity_feedforward(); const auto pitch_velocity_ref = - pitch_angle_pid_.update(angle_error.pitch_angle_error); + pitch_angle_pid_.update(angle_error.pitch_angle_error) + + trajectory_ff.pitch_ref_velocity; if (pitch_torque_control_enabled_) { *output_.pitch_control_velocity = kNaN; *output_.pitch_control_torque = pitch_velocity_pid_.update(pitch_velocity_ref - *input_.pitch_velocity_imu) - + pitch_gravity_ff; + + pitch_gravity_ff + trajectory_ff.pitch_velocity + + trajectory_ff.pitch_acceleration; } else { pitch_velocity_pid_.reset(); *output_.pitch_control_velocity = pitch_velocity_ref; @@ -130,16 +140,15 @@ class DeformableInfantryGimbalController component.register_input("/remote/mouse", mouse); component.register_input("/predefined/update_rate", update_rate, false); - component.register_input("/gimbal/yaw/angle", yaw_angle); - component.register_input("/gimbal/yaw/velocity", yaw_velocity); component.register_input("/gimbal/pitch/angle", pitch_angle); - component.register_input("/gimbal/pitch/velocity", pitch_velocity); component.register_input("/gimbal/yaw/velocity_imu", yaw_velocity_imu); component.register_input("/gimbal/pitch/velocity_imu", pitch_velocity_imu); component.register_input("/auto_aim/should_control", auto_aim_should_control, false); component.register_input( "/auto_aim/control_direction", auto_aim_control_direction, false); + component.register_input("/auto_aim/ff_v", auto_aim_ff_v, false); + component.register_input("/auto_aim/ff_a", auto_aim_ff_a, false); } InputInterface joystick_left; @@ -150,15 +159,14 @@ class DeformableInfantryGimbalController InputInterface mouse; InputInterface update_rate; - InputInterface yaw_angle; - InputInterface yaw_velocity; InputInterface pitch_angle; - InputInterface pitch_velocity; InputInterface yaw_velocity_imu; InputInterface pitch_velocity_imu; InputInterface auto_aim_should_control; InputInterface auto_aim_control_direction; + InputInterface auto_aim_ff_v; + InputInterface auto_aim_ff_a; } input_{*this}; struct Output { @@ -220,9 +228,36 @@ class DeformableInfantryGimbalController auto pitch_gravity_feedforward() const -> double { if (ctrl_hold_active_) return 0.0; - if (!input_.pitch_angle.ready() || !std::isfinite(*input_.pitch_angle)) - return 0.0; - return pitch_gravity_ff_gain_ * std::sin(*input_.pitch_angle - pitch_gravity_ff_phase_); + return pitch_gravity_ff_gain_ + * std::sin(gimbal_solver_.gimbal_world_pitch() - pitch_gravity_ff_phase_); + } + + struct TrajectoryFeedforward { + double yaw_ref_velocity = 0.0; + double pitch_ref_velocity = 0.0; + double yaw_velocity = 0.0; + double yaw_acceleration = 0.0; + double pitch_velocity = 0.0; + double pitch_acceleration = 0.0; + }; + + auto trajectory_feedforward(bool auto_aim_active) const -> TrajectoryFeedforward { + if (!auto_aim_active || !input_.auto_aim_ff_v.ready() || !input_.auto_aim_ff_a.ready() + || !input_.auto_aim_ff_v->allFinite() || !input_.auto_aim_ff_a->allFinite()) + return {}; + + const auto ff_v = + gimbal_solver_.odom_to_yaw_link(OdomImu::DirectionVector{*input_.auto_aim_ff_v}); + const auto ff_a = + gimbal_solver_.odom_to_yaw_link(OdomImu::DirectionVector{*input_.auto_aim_ff_a}); + return { + .yaw_ref_velocity = yaw_ref_velocity_gain_ * ff_v->z(), + .pitch_ref_velocity = pitch_ref_velocity_gain_ * ff_v->y(), + .yaw_velocity = yaw_velocity_ff_gain_ * ff_v->z(), + .yaw_acceleration = yaw_acceleration_ff_gain_ * ff_a->z(), + .pitch_velocity = pitch_velocity_ff_gain_ * ff_v->y(), + .pitch_acceleration = pitch_acceleration_ff_gain_ * ff_a->y(), + }; } auto update_pitch_lock_state( @@ -233,7 +268,7 @@ class DeformableInfantryGimbalController suspension_on_by_switch_ = !suspension_on_by_switch_; } - pitch_lock_active_ = keyboard.ctrl || suspension_on_by_switch_; + pitch_lock_active_ = keyboard.ctrl || keyboard.e || suspension_on_by_switch_; last_switch_right_ = switch_right; } @@ -337,6 +372,12 @@ class DeformableInfantryGimbalController double ctrl_hold_pitch_target_angle_ = 0.0; double pitch_gravity_ff_gain_ = 0.0; double pitch_gravity_ff_phase_ = 0.0; + double yaw_ref_velocity_gain_ = 1.0; + double pitch_ref_velocity_gain_ = 1.0; + double yaw_velocity_ff_gain_ = 0.0; + double yaw_acceleration_ff_gain_ = 0.0; + double pitch_velocity_ff_gain_ = 0.0; + double pitch_acceleration_ff_gain_ = 0.0; bool pitch_lock_active_ = false; bool suspension_on_by_switch_ = false; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp index 5c048d0ef..e59da7d2e 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp @@ -125,6 +125,16 @@ class TwoAxisGimbalSolver { bool enabled() const { return control_enabled_; } + double gimbal_world_pitch() const { + auto dir = + fast_tf::cast(PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); + return std::asin(std::clamp(dir->z(), -1.0, 1.0)); + } + + YawLink::DirectionVector odom_to_yaw_link(const OdomImu::DirectionVector& vector) const { + return fast_tf::cast(vector, *tf_); + } + private: void update_yaw_axis() { auto yaw_axis = diff --git a/rmcs_ws/src/rmcs_core/src/controller/pid/pid_calculator.hpp b/rmcs_ws/src/rmcs_core/src/controller/pid/pid_calculator.hpp index 2951f53f8..b6bf346ef 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/pid/pid_calculator.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/pid/pid_calculator.hpp @@ -31,6 +31,7 @@ class PidCalculator { double update(double err) { if (!std::isfinite(err)) { + reset(); return nan; } else { double control = kp * err; diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp index 76beb6868..927886e4c 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp @@ -31,6 +31,8 @@ class FrictionWheelController register_input("/remote/switch/right", switch_right_); register_input("/remote/switch/left", switch_left_); register_input("/remote/keyboard", keyboard_); + register_input("/remote/mouse/mouse_wheel", mouse_wheel_); + register_input("/remote/rotary_knob", rotary_knob_, false); auto friction_wheels = get_parameter("friction_wheels").as_string_array(); auto friction_working_velocities = get_parameter("friction_velocities").as_double_array(); @@ -48,6 +50,16 @@ class FrictionWheelController friction_working_velocity_outputs_ = std::make_unique[]>(friction_count_); friction_control_velocities_ = std::make_unique[]>(friction_count_); + if (has_parameter("friction_velocities_low_mode")) { + auto friction_working_velocities_low = + get_parameter("friction_velocities_low_mode").as_double_array(); + if (friction_working_velocities_low.size() == friction_count_) { + friction_working_velocities_low_ = std::make_unique(friction_count_); + for (size_t i = 0; i < friction_count_; i++) + friction_working_velocities_low_[i] = friction_working_velocities_low[i]; + low_mode_enabled_ = true; + } + } for (size_t i = 0; i < friction_count_; i++) { friction_working_velocities_[i] = friction_working_velocities[i]; register_input(friction_wheels[i] + "/velocity", friction_velocities_[i]); @@ -58,6 +70,15 @@ class FrictionWheelController friction_wheels[i] + "/control_velocity", friction_control_velocities_[i], nan_); } + friction_velocity_max_ = std::make_unique(friction_count_); + friction_velocity_min_ = std::make_unique(friction_count_); + for (size_t i = 0; i < friction_count_; i++) { + friction_velocity_max_[i] = friction_working_velocities_[i]; + friction_velocity_min_[i] = + low_mode_enabled_ ? friction_working_velocities_low_[i] + : friction_working_velocities_[i]; + } + friction_soft_start_stop_step_ = (1 / 1000.0) / get_parameter("friction_soft_start_stop_time").as_double(); @@ -72,6 +93,13 @@ class FrictionWheelController const auto keyboard = *keyboard_; using namespace rmcs_msgs; + const bool knob_up = rotary_knob_.ready() && *rotary_knob_ <= -knob_edge_threshold_; + if (low_mode_enabled_ && switch_left == Switch::DOWN && switch_right == Switch::MIDDLE + && knob_up && !last_knob_up_) + toggle_low_mode(); + + last_knob_up_ = knob_up; + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { reset_all_controls(); @@ -79,6 +107,13 @@ class FrictionWheelController } if (switch_right != Switch::DOWN) { + if (keyboard.ctrl && keyboard.f) + update_friction_speed_by_mouse_wheel(); + else { + wheel_accumulator_ = 0.0; + wheel_tick_pending_ = false; + } + update_friction_working_velocity_outputs(); if ((!last_keyboard_.v && keyboard.v) @@ -102,6 +137,7 @@ class FrictionWheelController private: void reset_all_controls() { friction_enabled_ = false; + low_mode_active_ = false; last_primary_friction_velocity_ = nan_; primary_friction_velocity_decrease_integral_ = 0; @@ -114,9 +150,50 @@ class FrictionWheelController *friction_ready_ = *friction_jammed_ = *bullet_fired_ = false; } + void toggle_low_mode() { + low_mode_active_ = !low_mode_active_; + + 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); + } + } + + void update_friction_speed_by_mouse_wheel() { + wheel_accumulator_ += *mouse_wheel_; + if (std::abs(*mouse_wheel_) < wheel_rest_threshold_) { + wheel_accumulator_ = 0.0; + wheel_tick_pending_ = false; + } + if (wheel_tick_pending_) + return; + if (wheel_accumulator_ > wheel_tick_threshold_) { + wheel_tick_pending_ = true; + adjust_friction_speed(friction_speed_adjust_step_); + } else if (wheel_accumulator_ < -wheel_tick_threshold_) { + wheel_tick_pending_ = true; + adjust_friction_speed(-friction_speed_adjust_step_); + } + } + + void adjust_friction_speed(double delta) { + for (size_t i = 0; i < friction_count_; i++) + friction_working_velocities_[i] = std::clamp( + friction_working_velocities_[i] + delta, friction_velocity_min_[i], + friction_velocity_max_[i]); + } + + double target_friction_velocity(size_t i) const { + return low_mode_active_ ? friction_working_velocities_low_[i] + : friction_working_velocities_[i]; + } + void update_friction_working_velocity_outputs() { for (size_t i = 0; i < friction_count_; i++) - *friction_working_velocity_outputs_[i] = friction_working_velocities_[i]; + *friction_working_velocity_outputs_[i] = target_friction_velocity(i); } void update_friction_velocities() { @@ -124,7 +201,7 @@ class FrictionWheelController friction_soft_start_stop_percentage_ = 0.0; for (size_t i = 0; i < friction_count_; i++) friction_soft_start_stop_percentage_ += - *friction_velocities_[i] / friction_working_velocities_[i]; + *friction_velocities_[i] / target_friction_velocity(i); friction_soft_start_stop_percentage_ /= static_cast(friction_count_); } friction_soft_start_stop_percentage_ += @@ -134,7 +211,7 @@ class FrictionWheelController for (size_t i = 0; i < friction_count_; i++) *friction_control_velocities_[i] = - friction_soft_start_stop_percentage_ * friction_working_velocities_[i]; + friction_soft_start_stop_percentage_ * target_friction_velocity(i); } void update_friction_status() { @@ -181,7 +258,7 @@ class FrictionWheelController primary_friction_velocity_decrease_integral_ += differential; else { if (primary_friction_velocity_decrease_integral_ < -14.0 - && last_primary_friction_velocity_ < friction_working_velocities_[0] - 20.0) + && last_primary_friction_velocity_ < target_friction_velocity(0) - 20.0) fired = true; primary_friction_velocity_decrease_integral_ = 0; @@ -193,20 +270,35 @@ class FrictionWheelController } static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + static constexpr double knob_edge_threshold_ = 0.7; + static constexpr double friction_speed_adjust_step_ = 5.0; + static constexpr double wheel_tick_threshold_ = 0.005; + static constexpr double wheel_rest_threshold_ = 1e-6; rclcpp::Logger logger_; InputInterface switch_right_; InputInterface switch_left_; InputInterface keyboard_; + InputInterface mouse_wheel_; + 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(); + bool last_knob_up_ = false; size_t friction_count_; std::unique_ptr friction_working_velocities_; + std::unique_ptr friction_working_velocities_low_; + std::unique_ptr friction_velocity_min_; + std::unique_ptr friction_velocity_max_; + bool low_mode_enabled_ = false; + bool low_mode_active_ = false; + + double wheel_accumulator_ = 0.0; + bool wheel_tick_pending_ = false; std::unique_ptr[]> friction_velocities_; std::unique_ptr[]> friction_working_velocity_outputs_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp index 8efa589ba..30f3d04c5 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp @@ -8,6 +8,7 @@ #include #include #include +#include #include #include #include @@ -204,7 +205,7 @@ class DeformableInfantryOmniB pitch_encoder_angle); } - void command_update() const { + void command_update() { auto builder = board_->start_transmit(); { auto packet = gimbal_pitch_motor_.generate_torque_command(); @@ -217,21 +218,21 @@ class DeformableInfantryOmniB } { auto packet = device::CanPacket8{uint64_t{0}}; - packet << gimbal_right_friction_; + packet << gimbal_left_friction_; builder.can_transmit( Spec::kCans.kCan1, // { - .can_id = gimbal_right_friction_.send_id(), + .can_id = gimbal_left_friction_.send_id(), .can_data = packet.as_bytes(), }); } { auto packet = device::CanPacket8{uint64_t{0}}; - packet << gimbal_left_friction_; + packet << gimbal_right_friction_; builder.can_transmit( Spec::kCans.kCan2, // { - .can_id = gimbal_left_friction_.send_id(), + .can_id = gimbal_right_friction_.send_id(), .can_data = packet.as_bytes(), }); } @@ -245,10 +246,10 @@ class DeformableInfantryOmniB gimbal_pitch_motor_.store_status(data.can_data); monitor_.tick("Top::Can0", data.can_id); } else if (can == Spec::kCans.kCan1) { - gimbal_right_friction_.match_then_store_status(data.can_id, data.can_data); + gimbal_left_friction_.match_then_store_status(data.can_id, data.can_data); monitor_.tick("Top::Can1", data.can_id); } else if (can == Spec::kCans.kCan2) { - gimbal_left_friction_.match_then_store_status(data.can_id, data.can_data); + gimbal_right_friction_.match_then_store_status(data.can_id, data.can_data); monitor_.tick("Top::Can2", data.can_id); } } @@ -380,11 +381,6 @@ class DeformableInfantryOmniB status.register_output("/chassis/encoder/alpha_dot", encoder_alpha_dot_, kNaN); status.register_output("/chassis/radius", radius_, kDefaultRadius); - status.get_parameter_or("debug_log_supercap", debug_log_supercap_, false); - status.get_parameter_or("debug_log_wheel_motor", debug_log_wheel_motor_, false); - status.get_parameter_or( - "debug_log_deformable_joint_motor", debug_log_deformable_joint_motor_, false); - auto options = librmcs::board::AdvancedOptions{}; options.dangerously_skip_version_checks = true; board_ = std::make_unique(*this, serial_filter, options); @@ -425,15 +421,11 @@ class DeformableInfantryOmniB i, joint_physical_angle_[i], joint_physical_velocity_[i]); update_geometry_feedback_(); - if (debug_log_wheel_motor_ || debug_log_deformable_joint_motor_) - log_chassis_feedback_once_per_second_(); dr16_.update_status(); gimbal_yaw_motor_.update_status(); if (supercap_status_received_.load(std::memory_order_relaxed)) supercap_.update_status(); - if (debug_log_supercap_) - log_supercap_feedback_once_per_second_(); gimbal_bullet_feeder_.update_status(); tf_->set_state( @@ -469,18 +461,17 @@ class DeformableInfantryOmniB } .as_bytes(), }); + auto packet_can2_200 = device::CanPacket8{ + chassis_wheel_motors_[kRightBack].generate_command(), + device::CanPacket8::PaddingQuarter{}, + gimbal_bullet_feeder_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + }; builder.can_transmit( Spec::kCans.kCan2, // { .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[kRightBack].generate_command(), - device::CanPacket8::PaddingQuarter{}, - gimbal_bullet_feeder_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), + .can_data = packet_can2_200.as_bytes(), }); builder.can_transmit( Spec::kCans.kCan3, // @@ -495,11 +486,12 @@ class DeformableInfantryOmniB } .as_bytes(), }); + auto packet_can2_142 = gimbal_yaw_motor_.generate_command(); builder.can_transmit( Spec::kCans.kCan2, // { .can_id = 0x142, - .can_data = gimbal_yaw_motor_.generate_command().as_bytes(), + .can_data = packet_can2_142.as_bytes(), }); builder.can_transmit( Spec::kCans.kCan1, // @@ -533,14 +525,16 @@ class DeformableInfantryOmniB .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), }); break; - case kRightBack: + case kRightBack: { + auto packet = chassis_joint_motors_[i].generate_command(); builder.can_transmit( Spec::kCans.kCan2, // { .can_id = 0x141, - .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + .can_data = packet.as_bytes(), }); break; + } case kRightFront: builder.can_transmit( Spec::kCans.kCan3, // @@ -584,20 +578,12 @@ class DeformableInfantryOmniB // State - std::atomic wheel_status_received_[4] = {false, false, false, false}; std::atomic joint_status_received_[4] = {false, false, false, false}; - bool debug_log_supercap_ = false; - bool debug_log_wheel_motor_ = false; - bool debug_log_deformable_joint_motor_ = false; - const double kChassisRadiusBase; const double kRodLength; const double kDefaultRadius; - Clock::time_point next_chassis_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; - Clock::time_point next_supercap_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; - // Device device::Bmi088 imu_{1000, 0.2, 0.0}; @@ -617,7 +603,6 @@ class DeformableInfantryOmniB device::LkMotor{status_, command_, "/chassis/right_front_joint"}, }; - std::atomic latest_supercap_status_{device::CanPacket8{uint64_t{0}}}; std::atomic supercap_status_received_{false}; device::Supercap supercap_{status_, command_}; @@ -628,7 +613,6 @@ class DeformableInfantryOmniB return; if (data.can_id == 0x201) { chassis_wheel_motors_[index].store_status(data.can_data); - wheel_status_received_[index].store(true, std::memory_order_relaxed); } else if (data.can_id == 0x141) { chassis_joint_motors_[index].store_status(data.can_data); joint_status_received_[index].store(true, std::memory_order_relaxed); @@ -678,91 +662,6 @@ class DeformableInfantryOmniB *radius_ = (kChassisRadiusBase + kRodLength * alpha_rad.array().cos()).mean(); } - void log_chassis_feedback_once_per_second_() { - const auto now = Clock::now(); - if (now < next_chassis_feedback_log_time_) - return; - - const auto wheel_rx = [this](size_t index) { - return wheel_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; - }; - const auto joint_rx = [this](size_t index) { - return joint_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; - }; - - if (debug_log_wheel_motor_) { - std::string wheel_rx_str; - for (size_t i = 0; i < 4; ++i) { - if (i > 0) - wheel_rx_str.push_back(' '); - wheel_rx_str.push_back(wheel_rx(i)); - } - RCLCPP_INFO( - status_.get_logger(), - "[wheel motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "encoder(deg) lf=% .1f lb=% .1f rb=% .1f rf=% .1f | " - "rx=[%s]", - chassis_wheel_motors_[kLeftFront].angle(), - chassis_wheel_motors_[kLeftBack].angle(), - chassis_wheel_motors_[kRightBack].angle(), - chassis_wheel_motors_[kRightFront].angle(), - chassis_wheel_motors_[kLeftFront].angle(), - chassis_wheel_motors_[kLeftBack].angle(), - chassis_wheel_motors_[kRightBack].angle(), - chassis_wheel_motors_[kRightFront].angle(), wheel_rx_str.c_str()); - } - - if (debug_log_deformable_joint_motor_) { - std::string joint_rx_str; - for (size_t i = 0; i < 4; ++i) { - if (i > 0) - joint_rx_str.push_back(' '); - joint_rx_str.push_back(joint_rx(i)); - } - RCLCPP_INFO( - status_.get_logger(), - "[deformable joint motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "velocity(rad/s) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "rx=[%s]", - *joint_physical_angle_[kLeftFront], *joint_physical_angle_[kLeftBack], - *joint_physical_angle_[kRightBack], *joint_physical_angle_[kRightFront], - *joint_physical_velocity_[kLeftFront], *joint_physical_velocity_[kLeftBack], - *joint_physical_velocity_[kRightBack], *joint_physical_velocity_[kRightFront], - joint_rx_str.c_str()); - } - - next_chassis_feedback_log_time_ = now + std::chrono::seconds(1); - } - - void log_supercap_feedback_once_per_second_() { - const auto now = Clock::now(); - if (now < next_supercap_feedback_log_time_) - return; - - const bool supercap_rx = supercap_status_received_.load(std::memory_order_relaxed); - auto supercap_raw_packet = latest_supercap_status_.load(std::memory_order_relaxed); - const auto supercap_raw_bytes = supercap_raw_packet.as_bytes(); - - RCLCPP_INFO( - status_.get_logger(), - "[supercap] can1 rx=%c id=0x300 enabled=%d supercap_v=% .3f chassis_v=% .3f " - "power=% .3f raw=[%02X %02X %02X %02X %02X %02X %02X %02X]", - supercap_rx ? 'Y' : 'N', supercap_rx ? (supercap_.supercap_enabled() ? 1 : 0) : -1, - supercap_rx ? supercap_.supercap_voltage() : kNaN, - supercap_rx ? supercap_.chassis_voltage() : kNaN, - supercap_rx ? supercap_.chassis_power() : kNaN, - std::to_integer(supercap_raw_bytes[0]), - std::to_integer(supercap_raw_bytes[1]), - std::to_integer(supercap_raw_bytes[2]), - std::to_integer(supercap_raw_bytes[3]), - std::to_integer(supercap_raw_bytes[4]), - std::to_integer(supercap_raw_bytes[5]), - std::to_integer(supercap_raw_bytes[6]), - std::to_integer(supercap_raw_bytes[7])); - - next_supercap_feedback_log_time_ = now + std::chrono::seconds(1); - } - void can_receive_callback(const Spec::Can& can, const View::Can& data) override { if (data.is_extended_can_id || data.is_remote_transmission) return; @@ -773,9 +672,6 @@ class DeformableInfantryOmniB process_chassis_can_receive_(1, data); if (!data.is_extended_can_id && !data.is_remote_transmission && data.can_id == 0x300) { - if (data.can_data.size() == 8) - latest_supercap_status_.store( - device::CanPacket8{data.can_data}, std::memory_order_relaxed); supercap_.store_status(data.can_data); supercap_status_received_.store(true, std::memory_order_relaxed); } @@ -784,9 +680,9 @@ class DeformableInfantryOmniB process_chassis_can_receive_(2, data); if (data.is_extended_can_id || data.is_remote_transmission) return; - if (data.can_id == 0x142) + if (data.can_id == 0x142) { gimbal_yaw_motor_.store_status(data.can_data); - else if (data.can_id == 0x203) + } else if (data.can_id == 0x203) gimbal_bullet_feeder_.store_status(data.can_data); monitor_.tick("Bottom::Can2", data.can_id); } else if (can == Spec::kCans.kCan3) { @@ -873,4 +769,4 @@ class DeformableInfantryOmniB } // namespace rmcs_core::hardware #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::DeformableInfantryOmniB, rmcs_executor::Component) +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::DeformableInfantryOmniB, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-c.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-c.cpp new file mode 100644 index 000000000..e5bad34f2 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-c.cpp @@ -0,0 +1,773 @@ +#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/bmi088_ekf.hpp" +#include "hardware/device/board_clock_lifter.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/remote_control.hpp" +#include "hardware/device/supercap.hpp" +#include "hardware/device/vt13.hpp" +#include "hardware/util/status_monitor.hpp" + +namespace rmcs_core::hardware { + +using Clock = std::chrono::steady_clock; + +class DeformableInfantryOmniC + : public rmcs_executor::Component + , public rclcpp::Node { +public: + DeformableInfantryOmniC() + : Node( + get_component_name(), + rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) + , command_(create_partner_component(get_component_name() + "_command", *this)) { + using namespace rmcs_description; + + register_input("/predefined/timestamp", timestamp_); + register_output("/tf", tf_); + register_output( + "/auto_aim/camera_transform", camera_transform_, Eigen::Isometry3d::Identity()); + register_output("/auto_aim/barrel_direction", barrel_direction_, Eigen::Vector3d::UnitX()); + register_output("/auto_aim/yaw_velocity", auto_aim_yaw_velocity_, 0.0); + + tf_->set_transform(Eigen::Translation3d{0.058, -0.08, 0.0}); + + remote_control_ = std::make_unique(*this); + + bottom_board_ = std::make_unique( + *this, *command_, get_parameter("serial_filter_bottom_board").as_string()); + top_board_ = std::make_unique( + *this, *command_, get_parameter("serial_filter_top_board").as_string()); + + // For command: remote-status + using Srv = std_srvs::srv::Trigger; + status_service_ = create_service( + "/rmcs/service/robot_status", + [this](const Srv::Request::SharedPtr&, const Srv::Response::SharedPtr& response) { + status_service_callback(response); + }); + } + + ~DeformableInfantryOmniC() override = default; + + void before_updating() override { top_board_->request_hard_sync_read(); } + + void update() override { + bottom_board_->update(); + top_board_->update(); + remote_control_->update(); + + using namespace rmcs_description; + *camera_transform_ = fast_tf::lookup_transform(*tf_); + *barrel_direction_ = + *fast_tf::cast(PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); + *auto_aim_yaw_velocity_ = top_board_->gimbal_yaw_velocity(); + } + + void command_update() { + const bool even = ((cmd_tick_++ & 1u) == 0u); + bottom_board_->command_update(even); + top_board_->command_update(); + } + +private: + static constexpr auto kNaN = std::numeric_limits::quiet_NaN(); + static constexpr auto kLeftFront = 0; + static constexpr auto kLeftBack = 1; + static constexpr auto kRightBack = 2; + static constexpr auto kRightFront = 3; + static constexpr auto kJointName = std::array{ + "left_front", + "left_back", + "right_back", + "right_front", + }; + + class Command : public Component { + public: + explicit Command(DeformableInfantryOmniC& deformableInfantry) + : deformableInfantry(deformableInfantry) {} + + void update() override { deformableInfantry.command_update(); } + + DeformableInfantryOmniC& deformableInfantry; + }; + + struct TopBoard final : public librmcs::board::RmcsBoardLite::Callback { + public: + explicit TopBoard( + DeformableInfantryOmniC& status, Component& command, + const std::string& serial_filter = {}) + : status_{status} + , tf_{status.tf_} + , bmi088_{device::Bmi088Ekf::Config{ + .body_to_sensor = + Eigen::AngleAxisd{std::numbers::pi / 2.0, Eigen::Vector3d::UnitX()} + .toRotationMatrix()}} + , gimbal_pitch_motor_(status, command, "/gimbal/pitch") + , gimbal_left_friction_(status, command, "/gimbal/left_friction") + , gimbal_right_friction_(status, command, "/gimbal/right_friction") { + + gimbal_pitch_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} + .set_reversed() + .set_encoder_zero_point( + static_cast(status.get_parameter("pitch_motor_zero_point").as_int()))); + + gimbal_left_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} + .set_reduction_ratio(1.) + .set_reversed()); + gimbal_right_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 2}.set_reduction_ratio( + 1.)); + + status.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); + status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); + status.register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); + status.register_output("/gimbal/auto_aim/exposure_signal", camera_signal_output_); + + auto options = librmcs::board::AdvancedOptions{}; + options.dangerously_skip_version_checks = true; + board_ = std::make_unique(*this, serial_filter, options); + + board_->start_transmit().gpio_digital_read( + Spec::kGpios.kUart1Rx, // + { + .period_ms = 0, + .asap = false, + .rising_edge = false, + .falling_edge = true, + .capture_timestamp = true, + .pull = librmcs::data::GpioPull::kUp, + }); + + board_->start_transmit().uart_config(Spec::kUarts.kUart0, {.baudrate = 921600}); + + status_.remote_control_->register_vt13(&vt13_); + } + + ~TopBoard() override = default; + + [[nodiscard]] auto gimbal_yaw_velocity() const -> double { + return *gimbal_yaw_velocity_bmi088_; + } + + void request_hard_sync_read() { + // RMCS-lite top board variant currently has no GPIO hard-sync request + // path. + } + + void update() { + vt13_.update_status(); + gimbal_pitch_motor_.update_status(); + gimbal_left_friction_.update_status(); + gimbal_right_friction_.update_status(); + + const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); + + if (auto snapshot = bmi088_.snapshot()) { + *gimbal_pitch_velocity_bmi088_ = snapshot->gyro_body.y(); + *gimbal_yaw_velocity_bmi088_ = snapshot->gyro_body.z(); + tf_->set_transform( + snapshot->orientation.conjugate()); + } + + tf_->set_state( + pitch_encoder_angle); + } + + void command_update() { + auto builder = board_->start_transmit(); + { + auto packet = gimbal_pitch_motor_.generate_torque_command(); + builder.can_transmit( + Spec::kCans.kCan0, // + { + .can_id = 0x141, + .can_data = packet.as_bytes(), + }); + } + { + auto packet = device::CanPacket8{uint64_t{0}}; + packet << gimbal_left_friction_; + builder.can_transmit( + Spec::kCans.kCan1, // + { + .can_id = gimbal_left_friction_.send_id(), + .can_data = packet.as_bytes(), + }); + } + { + auto packet = device::CanPacket8{uint64_t{0}}; + packet << gimbal_right_friction_; + builder.can_transmit( + Spec::kCans.kCan2, // + { + .can_id = gimbal_right_friction_.send_id(), + .can_data = packet.as_bytes(), + }); + } + } + + void can_receive_callback(const Spec::Can& can, const View::Can& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + if (can == Spec::kCans.kCan0) { + if (data.can_id == 0x141) + gimbal_pitch_motor_.store_status(data.can_data); + monitor_.tick("Top::Can0", data.can_id); + } else if (can == Spec::kCans.kCan1) { + gimbal_left_friction_.match_then_store_status(data.can_id, data.can_data); + monitor_.tick("Top::Can1", data.can_id); + } else if (can == Spec::kCans.kCan2) { + gimbal_right_friction_.match_then_store_status(data.can_id, data.can_data); + monitor_.tick("Top::Can2", data.can_id); + } + } + + void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { + if (uart == Spec::kUarts.kUart0) + vt13_.store_status(data.uart_data); + } + + void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { + const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); + bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); + monitor_.tick("Top::Imu", "Acc"); + } + + void gyroscope_receive_callback(const View::ImuGyroscope& data) override { + const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); + monitor_.tick("Top::Imu", "Gyr"); + if (!timestamp.has_value()) + return; + auto snapshot = + bmi088_.try_update_with_gyroscope_sample(data.x, data.y, data.z, *timestamp); + if (snapshot) + imu_snapshot_output_.emit(*snapshot); + } + + void gpio_digital_read_result_callback( + const Spec::Gpio& gpio, const View::GpioDigital& data) override { + if (gpio != Spec::kGpios.kUart1Rx) + return; + if (!data.timestamp_quarter_us) + return; + + const auto timestamp = board_clock_lifter_.lift_timestamp(*data.timestamp_quarter_us); + if (!timestamp.has_value()) + return; + + camera_signal_output_.emit(*timestamp); + monitor_.tick("Top::CameraSync", "Active"); + } + + auto status() const -> std::vector { return monitor_.text(); } + + DeformableInfantryOmniC& status_; + OutputInterface& tf_; + OutputInterface gimbal_yaw_velocity_bmi088_; + OutputInterface gimbal_pitch_velocity_bmi088_; + + EventOutputInterface imu_snapshot_output_; + EventOutputInterface camera_signal_output_; + + device::Bmi088Ekf bmi088_; + device::BoardClockLifter board_clock_lifter_; + device::Vt13 vt13_; + device::LkMotor gimbal_pitch_motor_; + device::DjiMotor gimbal_left_friction_; + device::DjiMotor gimbal_right_friction_; + + StatusMonitor monitor_{}; + std::unique_ptr board_; + }; + + struct BottomBoard final : public librmcs::board::RmcsBoardLite::Callback { + public: + explicit BottomBoard( + DeformableInfantryOmniC& status, Component& command, + const std::string& serial_filter = {}) + : status_{status} + , command_{command} + , kChassisRadiusBase(status.get_parameter("chassis_radius").as_double()) + , kRodLength(status.get_parameter("rod_length").as_double()) + , kDefaultRadius(kChassisRadiusBase + kRodLength) { + + status.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) { + board_->start_transmit().uart_transmit( + Spec::kUarts.kUart0, {.uart_data = std::span{buffer, size}}); + return size; + }; + + gimbal_yaw_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( + static_cast(status.get_parameter("yaw_motor_zero_point").as_int()))); + + for (auto& motor : chassis_wheel_motors_) + motor.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} + .set_reduction_ratio(19.0) + .enable_multi_turn_angle()); + + for (auto& motor : chassis_joint_motors_) + motor.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei36} + .set_reversed() + .enable_multi_turn_angle()); + + imu_.set_coordinate_mapping([](double x, double y, double z) { + // Keep the existing chassis yaw axis mapping explicit until the bottom-board IMU + // installation is re-validated on hardware. + return std::make_tuple(-y, x, z); + }); + + gimbal_bullet_feeder_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM2006, 3} + .enable_multi_turn_angle()); + + status.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); + status.register_output("/chassis/imu/pitch", chassis_imu_pitch_, 0.0); + status.register_output("/chassis/imu/roll", chassis_imu_roll_, 0.0); + status.register_output("/chassis/imu/pitch_rate", chassis_imu_pitch_rate_, 0.0); + status.register_output("/chassis/imu/roll_rate", chassis_imu_roll_rate_, 0.0); + for (size_t i = 0; i < 4; ++i) { + status.register_output( + std::format( + "/chassis/{}_joint/physical_angle", DeformableInfantryOmniC::kJointName[i]), + joint_physical_angle_[i], kNaN); + status.register_output( + std::format( + "/chassis/{}_joint/physical_velocity", + DeformableInfantryOmniC::kJointName[i]), + joint_physical_velocity_[i], kNaN); + } + status.register_output("/chassis/encoder/alpha", encoder_alpha_, kNaN); + status.register_output("/chassis/encoder/alpha_dot", encoder_alpha_dot_, kNaN); + status.register_output("/chassis/radius", radius_, kDefaultRadius); + + auto options = librmcs::board::AdvancedOptions{}; + options.dangerously_skip_version_checks = true; + board_ = std::make_unique(*this, serial_filter, options); + + status_.remote_control_->register_dr16(&dr16_); + } + + void update() { + imu_.update_status(); + *chassis_yaw_velocity_imu_ = imu_.gz(); + { + const double q0 = imu_.q0(); + const double q1 = imu_.q1(); + const double q2 = imu_.q2(); + const double q3 = imu_.q3(); + + double sin_pitch = 2.0 * (q0 * q2 - q3 * q1); + sin_pitch = std::clamp(sin_pitch, -1.0, 1.0); + + const double standard_pitch = std::asin(sin_pitch); + const double standard_roll = + std::atan2(2.0 * (q0 * q1 + q2 * q3), 1.0 - 2.0 * (q1 * q1 + q2 * q2)); + + // Export chassis attitude using the requested convention: + // pitch < 0 when the front is higher, roll > 0 when the left side is higher. + *chassis_imu_pitch_ = -standard_pitch; + *chassis_imu_roll_ = standard_roll; + *chassis_imu_pitch_rate_ = -imu_.gy(); + *chassis_imu_roll_rate_ = imu_.gx(); + } + + for (auto& motor : chassis_wheel_motors_) + motor.update_status(); + for (auto& motor : chassis_joint_motors_) + motor.update_status(); + + for (size_t i = 0; i < 4; ++i) + update_joint_physical_feedback_( + i, joint_physical_angle_[i], joint_physical_velocity_[i]); + + update_geometry_feedback_(); + + dr16_.update_status(); + gimbal_yaw_motor_.update_status(); + if (supercap_status_received_.load(std::memory_order_relaxed)) + supercap_.update_status(); + gimbal_bullet_feeder_.update_status(); + + tf_->set_state( + gimbal_yaw_motor_.angle()); + } + + void command_update(bool even) { + auto builder = board_->start_transmit(); + if (even) { + builder.can_transmit( + Spec::kCans.kCan0, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[kLeftFront].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan1, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[kLeftBack].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + auto packet_can2_200 = device::CanPacket8{ + chassis_wheel_motors_[kRightBack].generate_command(), + device::CanPacket8::PaddingQuarter{}, + gimbal_bullet_feeder_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + }; + builder.can_transmit( + Spec::kCans.kCan2, // + { + .can_id = 0x200, + .can_data = packet_can2_200.as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan3, // + { + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[kRightFront].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + auto packet_can2_142 = gimbal_yaw_motor_.generate_command(); + builder.can_transmit( + Spec::kCans.kCan2, // + { + .can_id = 0x142, + .can_data = packet_can2_142.as_bytes(), + }); + builder.can_transmit( + Spec::kCans.kCan1, // + { + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + supercap_.generate_command(), + } + .as_bytes(), + }); + } else { + for (size_t i = 0; i < 4; ++i) { + switch (i) { + case kLeftFront: + builder.can_transmit( + Spec::kCans.kCan0, // + { + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); + break; + case kLeftBack: + builder.can_transmit( + Spec::kCans.kCan1, // + { + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); + break; + case kRightBack: { + auto packet = chassis_joint_motors_[i].generate_command(); + builder.can_transmit( + Spec::kCans.kCan2, // + { + .can_id = 0x141, + .can_data = packet.as_bytes(), + }); + break; + } + case kRightFront: + builder.can_transmit( + Spec::kCans.kCan3, // + { + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); + break; + default: break; + } + } + } + } + + static constexpr double kJointZeroPhysicalAngleRad = 62.5 * std::numbers::pi / 180.0; + + DeformableInfantryOmniC& status_; + Component& command_; + + std::unique_ptr board_; + + // Interfaces + + OutputInterface& tf_{status_.tf_}; + + OutputInterface chassis_yaw_velocity_imu_; + OutputInterface chassis_imu_pitch_; + OutputInterface chassis_imu_roll_; + OutputInterface chassis_imu_pitch_rate_; + OutputInterface chassis_imu_roll_rate_; + + std::array, 4> joint_physical_angle_; + std::array, 4> joint_physical_velocity_; + + OutputInterface encoder_alpha_; + OutputInterface encoder_alpha_dot_; + OutputInterface radius_; + + rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; + OutputInterface referee_serial_; + + // State + + std::atomic joint_status_received_[4] = {false, false, false, false}; + + const double kChassisRadiusBase; + const double kRodLength; + const double kDefaultRadius; + + // Device + + device::Bmi088 imu_{1000, 0.2, 0.0}; + device::LkMotor gimbal_yaw_motor_{status_, command_, "/gimbal/yaw"}; + device::Dr16 dr16_; + + device::DjiMotor chassis_wheel_motors_[4]{ + device::DjiMotor{status_, command_, "/chassis/left_front_wheel"}, + device::DjiMotor{status_, command_, "/chassis/left_back_wheel"}, + device::DjiMotor{status_, command_, "/chassis/right_back_wheel"}, + device::DjiMotor{status_, command_, "/chassis/right_front_wheel"}, + }; + device::LkMotor chassis_joint_motors_[4]{ + device::LkMotor{status_, command_, "/chassis/left_front_joint"}, + device::LkMotor{status_, command_, "/chassis/left_back_joint"}, + device::LkMotor{status_, command_, "/chassis/right_back_joint"}, + device::LkMotor{status_, command_, "/chassis/right_front_joint"}, + }; + + std::atomic supercap_status_received_{false}; + device::Supercap supercap_{status_, command_}; + + device::DjiMotor gimbal_bullet_feeder_{status_, command_, "/gimbal/bullet_feeder"}; + + void process_chassis_can_receive_(size_t index, const View::Can& data) { + if (data.is_extended_can_id || data.is_remote_transmission) + return; + if (data.can_id == 0x201) { + chassis_wheel_motors_[index].store_status(data.can_data); + } else if (data.can_id == 0x141) { + chassis_joint_motors_[index].store_status(data.can_data); + joint_status_received_[index].store(true, std::memory_order_relaxed); + } + } + + void update_joint_physical_feedback_( + size_t index, OutputInterface& angle_output, + OutputInterface& velocity_output) { + + if (!joint_status_received_[index].load(std::memory_order_relaxed)) { + *angle_output = kNaN; + *velocity_output = kNaN; + return; + } + + const auto to_physical_angle = [](double motor_angle) { + return kJointZeroPhysicalAngleRad - motor_angle; + }; + const auto to_physical_velocity = [](double motor_velocity) { return -motor_velocity; }; + + *angle_output = to_physical_angle(chassis_joint_motors_[index].angle()); + *velocity_output = to_physical_velocity(chassis_joint_motors_[index].velocity()); + } + + void update_geometry_feedback_() { + const Eigen::Vector4d alpha_rad{ + *joint_physical_angle_[kLeftFront], *joint_physical_angle_[kLeftBack], + *joint_physical_angle_[kRightBack], *joint_physical_angle_[kRightFront]}; + const Eigen::Vector4d alpha_dot_rad{ + *joint_physical_velocity_[kLeftFront], *joint_physical_velocity_[kLeftBack], + *joint_physical_velocity_[kRightBack], *joint_physical_velocity_[kRightFront]}; + + if (!alpha_rad.array().isFinite().all() || !alpha_dot_rad.array().isFinite().all()) { + *encoder_alpha_ = kNaN; + *encoder_alpha_dot_ = kNaN; + *radius_ = kDefaultRadius; + RCLCPP_WARN_THROTTLE( + status_.get_logger(), *status_.get_clock(), 1000, + "deformable joint feedback invalid, fallback chassis radius to default %.3f m", + kDefaultRadius); + return; + } + + *encoder_alpha_ = alpha_rad.mean(); + *encoder_alpha_dot_ = alpha_dot_rad.mean(); + *radius_ = (kChassisRadiusBase + kRodLength * alpha_rad.array().cos()).mean(); + } + + void can_receive_callback(const Spec::Can& can, const View::Can& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) + return; + if (can == Spec::kCans.kCan0) { + process_chassis_can_receive_(0, data); + monitor_.tick("Bottom::Can0", data.can_id); + } else if (can == Spec::kCans.kCan1) { + process_chassis_can_receive_(1, data); + if (!data.is_extended_can_id && !data.is_remote_transmission + && data.can_id == 0x300) { + supercap_.store_status(data.can_data); + supercap_status_received_.store(true, std::memory_order_relaxed); + } + monitor_.tick("Bottom::Can1", data.can_id); + } else if (can == Spec::kCans.kCan2) { + process_chassis_can_receive_(2, data); + if (data.is_extended_can_id || data.is_remote_transmission) + return; + if (data.can_id == 0x142) { + gimbal_yaw_motor_.store_status(data.can_data); + } else if (data.can_id == 0x203) + gimbal_bullet_feeder_.store_status(data.can_data); + monitor_.tick("Bottom::Can2", data.can_id); + } else if (can == Spec::kCans.kCan3) { + process_chassis_can_receive_(3, data); + monitor_.tick("Bottom::Can3", data.can_id); + } + } + + void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { + if (uart == Spec::kUarts.kDbus) { + dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + monitor_.tick("Bottom::Dbus", "Active"); + } else if (uart == Spec::kUarts.kUart0) { + 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()); + monitor_.tick("Bottom::Uart0", "Active"); + } + } + + void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { + imu_.store_accelerometer_status(data.x, data.y, data.z); + monitor_.tick("Bottom::Imu", "Acc"); + } + + void gyroscope_receive_callback(const View::ImuGyroscope& data) override { + imu_.store_gyroscope_status(data.x, data.y, data.z); + monitor_.tick("Bottom::Imu", "Gyr"); + } + + auto status() const -> std::vector { return monitor_.text(); } + + StatusMonitor monitor_{}; + }; + + auto status_service_callback(const std::shared_ptr& response) + -> void { + response->success = true; + + auto feedback_message = std::ostringstream{}; + auto text = [&](std::format_string format, Args&&... args) { + std::println(feedback_message, format, std::forward(args)...); + }; + + text(" yaw_motor_zero_point: {}", bottom_board_->gimbal_yaw_motor_.last_raw_angle()); + text(" pitch_motor_zero_point: {}", top_board_->gimbal_pitch_motor_.last_raw_angle()); + constexpr auto kPosition = + std::array{"left_front", "left_back", "right_back", "right_front"}; + + text(""); + for (auto&& [index, motor] : + std::views::zip(kPosition, bottom_board_->chassis_joint_motors_)) { + text(" {}_zero_point: {}", index, motor.last_raw_angle()); + } + + text("\nBottomBoard Status:"); + for (const auto& line : bottom_board_->status()) + text("> {}", line); + + text("\nTopBoard Status:"); + for (const auto& line : top_board_->status()) + text("> {}", line); + + response->message = feedback_message.str(); + } + + OutputInterface tf_; + OutputInterface camera_transform_; + OutputInterface barrel_direction_; + OutputInterface auto_aim_yaw_velocity_; + InputInterface timestamp_; + + std::unique_ptr bottom_board_; + std::unique_ptr top_board_; + std::unique_ptr remote_control_; + + std::shared_ptr command_; + uint32_t cmd_tick_ = 0; + + std::shared_ptr> status_service_; +}; + +} // namespace rmcs_core::hardware + +#include +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::DeformableInfantryOmniC, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp index 83c40c97b..023bc203a 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp @@ -8,12 +8,12 @@ #include #include #include +#include #include #include #include #include -#include #include #include @@ -34,6 +34,7 @@ #include "hardware/device/lk_motor.hpp" #include "hardware/device/remote_control.hpp" #include "hardware/device/supercap.hpp" +#include "hardware/device/vt13.hpp" #include "hardware/util/status_monitor.hpp" namespace rmcs_core::hardware { @@ -111,7 +112,7 @@ class DeformableInfantryOmni "right_front", }; - class Command : public rmcs_executor::Component { + class Command : public Component { public: explicit Command(DeformableInfantryOmni& deformableInfantry) : deformableInfantry(deformableInfantry) {} @@ -121,10 +122,201 @@ class DeformableInfantryOmni DeformableInfantryOmni& deformableInfantry; }; + struct TopBoard final : public librmcs::board::RmcsBoardLite::Callback { + public: + explicit TopBoard( + DeformableInfantryOmni& status, Component& command, + const std::string& serial_filter = {}) + : status_{status} + , tf_{status.tf_} + , bmi088_{device::Bmi088Ekf::Config{ + .body_to_sensor = + Eigen::AngleAxisd{std::numbers::pi / 2.0, Eigen::Vector3d::UnitX()} + .toRotationMatrix()}} + , gimbal_pitch_motor_(status, command, "/gimbal/pitch") + , gimbal_left_friction_(status, command, "/gimbal/left_friction") + , gimbal_right_friction_(status, command, "/gimbal/right_friction") { + + gimbal_pitch_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} + .set_reversed() + .set_encoder_zero_point( + static_cast(status.get_parameter("pitch_motor_zero_point").as_int()))); + + gimbal_left_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} + .set_reduction_ratio(1.) + .set_reversed()); + gimbal_right_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 2}.set_reduction_ratio( + 1.)); + + status.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); + status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); + status.register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); + status.register_output("/gimbal/auto_aim/exposure_signal", camera_signal_output_); + + auto options = librmcs::board::AdvancedOptions{}; + options.dangerously_skip_version_checks = true; + board_ = std::make_unique(*this, serial_filter, options); + + board_->start_transmit().gpio_digital_read( + Spec::kGpios.kUart1Rx, // + { + .period_ms = 0, + .asap = false, + .rising_edge = false, + .falling_edge = true, + .capture_timestamp = true, + .pull = librmcs::data::GpioPull::kUp, + }); + + board_->start_transmit().uart_config(Spec::kUarts.kUart0, {.baudrate = 921600}); + + status_.remote_control_->register_vt13(&vt13_); + } + + ~TopBoard() override = default; + + [[nodiscard]] auto gimbal_yaw_velocity() const -> double { + return *gimbal_yaw_velocity_bmi088_; + } + + void request_hard_sync_read() { + // RMCS-lite top board variant currently has no GPIO hard-sync request + // path. + } + + void update() { + vt13_.update_status(); + gimbal_pitch_motor_.update_status(); + gimbal_left_friction_.update_status(); + gimbal_right_friction_.update_status(); + + const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); + + if (auto snapshot = bmi088_.snapshot()) { + *gimbal_pitch_velocity_bmi088_ = snapshot->gyro_body.y(); + *gimbal_yaw_velocity_bmi088_ = snapshot->gyro_body.z(); + tf_->set_transform( + snapshot->orientation.conjugate()); + } + + tf_->set_state( + pitch_encoder_angle); + } + + void command_update() { + auto builder = board_->start_transmit(); + { + auto packet = gimbal_pitch_motor_.generate_torque_command(); + builder.can_transmit( + Spec::kCans.kCan0, // + { + .can_id = 0x141, + .can_data = packet.as_bytes(), + }); + } + { + auto packet = device::CanPacket8{uint64_t{0}}; + packet << gimbal_left_friction_; + builder.can_transmit( + Spec::kCans.kCan1, // + { + .can_id = gimbal_left_friction_.send_id(), + .can_data = packet.as_bytes(), + }); + } + { + auto packet = device::CanPacket8{uint64_t{0}}; + packet << gimbal_right_friction_; + builder.can_transmit( + Spec::kCans.kCan2, // + { + .can_id = gimbal_right_friction_.send_id(), + .can_data = packet.as_bytes(), + }); + } + } + + void can_receive_callback(const Spec::Can& can, const View::Can& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + if (can == Spec::kCans.kCan0) { + if (data.can_id == 0x141) + gimbal_pitch_motor_.store_status(data.can_data); + monitor_.tick("Top::Can0", data.can_id); + } else if (can == Spec::kCans.kCan1) { + gimbal_left_friction_.match_then_store_status(data.can_id, data.can_data); + monitor_.tick("Top::Can1", data.can_id); + } else if (can == Spec::kCans.kCan2) { + gimbal_right_friction_.match_then_store_status(data.can_id, data.can_data); + monitor_.tick("Top::Can2", data.can_id); + } + } + + void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { + if (uart == Spec::kUarts.kUart0) + vt13_.store_status(data.uart_data); + } + + void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { + const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); + bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); + monitor_.tick("Top::Imu", "Acc"); + } + + void gyroscope_receive_callback(const View::ImuGyroscope& data) override { + const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); + monitor_.tick("Top::Imu", "Gyr"); + if (!timestamp.has_value()) + return; + auto snapshot = + bmi088_.try_update_with_gyroscope_sample(data.x, data.y, data.z, *timestamp); + if (snapshot) + imu_snapshot_output_.emit(*snapshot); + } + + void gpio_digital_read_result_callback( + const Spec::Gpio& gpio, const View::GpioDigital& data) override { + if (gpio != Spec::kGpios.kUart1Rx) + return; + if (!data.timestamp_quarter_us) + return; + + const auto timestamp = board_clock_lifter_.lift_timestamp(*data.timestamp_quarter_us); + if (!timestamp.has_value()) + return; + + camera_signal_output_.emit(*timestamp); + monitor_.tick("Top::CameraSync", "Active"); + } + + auto status() const -> std::vector { return monitor_.text(); } + + DeformableInfantryOmni& status_; + OutputInterface& tf_; + OutputInterface gimbal_yaw_velocity_bmi088_; + OutputInterface gimbal_pitch_velocity_bmi088_; + + EventOutputInterface imu_snapshot_output_; + EventOutputInterface camera_signal_output_; + + device::Bmi088Ekf bmi088_; + device::BoardClockLifter board_clock_lifter_; + device::Vt13 vt13_; + device::LkMotor gimbal_pitch_motor_; + device::DjiMotor gimbal_left_friction_; + device::DjiMotor gimbal_right_friction_; + + StatusMonitor monitor_{}; + std::unique_ptr board_; + }; + struct BottomBoard final : public librmcs::board::RmcsBoardLite::Callback { public: explicit BottomBoard( - DeformableInfantryOmni& status, rmcs_executor::Component& command, + DeformableInfantryOmni& status, Component& command, const std::string& serial_filter = {}) : status_{status} , command_{command} @@ -189,11 +381,6 @@ class DeformableInfantryOmni status.register_output("/chassis/encoder/alpha_dot", encoder_alpha_dot_, kNaN); status.register_output("/chassis/radius", radius_, kDefaultRadius); - status.get_parameter_or("debug_log_supercap", debug_log_supercap_, false); - status.get_parameter_or("debug_log_wheel_motor", debug_log_wheel_motor_, false); - status.get_parameter_or( - "debug_log_deformable_joint_motor", debug_log_deformable_joint_motor_, false); - auto options = librmcs::board::AdvancedOptions{}; options.dangerously_skip_version_checks = true; board_ = std::make_unique(*this, serial_filter, options); @@ -235,15 +422,11 @@ class DeformableInfantryOmni i, joint_physical_angle_[i], joint_physical_velocity_[i]); update_geometry_feedback_(); - if (debug_log_wheel_motor_ || debug_log_deformable_joint_motor_) - log_chassis_feedback_once_per_second_(); dr16_.update_status(); gimbal_yaw_motor_.update_status(); if (supercap_status_received_.load(std::memory_order_relaxed)) supercap_.update_status(); - if (debug_log_supercap_) - log_supercap_feedback_once_per_second_(); gimbal_bullet_feeder_.update_status(); tf_->set_state( @@ -279,18 +462,17 @@ class DeformableInfantryOmni } .as_bytes(), }); + auto packet_can2_200 = device::CanPacket8{ + chassis_wheel_motors_[kRightBack].generate_command(), + device::CanPacket8::PaddingQuarter{}, + gimbal_bullet_feeder_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + }; builder.can_transmit( Spec::kCans.kCan2, // { .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[kRightBack].generate_command(), - device::CanPacket8::PaddingQuarter{}, - gimbal_bullet_feeder_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), + .can_data = packet_can2_200.as_bytes(), }); builder.can_transmit( Spec::kCans.kCan3, // @@ -305,11 +487,12 @@ class DeformableInfantryOmni } .as_bytes(), }); + auto packet_can2_142 = gimbal_yaw_motor_.generate_command(); builder.can_transmit( Spec::kCans.kCan2, // { .can_id = 0x142, - .can_data = gimbal_yaw_motor_.generate_command().as_bytes(), + .can_data = packet_can2_142.as_bytes(), }); builder.can_transmit( Spec::kCans.kCan1, // @@ -343,14 +526,16 @@ class DeformableInfantryOmni .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), }); break; - case kRightBack: + case kRightBack: { + auto packet = chassis_joint_motors_[i].generate_command(); builder.can_transmit( Spec::kCans.kCan2, // { .can_id = 0x141, - .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + .can_data = packet.as_bytes(), }); break; + } case kRightFront: builder.can_transmit( Spec::kCans.kCan3, // @@ -394,25 +579,17 @@ class DeformableInfantryOmni // State - std::atomic wheel_status_received_[4] = {false, false, false, false}; std::atomic joint_status_received_[4] = {false, false, false, false}; - bool debug_log_supercap_ = false; - bool debug_log_wheel_motor_ = false; - bool debug_log_deformable_joint_motor_ = false; - const double kChassisRadiusBase; const double kRodLength; const double kDefaultRadius; - Clock::time_point next_chassis_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; - Clock::time_point next_supercap_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; - // Device device::Bmi088 imu_{1000, 0.2, 0.0}; device::LkMotor gimbal_yaw_motor_{status_, command_, "/gimbal/yaw"}; - device::Dr16 dr16_{}; + device::Dr16 dr16_; device::DjiMotor chassis_wheel_motors_[4]{ device::DjiMotor{status_, command_, "/chassis/left_front_wheel"}, @@ -427,7 +604,6 @@ class DeformableInfantryOmni device::LkMotor{status_, command_, "/chassis/right_front_joint"}, }; - std::atomic latest_supercap_status_{device::CanPacket8{uint64_t{0}}}; std::atomic supercap_status_received_{false}; device::Supercap supercap_{status_, command_}; @@ -438,7 +614,6 @@ class DeformableInfantryOmni return; if (data.can_id == 0x201) { chassis_wheel_motors_[index].store_status(data.can_data); - wheel_status_received_[index].store(true, std::memory_order_relaxed); } else if (data.can_id == 0x141) { chassis_joint_motors_[index].store_status(data.can_data); joint_status_received_[index].store(true, std::memory_order_relaxed); @@ -488,91 +663,6 @@ class DeformableInfantryOmni *radius_ = (kChassisRadiusBase + kRodLength * alpha_rad.array().cos()).mean(); } - void log_chassis_feedback_once_per_second_() { - const auto now = Clock::now(); - if (now < next_chassis_feedback_log_time_) - return; - - const auto wheel_rx = [this](size_t index) { - return wheel_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; - }; - const auto joint_rx = [this](size_t index) { - return joint_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; - }; - - if (debug_log_wheel_motor_) { - std::string wheel_rx_str; - for (size_t i = 0; i < 4; ++i) { - if (i > 0) - wheel_rx_str.push_back(' '); - wheel_rx_str.push_back(wheel_rx(i)); - } - RCLCPP_INFO( - status_.get_logger(), - "[wheel motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "encoder(deg) lf=% .1f lb=% .1f rb=% .1f rf=% .1f | " - "rx=[%s]", - chassis_wheel_motors_[kLeftFront].angle(), - chassis_wheel_motors_[kLeftBack].angle(), - chassis_wheel_motors_[kRightBack].angle(), - chassis_wheel_motors_[kRightFront].angle(), - chassis_wheel_motors_[kLeftFront].angle(), - chassis_wheel_motors_[kLeftBack].angle(), - chassis_wheel_motors_[kRightBack].angle(), - chassis_wheel_motors_[kRightFront].angle(), wheel_rx_str.c_str()); - } - - if (debug_log_deformable_joint_motor_) { - std::string joint_rx_str; - for (size_t i = 0; i < 4; ++i) { - if (i > 0) - joint_rx_str.push_back(' '); - joint_rx_str.push_back(joint_rx(i)); - } - RCLCPP_INFO( - status_.get_logger(), - "[deformable joint motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "velocity(rad/s) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "rx=[%s]", - *joint_physical_angle_[kLeftFront], *joint_physical_angle_[kLeftBack], - *joint_physical_angle_[kRightBack], *joint_physical_angle_[kRightFront], - *joint_physical_velocity_[kLeftFront], *joint_physical_velocity_[kLeftBack], - *joint_physical_velocity_[kRightBack], *joint_physical_velocity_[kRightFront], - joint_rx_str.c_str()); - } - - next_chassis_feedback_log_time_ = now + std::chrono::seconds(1); - } - - void log_supercap_feedback_once_per_second_() { - const auto now = Clock::now(); - if (now < next_supercap_feedback_log_time_) - return; - - const bool supercap_rx = supercap_status_received_.load(std::memory_order_relaxed); - auto supercap_raw_packet = latest_supercap_status_.load(std::memory_order_relaxed); - const auto supercap_raw_bytes = supercap_raw_packet.as_bytes(); - - RCLCPP_INFO( - status_.get_logger(), - "[supercap] can1 rx=%c id=0x300 enabled=%d supercap_v=% .3f chassis_v=% .3f " - "power=% .3f raw=[%02X %02X %02X %02X %02X %02X %02X %02X]", - supercap_rx ? 'Y' : 'N', supercap_rx ? (supercap_.supercap_enabled() ? 1 : 0) : -1, - supercap_rx ? supercap_.supercap_voltage() : kNaN, - supercap_rx ? supercap_.chassis_voltage() : kNaN, - supercap_rx ? supercap_.chassis_power() : kNaN, - std::to_integer(supercap_raw_bytes[0]), - std::to_integer(supercap_raw_bytes[1]), - std::to_integer(supercap_raw_bytes[2]), - std::to_integer(supercap_raw_bytes[3]), - std::to_integer(supercap_raw_bytes[4]), - std::to_integer(supercap_raw_bytes[5]), - std::to_integer(supercap_raw_bytes[6]), - std::to_integer(supercap_raw_bytes[7])); - - next_supercap_feedback_log_time_ = now + std::chrono::seconds(1); - } - void can_receive_callback(const Spec::Can& can, const View::Can& data) override { if (data.is_extended_can_id || data.is_remote_transmission) return; @@ -583,9 +673,6 @@ class DeformableInfantryOmni process_chassis_can_receive_(1, data); if (!data.is_extended_can_id && !data.is_remote_transmission && data.can_id == 0x300) { - if (data.can_data.size() == 8) - latest_supercap_status_.store( - device::CanPacket8{data.can_data}, std::memory_order_relaxed); supercap_.store_status(data.can_data); supercap_status_received_.store(true, std::memory_order_relaxed); } @@ -594,9 +681,9 @@ class DeformableInfantryOmni process_chassis_can_receive_(2, data); if (data.is_extended_can_id || data.is_remote_transmission) return; - if (data.can_id == 0x142) + if (data.can_id == 0x142) { gimbal_yaw_motor_.store_status(data.can_data); - else if (data.can_id == 0x203) + } else if (data.can_id == 0x203) gimbal_bullet_feeder_.store_status(data.can_data); monitor_.tick("Bottom::Can2", data.can_id); } else if (can == Spec::kCans.kCan3) { @@ -633,188 +720,6 @@ class DeformableInfantryOmni StatusMonitor monitor_{}; }; - struct TopBoard final : public librmcs::board::RmcsBoardLite::Callback { - public: - explicit TopBoard( - DeformableInfantryOmni& status, rmcs_executor::Component& command, - const std::string& serial_filter = {}) - : tf_{status.tf_} - , bmi088_{device::Bmi088Ekf::Config{ - .body_to_sensor = - Eigen::AngleAxisd{std::numbers::pi / 2.0, Eigen::Vector3d::UnitX()} - .toRotationMatrix()}} - , gimbal_pitch_motor_(status, command, "/gimbal/pitch") - , gimbal_left_friction_(status, command, "/gimbal/left_friction") - , gimbal_right_friction_(status, command, "/gimbal/right_friction") { - - gimbal_pitch_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} - .set_reversed() - .set_encoder_zero_point( - static_cast(status.get_parameter("pitch_motor_zero_point").as_int()))); - - gimbal_left_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} - .set_reduction_ratio(1.) - .set_reversed()); - gimbal_right_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 2}.set_reduction_ratio( - 1.)); - - status.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); - status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); - status.register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); - status.register_output("/gimbal/auto_aim/exposure_signal", camera_signal_output_); - - auto options = librmcs::board::AdvancedOptions{}; - options.dangerously_skip_version_checks = true; - board_ = std::make_unique(*this, serial_filter, options); - - board_->start_transmit().gpio_digital_read( - Spec::kGpios.kUart1Rx, { - .period_ms = 0, - .asap = false, - .rising_edge = false, - .falling_edge = true, - .capture_timestamp = true, - .pull = librmcs::data::GpioPull::kUp, - }); - } - - ~TopBoard() override = default; - - [[nodiscard]] auto gimbal_yaw_velocity() const -> double { - return *gimbal_yaw_velocity_bmi088_; - } - - void request_hard_sync_read() { - // RMCS-lite top board variant currently has no GPIO hard-sync request - // path. - } - - void update() { - gimbal_pitch_motor_.update_status(); - gimbal_left_friction_.update_status(); - gimbal_right_friction_.update_status(); - - const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); - - if (auto snapshot = bmi088_.snapshot()) { - *gimbal_pitch_velocity_bmi088_ = snapshot->gyro_body.y(); - *gimbal_yaw_velocity_bmi088_ = snapshot->gyro_body.z(); - tf_->set_transform( - snapshot->orientation.conjugate()); - } - - tf_->set_state( - pitch_encoder_angle); - } - - void command_update() const { - auto builder = board_->start_transmit(); - builder.can_transmit( - Spec::kCans.kCan0, // - { - .can_id = 0x141, - .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), - }); - builder.can_transmit( - Spec::kCans.kCan1, // - { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_left_friction_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can_transmit( - Spec::kCans.kCan2, // - { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - gimbal_right_friction_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - } - - void can_receive_callback(const Spec::Can& can, const View::Can& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - if (can == Spec::kCans.kCan0) { - if (data.can_id == 0x141) - gimbal_pitch_motor_.store_status(data.can_data); - monitor_.tick("Top::Can0", data.can_id); - } else if (can == Spec::kCans.kCan1) { - if (data.can_id == 0x201) - gimbal_left_friction_.store_status(data.can_data); - monitor_.tick("Top::Can1", data.can_id); - } else if (can == Spec::kCans.kCan2) { - if (data.can_id == 0x202) - gimbal_right_friction_.store_status(data.can_data); - monitor_.tick("Top::Can2", data.can_id); - } - } - - void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { - const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); - bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); - monitor_.tick("Top::Imu", "Acc"); - } - - void gyroscope_receive_callback(const View::ImuGyroscope& data) override { - const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); - monitor_.tick("Top::Imu", "Gyr"); - if (!timestamp.has_value()) - return; - auto snapshot = - bmi088_.try_update_with_gyroscope_sample(data.x, data.y, data.z, *timestamp); - if (snapshot) - imu_snapshot_output_.emit(*snapshot); - } - - void gpio_digital_read_result_callback( - const Spec::Gpio& gpio, const View::GpioDigital& data) override { - if (gpio != Spec::kGpios.kUart1Rx) - return; - if (!data.timestamp_quarter_us) - return; - - const auto timestamp = board_clock_lifter_.lift_timestamp(*data.timestamp_quarter_us); - if (!timestamp.has_value()) - return; - - camera_signal_output_.emit(*timestamp); - monitor_.tick("Top::CameraSync", "Active"); - } - - auto status() const -> std::vector { return monitor_.text(); } - - OutputInterface& tf_; - OutputInterface gimbal_yaw_velocity_bmi088_; - OutputInterface gimbal_pitch_velocity_bmi088_; - - EventOutputInterface imu_snapshot_output_; - EventOutputInterface camera_signal_output_; - - device::Bmi088Ekf bmi088_; - device::BoardClockLifter board_clock_lifter_; - device::LkMotor gimbal_pitch_motor_; - device::DjiMotor gimbal_left_friction_; - device::DjiMotor gimbal_right_friction_; - - StatusMonitor monitor_{}; - std::unique_ptr board_; - }; - auto status_service_callback(const std::shared_ptr& response) -> void { response->success = true; @@ -865,4 +770,4 @@ class DeformableInfantryOmni } // namespace rmcs_core::hardware #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::DeformableInfantryOmni, rmcs_executor::Component) +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::DeformableInfantryOmni, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088_ekf.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088_ekf.hpp index 83c1b2e32..33ae9bef4 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088_ekf.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088_ekf.hpp @@ -84,7 +84,7 @@ class Bmi088Ekf { ekf_state_time_ = accel_sample_time; const auto correction = ekf_.prepare_correction(pending_accel_sample_->accel_g); - if (!correction || correction->chi_square() >= 3.0) + if (!correction || correction->chi_square() >= 16.0) break; if (!ekf_.correct(*correction)) break; diff --git a/rmcs_ws/src/rmcs_core/src/identification/static_torque_test_controller.cpp b/rmcs_ws/src/rmcs_core/src/identification/static_torque_test_controller.cpp index e537da8e8..81b33392b 100644 --- a/rmcs_ws/src/rmcs_core/src/identification/static_torque_test_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/identification/static_torque_test_controller.cpp @@ -9,10 +9,14 @@ #include #include #include +#include + +#include #include #include #include +#include #include #include #include @@ -25,7 +29,6 @@ namespace { using Clock = std::chrono::steady_clock; -constexpr auto kCenterInfoInterval = std::chrono::duration(0.5); constexpr double kRangeTolerance = 1e-9; template @@ -93,17 +96,14 @@ double wrap_to_pi(double angle) { enum class RemoteMode { kMeasureRange, - kCenterHold, kTestCommand, kIdle, }; RemoteMode decode_remote_mode(rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right) { using rmcs_msgs::Switch; - if (switch_left == Switch::MIDDLE && switch_right == Switch::DOWN) - return RemoteMode::kMeasureRange; if (switch_left == Switch::MIDDLE && switch_right == Switch::MIDDLE) - return RemoteMode::kCenterHold; + return RemoteMode::kMeasureRange; if (switch_left == Switch::MIDDLE && switch_right == Switch::UP) return RemoteMode::kTestCommand; return RemoteMode::kIdle; @@ -149,6 +149,7 @@ class StaticTorqueTestController register_input("/predefined/timestamp", timestamp_); register_input("/remote/switch/left", switch_left_); register_input("/remote/switch/right", switch_right_); + register_input("/tf", tf_); register_input(measured_torque_name_, measured_torque_); register_input(measured_velocity_name_, measured_velocity_); register_input(measured_angle_name_, measured_angle_); @@ -167,7 +168,6 @@ class StaticTorqueTestController angle_tracking_initialized_ = false; range_initialized_ = false; remote_mode_initialized_ = false; - next_center_info_time_ = Clock::time_point{}; } void update() override { @@ -178,7 +178,6 @@ class StaticTorqueTestController switch (remote_mode) { case RemoteMode::kMeasureRange: handle_measure_range(); break; - case RemoteMode::kCenterHold: handle_center_hold(mode_changed); break; case RemoteMode::kTestCommand: handle_test_command(mode_changed); break; case RemoteMode::kIdle: handle_idle(); break; } @@ -224,56 +223,11 @@ class StaticTorqueTestController range_max_ = std::max(range_max_, current_continuous_angle_); } - void handle_center_hold(bool mode_changed) { - stop_test(true); - - if (!range_initialized_) { - *control_torque_ = nan_; - return; - } - - if (mode_changed) { - position_pid_.reset(); - velocity_pid_.reset(); - next_center_info_time_ = *timestamp_; - } - - active_setpoint_ = 0.5 * (range_min_ + range_max_); - *control_torque_ = calculate_pid_output(active_setpoint_); - - maybe_log_center_hold_info(); - } - - void maybe_log_center_hold_info() { - if (*timestamp_ < next_center_info_time_) - return; - - const double control_error = active_setpoint_ - current_continuous_angle_; - RCLCPP_INFO( - get_logger(), - "Center hold error=%.6f rad, setpoint=%.6f rad, angle=%.6f rad, range=[%.6f, %.6f] rad", - control_error, wrap_to_pi(active_setpoint_), current_wrapped_angle_, - wrap_to_pi(range_min_), wrap_to_pi(range_max_)); - - do { - next_center_info_time_ += - std::chrono::duration_cast(kCenterInfoInterval); - } while (*timestamp_ >= next_center_info_time_); - } - void handle_test_command(bool mode_changed) { if (mode_changed) { - if (last_remote_mode_ == RemoteMode::kCenterHold) { - if (!start_test()) { - *control_torque_ = nan_; - return; - } - } else if (!test_setpoint_valid_) { + if (!start_test()) { *control_torque_ = nan_; return; - } else { - position_pid_.reset(); - velocity_pid_.reset(); } } @@ -334,7 +288,8 @@ class StaticTorqueTestController csv_writer_.open(path); csv_writer_.write_row( "update_count", "elapsed_s", control_torque_name_, measured_torque_name_, - measured_velocity_name_, measured_angle_name_); + measured_velocity_name_, measured_angle_name_, "imu_pitch_angle", + "imu_yaw_angle"); csv_writer_.flush(); } catch (const std::exception& exception) { const auto path_string = path.string(); @@ -364,12 +319,21 @@ class StaticTorqueTestController const double elapsed_s = std::max(0.0, std::chrono::duration(*timestamp_ - test_start_time_).count()); + const auto [imu_yaw, imu_pitch] = imu_yaw_pitch(); + csv_writer_.write_row( *update_count_, elapsed_s, *control_torque_, *measured_torque_, *measured_velocity_, - current_wrapped_angle_); + current_wrapped_angle_, imu_pitch, imu_yaw); csv_writer_.flush(); } + std::pair imu_yaw_pitch() const { + auto dir = fast_tf::cast( + rmcs_description::PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); + return { + std::atan2(dir->y(), dir->x()), std::asin(std::clamp(dir->z(), -1.0, 1.0))}; + } + void finish_test_sequence() { test_active_ = false; close_csv(); @@ -430,6 +394,7 @@ class StaticTorqueTestController InputInterface timestamp_; InputInterface switch_left_; InputInterface switch_right_; + InputInterface tf_; InputInterface measured_torque_; InputInterface measured_velocity_; InputInterface measured_angle_; @@ -450,7 +415,6 @@ class StaticTorqueTestController bool remote_mode_initialized_ = false; RemoteMode last_remote_mode_ = RemoteMode::kIdle; - Clock::time_point next_center_info_time_{}; double active_setpoint_ = 0.0; bool test_setpoint_valid_ = false; diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp index b5e7640e1..a91679a40 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp @@ -145,6 +145,7 @@ class DeformableInfantry case rmcs_msgs::ChassisMode::SPIN_FAST: return Shape::Color::GREEN; case rmcs_msgs::ChassisMode::AUTO: return Shape::Color::CYAN; case rmcs_msgs::ChassisMode::STEP_DOWN: return Shape::Color::PINK; + case rmcs_msgs::ChassisMode::WIRELESS_CHARGING: return Shape::Color::BLACK; default: return Shape::Color::WHITE; } } diff --git a/rmcs_ws/src/rmcs_core/src/referee/status.cpp b/rmcs_ws/src/rmcs_core/src/referee/status.cpp index c0a7db62e..3f11a3af6 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/status.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/status.cpp @@ -1,3 +1,4 @@ +#include #include #include #include @@ -103,6 +104,9 @@ class Status register_output("/referee/map_command/event/source", map_command_event_source_, 0); register_output("/referee/map_command/event/timestamp", map_command_event_timestamp_, 0.0); register_output("/referee/map_command/event/sequence", map_command_event_sequence_, 0); + register_output( + "/referee/multi_robot_communication/lidar_msg_broadcast", lidar_msg_broadcast_, + LidarMsgBroadcast{}); robot_status_watchdog_.reset(5'000); } @@ -182,6 +186,8 @@ class Status update_sentry_info(); else if (command_id == 0x0303) update_map_command(); + else if (command_id == 0x0301) + update_robot_interaction_data(); } void update_game_status() { @@ -326,6 +332,28 @@ class Status *map_command_event_timestamp_ = *map_command_received_timestamp_; *map_command_event_sequence_ += 1; } + + void update_robot_interaction_data() { + if (frame_.header.data_length < sizeof(RobotInteractionData)) { + RCLCPP_WARN( + logger_, "Robot interaction data length invalid: %u", + static_cast(frame_.header.data_length)); + return; + } + + RobotInteractionData data; + std::memcpy(&data, frame_.body.data, sizeof(data)); + + if (data.sender_id != 9 && data.sender_id != 109) + return; + if (data.data_cmd_id < 0x0200 || data.data_cmd_id > 0x02ff) + return; + + LidarMsgBroadcast lidar_msg{}; + std::memcpy(lidar_msg.data(), data.user_data, lidar_msg.size()); + *lidar_msg_broadcast_ = lidar_msg; + } + // When referee system loses connection unexpectedly, // use these indicators make sure the robot safe. // Muzzle: Cooling priority with level 1 @@ -403,6 +431,9 @@ class Status OutputInterface map_command_event_sequence_; MapCommand last_map_command_{}; bool has_last_map_command_ = false; + + using LidarMsgBroadcast = std::array; + OutputInterface lidar_msg_broadcast_; }; } // namespace rmcs_core::referee diff --git a/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp b/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp index ad5e21619..1f2f02821 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp @@ -158,4 +158,11 @@ struct __attribute__((packed)) SentryInfo { }; static_assert(sizeof(SentryInfo) == 14); +struct __attribute__((packed)) RobotInteractionData { + uint16_t data_cmd_id; + uint16_t sender_id; + uint16_t receiver_id; + uint8_t user_data[112]; +}; + } // namespace rmcs_core::referee::status diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp index a279b92c7..87599fb7f 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp @@ -12,6 +12,7 @@ enum class ChassisMode : uint8_t { LAUNCH_RAMP, ALIGNMENT, ALIGNMENT_POWERED, + WIRELESS_CHARGING, CLIMB, }; diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp index 1cccd175a..456e1f672 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp @@ -50,6 +50,7 @@ constexpr auto to_string(ChassisMode mode) noexcept -> const char* { case ChassisMode::SPIN_SLOW: return "SPIN_SLOW"; case ChassisMode::ALIGNMENT: return "ALIGNMENT"; case ChassisMode::ALIGNMENT_POWERED: return "ALIGNMENT_POWERED"; + case ChassisMode::WIRELESS_CHARGING: return "WIRELESS_CHARGING"; case ChassisMode::CLIMB: return "CLIMB"; } return "INVALID";