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:
@@ -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;
|
||||
|
||||
Reference in New Issue
Block a user