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:
@@ -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)
|
||||
|
||||
@@ -5,6 +5,9 @@
|
||||
#include "ins_task.h"
|
||||
#include "stdint.h"
|
||||
|
||||
// 机器人核心任务的延时时间(ms)
|
||||
#define ROBOT_CMD_DELAY 2
|
||||
|
||||
/* 开发板类型定义 */
|
||||
#define ONE_BOARD // 单板控制整车
|
||||
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user