Climbable infantry

docs/zh-cn/steering_infantry_can_id_audit.md

@@ -0,0 +1,81 @@
+# steering-infantry 电机 CAN ID 对照审计
+
+## 范围
+- 文件: `rmcs_ws/src/rmcs_core/src/hardware/steering-infantry.cpp`
+- 目标: 收集所有电机的发送报文 ID 与回调报文 ID，反推出电机 ID，并检查是否有偏差。
+
+## 反推规则
+- `DjiMotor` 采用 4 路合帧发送（`device::CanPacket8`）。
+- 对于 `can_id = 0x200` 的发送帧:
+  - `q0 -> 0x201`
+  - `q1 -> 0x202`
+  - `q2 -> 0x203`
+  - `q3 -> 0x204`
+- 对于 `can_id = 0x1FF / 0x1FE` 的发送帧（本项目代码中的使用方式）:
+  - `q0 -> 0x205`
+  - `q1 -> 0x206`
+  - `q2 -> 0x207`
+  - `q3 -> 0x208`
+- `LkMotor` 直接使用单独 `can_id` 发送与接收（非 4 路合帧电机映射）。
+
+## TopBoard 电机映射
+代码位置:
+- 构造/电机定义: `121-137`
+- 发送: `212-253`
+- 回调: `256-279`
+
+| 电机 | 发送总线/ID/槽位 | 回调 ID | 反推电机 ID | 结果 |
+|---|---|---|---|---|
+| `/gimbal/left_friction` | CAN1, `0x200`, `q2` | `0x203` | `3` | 一致 |
+| `/gimbal/right_friction` | CAN1, `0x200`, `q3` | `0x204` | `4` | 一致 |
+| `/gimbal/bullet_feeder` | CAN1, `0x1FF`, `q0` | `0x205` | `5` | 一致 |
+| `/gimbal/yaw` (LK) | CAN2, `0x141` | `0x141` | `0x141` | 一致 |
+| `/gimbal/pitch` (LK) | CAN2, `0x142` | `0x142` | `0x142` | 一致 |
+
+## BottomBoard_one 电机映射
+代码位置:
+- 构造/电机定义: `308-328`
+- 发送: `396-434`
+- 回调: `437-466`
+
+| 电机 | 发送总线/ID/槽位 | 回调 ID | 反推电机 ID | 结果 |
+|---|---|---|---|---|
+| `/chassis/right_front_wheel` (`chassis_wheel_motors_[1]`) | CAN1, `0x200`, `q0` | `0x201` | `1` | 一致 |
+| `/chassis/left_front_wheel` (`chassis_wheel_motors_[0]`) | CAN1, `0x200`, `q1` | `0x202` | `2` | 一致 |
+| `/chassis/left_front_steering` (`chassis_steering_motors_[0]`) | CAN1, `0x1FE`, `q1` | `0x206` | `6` | 一致 |
+| `/chassis/right_front_steering` (`chassis_steering_motors_[1]`) | CAN1, `0x1FE`, `q3` | `0x208` | `8` | 一致 |
+| `/chassis/climber/left_back_motor` (`chassis_back_climber_motor_[0]`) | CAN2, `0x200`, `q0` | `0x201` | `1` | 一致 |
+| `/chassis/climber/right_back_motor` (`chassis_back_climber_motor_[1]`) | CAN2, `0x200`, `q1` | `0x202` | `2` | 一致 |
+| `/chassis/climber/left_front_motor` (`chassis_front_climber_motor_[0]`) | CAN2, `0x200`, `q2` | `0x203` | `3` | 一致 |
+| `/chassis/climber/right_front_motor` (`chassis_front_climber_motor_[1]`) | CAN2, `0x200`, `q3` | `0x204` | `4` | 一致 |
+
+## BottomBoard_two 电机映射
+代码位置:
+- 构造/电机定义: `491-507`
+- 发送: `563-591`
+- 回调: `594-610`
+
+| 电机 | 发送总线/ID/槽位 | 回调 ID | 反推电机 ID | 结果 |
+|---|---|---|---|---|
+| `/chassis/right_back_wheel` (`chassis_wheel_motors2_[1]`) | CAN1, `0x200`, `q2` | `0x203` | `3` | 一致 |
+| `/chassis/left_back_wheel` (`chassis_wheel_motors2_[0]`) | CAN1, `0x200`, `q3` | `0x204` | `4` | 一致 |
+| `/chassis/left_back_steering` (`chassis_steering_motors2_[0]`) | CAN1, `0x1FE`, `q0` | `0x205` | `5` | 一致 |
+| `/chassis/right_back_steering` (`chassis_steering_motors2_[1]`) | CAN1, `0x1FE`, `q2` | `0x207` | `7` | 一致 |
+
+## 总结
+- 结论 1: 所有电机都能在代码中找到“发送 -> 回调”的闭环映射，未发现直接冲突或漏接收。
+- 结论 2: 从代码反推，当前电机 ID 规划为:
+  - TopBoard: 摩擦轮 `3/4`，拨弹 `5`，LK 云台 `0x141/0x142`
+  - BottomBoard_one: 轮 `1/2`，前转向 `6/8`，攀爬 `1/2/3/4`（CAN2）
+  - BottomBoard_two: 后轮 `3/4`，后转向 `5/7`
+
+## 偏差/问题反馈
+- 问题 1（注释已提示）: 文件头标注“电机ID没更改”，说明该映射可能为占位或沿用旧车配置，不能等同于当前实车真值。
+- 问题 2（可维护性）: 多处左右轮顺序与 ID 顺序是“交叉映射”（例如右前轮是 ID1、左前轮是 ID2），逻辑上可行，但后续维护容易误改。
+- 问题 3（非 ID 但会影响状态）: `TopBoard::update()` 中调用了 `gimbal_pitch_motor_.update_status()`，但未调用 `gimbal_yaw_motor_.update_status()`，可能导致 yaw 角度状态不更新。
+
+## 建议
+- 用实车实测确认最终 ID:
+  1. 在各 `can*_receive_callback` 临时打印所有 `can_id`。
+  2. 单电机动作，建立“物理电机 <-> can_id”表。
+  3. 以实测表覆盖本文件映射，再同步更新发送槽位与回调分发。

rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml

@@ -0,0 +1,266 @@
+rmcs_executor:
+  ros__parameters:
+    update_rate: 1000.0
+    components:
+      - rmcs_core::hardware::ClimbableInfantry -> Infantry_hardware
+
+      - rmcs_core::referee::Status -> referee_status
+      - rmcs_core::referee::command::Interaction -> referee_interaction
+      - rmcs_core::referee::command::interaction::Ui -> referee_ui
+      - rmcs_core::referee::app::ui::UIClimbableInfantry -> referee_ui_infantry
+      - rmcs_core::referee::Command -> referee_command
+
+      - rmcs_core::controller::gimbal::SimpleGimbalController -> gimbal_controller
+      - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller
+      - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller
+      - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_controller
+      - rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller
+
+      # - rmcs_core::controller::gimbal::PlayerViewer -> gimbal_player_viewer_controller
+      # - rmcs_core::controller::pid::ErrorPidController -> viewer_angle_pid_controller
+
+      - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller
+      - rmcs_core::controller::shooting::HeatController -> heat_controller
+      - rmcs_core::controller::shooting::BulletFeederController17mm -> bullet_feeder_controller
+      # - rmcs_core::controller::shooting::ShootingRecorder -> shooting_recorder
+      - rmcs_core::controller::pid::PidController -> left_friction_velocity_pid_controller
+      - rmcs_core::controller::pid::PidController -> right_friction_velocity_pid_controller
+      - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller
+
+      - rmcs_core::controller::chassis::SteeringWheelStatus -> steering_wheel_status
+      - rmcs_core::controller::chassis::ClimbableInfantryChassisController -> chassis_controller
+      - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller
+      - rmcs_core::controller::chassis::SteeringWheelController -> steering_wheel_controller
+      - rmcs_core::controller::chassis::ClimbableInfantryChassisClimberController -> climber_controller
+
+      # - rmcs_auto_aim::AutoAimInitializer -> auto_aim_initializer
+      # - rmcs_auto_aim::AutoAimController -> auto_aim_controller
+
+      # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster
+
+Infantry_hardware:
+  ros__parameters:
+    board_serial_top_board: "D4-632C-37BC-6A17-ACA2-E3FE-91E9"
+    board_serial_bottom_board: "AF-EEF5-24BA-6675-F1B1-C797-50AF-869C-870E"
+    yaw_motor_zero_point: 7570
+    pitch_motor_zero_point: 24243
+    # top_yaw_motor_zero_point: 58744
+    # viewer_motor_zero_point: 3030
+    # external_imu_port: /dev/ttyUSB0
+    # bullet_feeder_motor_zero_point: 15111
+    left_front_zero_point: 5806
+    right_front_zero_point: 5092
+    left_back_zero_point: 3759
+    right_back_zero_point: 3103
+
+
+value_broadcaster:
+  ros__parameters:
+    forward_list:
+    - /gimbal/yaw/velocity
+    - /gimbal/yaw/control_velocity
+    # - /gimbal/yaw/control_torque
+      # - /gimbal/left_friction/velocity
+      # - /gimbal/right_friction/velocity
+        # - /chassis/pitch_imu
+        # - /gimbal/pitch/velocity_imu
+        # - /gimbal/pitch/angle
+        # - /gimbal/yaw/angle
+        # - /chassis/climber/right_back_motor/control_torque
+        # - /chassis/climber/left_back_motor/control_torque
+        # - /gimbal/putter/velocity
+        # - /gimbal/first_left_friction/velocity
+        # - /gimbal/first_second_friction/velocity
+        # - /gimbal/second_left_friction/velocity
+        # - /gimbal/second_right_friction/velocity
+        # - /gimbal/first_left_friction/control_torque
+        # - /gimbal/first_second_friction/control_torque
+        # - /gimbal/second_left_friction/control_torque
+        # - /gimbal/second_right_friction/control_torque
+      # - /chassis/left_front_steering/angle
+      # - /chassis/left_back_steering/angle
+      # - /chassis/right_back_steering/angle
+      # - /chassis/right_front_steering/angle  
+      # - /chassis/left_front_steering/control_torque
+      # - /chassis/left_back_steering/control_torque
+      # - /chassis/right_back_steering/control_torque
+      # - /chassis/right_front_steering/control_torque  
+      # - /gimbal/pitch/control_torque
+      # - /gimbal/pitch/velocity
+      # - /gimbal/pitch/control_velocity
+      # - /gimbal/pitch/control_angle_error
+      # - /gimbal/bullet_feeder/phase_zero
+      # - /gimbal/bullet_feeder/alignment_error
+      # - /chassis/climber/left_back_motor/torque
+      # - /chassis/climber/right_back_motor/torque
+      # - /chassis/climber/left_back_motor/velocity
+      # - /chassis/climber/right_back_motor/velocity
+      # - /chassis/climber/right_back_motor/control_torque
+      # - /chassis/climber/left_back_motor/control_torque
+
+climber_controller:
+  ros__parameters:
+    front_climber_velocity: 60.0
+    back_climber_velocity: 20.0 #13.0
+    auto_climb_dash_velocity: 4.0 #3.0
+    auto_climb_support_retract_velocity: 40.0 #47.0
+    sync_coefficient: 0.8
+    first_stair_approach_pitch: 0.590
+    second_stair_approach_pitch: 0.320
+    front_kp: 1.0
+    front_ki: 0.0
+    front_kd: 0.5
+    back_kp: 1.0
+    back_ki: 0.0
+    back_kd: 0.0
+
+gimbal_controller:
+  ros__parameters:
+    upper_limit: -0.54
+    lower_limit: 0.274
+
+pitch_angle_pid_controller:
+  ros__parameters:
+    measurement: /gimbal/pitch/control_angle_error
+    control: /gimbal/pitch/control_velocity
+    kp: 15.0 #5.0
+    ki: 0.0
+    kd: 0.0 #1.5
+
+pitch_velocity_pid_controller:
+  ros__parameters:
+    measurement: /gimbal/pitch/velocity_imu
+    setpoint: /gimbal/pitch/control_velocity
+    control: /gimbal/pitch/control_torque
+    kp: 40.0 #
+    ki: 0.00
+    kd: 0.8 #
+
+yaw_angle_pid_controller:
+  ros__parameters:
+    measurement: /gimbal/yaw/control_angle_error
+    control: /gimbal/yaw/control_velocity
+    kp: 20.0 #20.00
+    ki: 0.0
+    kd: 0.9 #0.9
+
+yaw_velocity_pid_controller:
+  ros__parameters:
+    measurement: /gimbal/yaw/velocity_imu
+    setpoint: /gimbal/yaw/control_velocity
+    control: /gimbal/yaw/control_torque
+    kp: 2.5 #2.5
+    ki: 0.00
+    kd: 0.6 #0.6
+
+friction_wheel_controller:
+  ros__parameters:
+    friction_wheels:
+      - /gimbal/left_friction
+      - /gimbal/right_friction
+    friction_velocities:
+      - 590.0
+      - 590.0
+    friction_soft_start_stop_time: 0.3
+
+heat_controller:
+  ros__parameters:
+    heat_per_shot: 10000
+    reserved_heat: 5000
+
+bullet_feeder_controller:
+  ros__parameters:
+    bullets_per_feeder_turn: 9.0
+    shot_frequency: 60.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
+
+shooting_recorder:
+  ros__parameters:
+    friction_wheel_count: 2
+    log_mode: 1 # 1: trigger, 2: timing
+
+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 #0.003436926 #0.0016
+    ki: 0.00
+    kd: 0.009373434 #0.009373434 #0.001
+
+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 #0.0016
+    ki: 0.00
+    kd: 0.009373434 #0.009373434 #0.001
+
+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.7
+    ki: 0.0
+    kd: 0.0
+
+steering_wheel_status:
+  ros__parameters:
+    vehicle_radius: 0.28284271247462
+    wheel_radius: 0.055
+
+steering_wheel_controller:
+  ros__parameters:
+    mess: 22.0
+    moment_of_inertia: 1.08
+    vehicle_radius: 0.28284271247462
+    wheel_radius: 0.055
+    friction_coefficient: 0.6
+    k1: 2.958580e+00
+    k2: 3.082190e-03
+    no_load_power: 11.37
+
+auto_aim_controller:
+  ros__parameters:
+    # capture
+    use_video: false # If true, use video stream instead of camera.
+    video_path: "/workspaces/RMCS/rmcs_ws/resources/1.avi"
+    exposure_time: 1
+    invert_image: false
+    # identifier
+    armor_model_path: "/models/mlp.onnx"
+    # pnp
+    fx: 1.722231837421459e+03
+    fy: 1.724876404292754e+03
+    cx: 7.013056440882832e+02
+    cy: 5.645821718351237e+02
+    k1: -0.064232403853946
+    k2: -0.087667493884102
+    k3: 0.792381808294582
+    # tracker
+    armor_predict_duration: 500
+    # controller
+    gimbal_predict_duration: 100
+    yaw_error: 0.
+    pitch_error: 0.
+    shoot_velocity: 28.0
+    predict_sec: 0.095
+    # etc
+    buff_predict_duration: 200
+    buff_model_path: "/models/buff_nocolor_v6.onnx"
+    omni_exposure: 1000.0
+    record_fps: 120
+    debug: false # Setup in actual using.Debug mode is used when referee is not ready
+    debug_color: 0 # 0 For blue while 1 for red. mine
+    debug_robot_id: 4
+    debug_buff_mode: false
+    record: false
+    raw_img_pub: false # Set false in actual use
+    image_viewer_type: 2