refactor: manual mode from absolute setpoint to velocity control, remove auto-enable at motor init

Manual mode now receives angular velocity (degrees/s) via 0xA6 frame instead of
absolute angle reference. The firmware integrates velocity * dt into gimbal angle ref.
Motors are no longer auto-enabled at DMMotorInit — they stay disabled until a mode
switch out of DEBUG. Extract ROBOT_CMD_DELAY macro to replace magic number.
Fix "volity" typo → "velocity" and update docs/protocol comments accordingly.
This commit is contained in:
2026-06-02 16:27:07 +08:00
parent 88702e2832
commit da6acdcc89
8 changed files with 30 additions and 30 deletions
+7 -7
View File
@@ -69,7 +69,7 @@ main()
│ │
│ AUTO: 上位机 0xA5 帧 → error_x/y → KF → vision PID│
│ → 角度增量 → 累计 angle ref │
│ MANUAL: 上位机 0xA6 帧 → yaw_ref/pitch_ref 直接设定 │
│ MANUAL: 上位机 0xA6 帧 → yaw/pitch 角速度 → 角度增量积分 │
│ DEBUG: 上位机 0xA8 帧 → 实时更新 PID 参数 │
│ │
│ ProcessCommand() 处理模式切换 / 激光控制 │
@@ -108,7 +108,7 @@ main()
| 帧头 | 长度 | 用途 |
|------|------|------|
| 0xA5 | 10 | 自动/视觉追踪数据帧 (error_x, error_y) |
| 0xA6 | 11 | 手动控制数据帧 (zero_force, yaw_ref, pitch_ref) |
| 0xA6 | 11 | 手动控制数据帧 (zero_force, yaw_velocity, pitch_velocity) |
| 0xA7 | 3 | 命令帧 (模式切换, 激光) |
| 0xA8 | 50 | 调试调参帧 (12 个 float PID 参数) |
@@ -127,7 +127,7 @@ main()
| 模式 | 值 | 说明 |
|------|-----|------|
| AUTO | 0 | 视觉误差 → KF → vision PID → 角度增量 |
| MANUAL | 1 | 上位机直接下发角度参考值 |
| MANUAL | 1 | 上位机下发角速度,固件积分累加角度参考 |
| DEBUG | 2 | 失能电机,接收 PID 参数实时调参 |
### 数据流
@@ -135,14 +135,14 @@ main()
```
上位机 USART10 → DecodeVision() → vision_data (全局结构体)
├── 0xA5 → auto_updated=1, error_x/y
├── 0xA6 → manual_updated=1, zero_force, yaw_ref/pitch_ref
├── 0xA6 → manual_updated=1, zero_force, yaw/pitch velocity
├── 0xA7 → cmd_pending=1, cmd
└── 0xA8 → debug_updated=1, 12 个 PID 参数
robot_cmd 每轮:
ProcessCommand() → 模式切换 / 激光
AUTO: vision_data.error_x/y → KF → vision PID → 角度增量
MANUAL: vision_data.yaw_ref/pitch_ref → 直接设定
MANUAL: vision_data.yaw/pitch velocity → 角度增量积分
DEBUG: vision_data PID 参数 → 直接写入 vision PID, 发布 motor_pid 话题
PubPushMessage(gimbal_cmd) → gimbal 执行
```
@@ -256,7 +256,7 @@ DMMotorControl() @ 1kHz:
│ RobotCMD │
│ ProcessCommand() │ ← 模式切换/激光
│ VisionTracking() │ ← KF → vision PID → 角度增量
│ ManualMode() │ ← 直接角度参考
│ ManualMode() │ ← 角速度积分
│ DebugMode() │ ← 实时 PID 调参
│ PubPushMessage() │
└───────┬───────┬────────┘
@@ -324,7 +324,7 @@ typedef struct {
float error_x, error_y; // 自动模式: 归一化误差
uint8_t auto_updated;
uint8_t zero_force; // 手动模式: 零力标志
float yaw_ref, pitch_ref; // 手动模式: 角度参考
float yaw_degree_velocity, pitch_degree_velocity; // 手动模式: 角速度
uint8_t manual_updated;
// 调试模式数据
float vision_yaw_Kp, vision_yaw_Ki, vision_yaw_Kd;