From da6acdcc892b4254339bf772c99c04ea537f5d12 Mon Sep 17 00:00:00 2001 From: SkyLi Date: Tue, 2 Jun 2026 16:27:07 +0800 Subject: [PATCH] refactor: manual mode from absolute setpoint to velocity control, remove auto-enable at motor init MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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. --- README.md | 14 +++++++------- application/cmd/robot_cmd.c | 12 ++++++------ application/robot_def.h | 3 +++ application/robot_task.h | 2 +- docs/host_comm_guide.md | 12 ++++++------ modules/master_machine/master_machine.c | 4 ++-- modules/master_machine/master_machine.h | 10 +++++----- modules/motor/DMmotor/dmmotor.c | 3 --- 8 files changed, 30 insertions(+), 30 deletions(-) diff --git a/README.md b/README.md index 12e9053..079e8ac 100644 --- a/README.md +++ b/README.md @@ -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; diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index cf8d182..b88607e 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -247,11 +247,11 @@ static void VisionTracking() Kalman_Filter_Update(&pitch_error_kf); filtered_pitch_error = pitch_error_kf.FilteredValue[0]; - float yaw_increment = PIDCalculate(&vision_yaw_pid, filtered_yaw_error, 0); - float pitch_increment = PIDCalculate(&vision_pitch_pid, filtered_pitch_error, 0); + float yaw_degree_velocity = PIDCalculate(&vision_yaw_pid, filtered_yaw_error, 0); + float pitch_degree_velocity = PIDCalculate(&vision_pitch_pid, filtered_pitch_error, 0); - gimbal_yaw_ref += 0.001f * yaw_increment; - gimbal_pitch_ref += 0.001f * pitch_increment; + gimbal_yaw_ref += 0.001f *ROBOT_CMD_DELAY * yaw_degree_velocity; + gimbal_pitch_ref += 0.001f *ROBOT_CMD_DELAY * pitch_degree_velocity; } if (gimbal_pitch_ref > 30.0f) @@ -269,8 +269,8 @@ static void ManualMode() if (vision_data.manual_updated) { vision_data.manual_updated = 0; - gimbal_yaw_ref = vision_data.yaw_ref; - gimbal_pitch_ref = vision_data.pitch_ref; + gimbal_yaw_ref += 0.001f *ROBOT_CMD_DELAY * vision_data.yaw_degree_velocity; + gimbal_pitch_ref += 0.001f *ROBOT_CMD_DELAY * vision_data.pitch_degree_velocity; } if (gimbal_pitch_ref > 30.0f) diff --git a/application/robot_def.h b/application/robot_def.h index 6a9ec91..440ade5 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -5,6 +5,9 @@ #include "ins_task.h" #include "stdint.h" +// 机器人核心任务的延时时间(ms) +#define ROBOT_CMD_DELAY 2 + /* 开发板类型定义 */ #define ONE_BOARD // 单板控制整车 diff --git a/application/robot_task.h b/application/robot_task.h index 18c08a5..f3145b8 100644 --- a/application/robot_task.h +++ b/application/robot_task.h @@ -113,6 +113,6 @@ __attribute__((noreturn)) void StartROBOTTASK(void *argument) robot_dt = DWT_GetTimeline_ms() - robot_start; if (robot_dt > 5) LOGERROR("[freeRTOS] ROBOT core Task DELAY dt=%.3f", robot_dt); - osDelay(2); + osDelay(ROBOT_CMD_DELAY); } } diff --git a/docs/host_comm_guide.md b/docs/host_comm_guide.md index b69a25d..145e967 100644 --- a/docs/host_comm_guide.md +++ b/docs/host_comm_guide.md @@ -55,28 +55,28 @@ def send_auto_tracking(ser, error_x: float, error_y: float): ## 2. 手动控制数据帧 0xA6 (11 字节) -**用途**: 手动模式下发送角度目标值。 +**用途**: 手动模式下发送角速度目标值。 ``` -[SOF:0xA6][zero_force:1][yaw_ref:4][pitch_ref:4][checksum:1] +[SOF:0xA6][zero_force:1][yaw_velocity:4][pitch_velocity:4][checksum:1] ``` | 偏移 | 长度 | 类型 | 含义 | |---|---|---|---| | 0 | 1 | uint8 | 帧头 0xA6 | | 1 | 1 | uint8 | 零力标志 (0=正常, 1=零力) | -| 2 | 4 | float | Yaw 角度参考 (degrees) | -| 6 | 4 | float | Pitch 角度参考 (degrees) | +| 2 | 4 | float | Yaw 角速度 (degrees/s) | +| 6 | 4 | float | Pitch 角速度 (degrees/s) | | 10 | 1 | uint8 | checksum | **Python 写入示例**: ```python -def send_manual_control(ser, yaw_deg: float, pitch_deg: float, zero_force: int = 0): +def send_manual_control(ser, yaw_deg_per_s: float, pitch_deg_per_s: float, zero_force: int = 0): data = bytearray() data.append(0xA6) data.append(0 if zero_force else 0) - data.extend(struct.pack('motor_daemon = DaemonRegister(&conf); - /* enable motor */ - DMMotorSetMode(DM_CMD_ENABLE_MODE, motor); - dm_motor_instance[idx++] = motor; return motor; }