TuxMonkey/half-steering-basic-framework
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/half-steering-basic-framework synced 2026-10-24 01:52:20 +08:00
Code Issues Packages Projects Releases Wiki Activity

reset again pro

Browse Source
This commit is contained in:
fcl
2026-03-07 19:52:36 +08:00
parent f41a2f3a4a
commit a99307cd04
5 changed files with 43 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
application/cmd/robot_cmd.c
View File

@@ -80,11 +80,24 @@ void RobotCMDInit()
* @brief 根据gimbal app传回的当前电机角度计算和零位的误差
* 单圈绝对角度的范围是0~360,说明文档中有图示
*
*/
*/static float base_total_angle = 0.0f; // 用于计算偏移的基准角度
static void CalcOffsetAngle()
{
static float last_offset_angle = 0;
const float smooth_factor = 0.3f; // 平滑因子,0~1之间,值越大平滑效果越明显
//const float deadband = 0.5f; // 死区值
// 新增：跟踪模式变化，用于重置基准角度
static chassis_mode_e last_chassis_mode = CHASSIS_NO_FOLLOW;//
// 当从旋转模式切换到跟随模式时，重置基准角度
if (last_chassis_mode == CHASSIS_ROTATE && chassis_cmd_send.chassis_mode == CHASSIS_FOLLOW_GIMBAL_YAW)
{
base_total_angle = gimbal_fetch_data.yaw_motor_total_angle;
last_offset_angle = 0.0f; // 同时重置历史偏移值
}
float new_offset = gimbal_fetch_data.yaw_motor_total_angle - YAW_ALIGN_ANGLE;
#if YAW_ECD_GREATER_THAN_4096
new_offset = -new_offset;
@@ -94,8 +107,25 @@ static void CalcOffsetAngle()
new_offset -= 360.0f;
else if (angle_diff < -180.0f)
new_offset += 360.0f;
chassis_cmd_send.offset_angle = last_offset_angle *(1.0f - smooth_factor) + new_offset * smooth_factor;
/////
// 应用死区
//if (fabs(new_offset) < deadband) {
// new_offset = 0.0f;
// } else if (new_offset > 0) {
// new_offset -= deadband;
//} else {
// new_offset += deadband;
//}//////
chassis_cmd_send.offset_angle = last_offset_angle *(1.0f - smooth_factor) + new_offset * smooth_factor;
last_offset_angle = chassis_cmd_send.offset_angle;
// 更新模式状态
last_chassis_mode = chassis_cmd_send.chassis_mode;
// 当处于旋转模式时，更新基准角度以防止累积过大误差
if (chassis_cmd_send.chassis_mode == CHASSIS_ROTATE)
{
base_total_angle = gimbal_fetch_data.yaw_motor_total_angle;
}
// static float angle;
// static float last_diff = 0.0f; // 用于在±180°附近保持上次符号，避免来回跳变
// /* 使用 yaw 电机的单圈角度与 `YAW_ALIGN_ANGLE` 对齐（保持原实现的参考系）