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

小陀螺旗舰版

Browse Source
This commit is contained in:
guoxy06
2026-04-29 19:43:54 +08:00
parent 428956e3bd
commit 72df1a2654
2 changed files with 40 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

47
application/chassis/chassis.c
View File

@@ -284,7 +284,8 @@ static void SteeringWheelCalculate()
vt_rf = 0, vt_lb = 0;
} else {
// 生成预计算变量，减少计算量，空间换时间
float w = chassis_cmd_recv.wz * CHASSIS_WHEEL_OFFSET * SQRT2;
//float w = chassis_cmd_recv.wz * CHASSIS_WHEEL_OFFSET * SQRT2;
float w = chassis_cmd_recv.wz * CHASSIS_WHEEL_OFFSET ;
// float temp_x = chassis_vx - w, temp_y = chassis_vy - w;
// arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_lf); // lf：y- , x-
// temp_y = chassis_vy + w; // 重复利用变量,temp_x = chassis_vy - w;与上次相同因此注释
@@ -408,25 +409,55 @@ void ChassisTask() {
{
single_round_offset += 360.0f;
}
chassis_cmd_recv.wz = -PIDCalculate(&chassis_follow_pid, single_round_offset, 0)/70.0f;
chassis_cmd_recv.wz = -PIDCalculate(&chassis_follow_pid, single_round_offset, 0)/60.0f;
break;
case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
chassis_cmd_recv.wz = 0.95;//实验版
chassis_cmd_recv.wz = 1.00;//实验版
break;
default:
break;
}
static float sin_theta, cos_theta;
cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
chassis_vx = (chassis_cmd_recv.vx * cos_theta + chassis_cmd_recv.vy * sin_theta)/4;
chassis_vy = (-chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta)/4;
// ------------------ 【修改部分：加入相位滞后动态补偿】 ------------------
// 提取当前的云台-底盘夹角
float real_offset_angle = chassis_cmd_recv.offset_angle;
// 如果处于小陀螺模式（或者 wz 不为 0），根据旋转速度进行超前补偿
if (chassis_cmd_recv.chassis_mode == CHASSIS_ROTATE )
{
// 补偿系数 K 需要实车微调。
// 根据你的描述 wz=0.95 时偏差约45度，初始 K 值可以设为 45.0 / 0.95 ≈ 47.3f
float lag_compensation_K = 45.0f;//47.3f;
// 符号(+或-)取决于你的电机正方向、底盘旋转正方向，如果不正就改成减号
real_offset_angle -= (chassis_cmd_recv.wz * lag_compensation_K);
cos_theta = arm_cos_f32(real_offset_angle * DEGREE_2_RAD);
sin_theta = arm_sin_f32(real_offset_angle * DEGREE_2_RAD);
}else if (chassis_cmd_recv.chassis_mode == CHASSIS_FOLLOW_GIMBAL_YAW)
{
cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
}
// ----------------------------------------------------------------------
chassis_vx = (chassis_cmd_recv.vx * cos_theta + chassis_cmd_recv.vy * sin_theta)/4;
chassis_vy = (-chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta)/4;
// 使用补偿后的角度进行坐标系投影
// static float sin_theta, cos_theta;
// cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
// sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
// chassis_vx = (chassis_cmd_recv.vx * cos_theta + chassis_cmd_recv.vy * sin_theta)/4;
// chassis_vy = (-chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta)/4;
// 根据控制模式进行正运动学解算,计算底盘输出
SteeringWheelCalculate();
// MecanumCalculate();
// 根据裁判系统的反馈数据和电容数据对输出限幅并设定闭环参考值
LimitChassisOutput();