From 72df1a2654fc3c2ae60774e25c6fd139f6de8c2a Mon Sep 17 00:00:00 2001 From: guoxy06 <209087794@qq.com> Date: Wed, 29 Apr 2026 19:43:54 +0800 Subject: [PATCH] =?UTF-8?q?=E5=B0=8F=E9=99=80=E8=9E=BA=E6=97=97=E8=88=B0?= =?UTF-8?q?=E7=89=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- application/chassis/chassis.c | 47 +++++++++++++++++++++++++++++------ application/robot_def.h | 2 +- 2 files changed, 40 insertions(+), 9 deletions(-) diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index ce227cf..f22e2a1 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -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(); diff --git a/application/robot_def.h b/application/robot_def.h index 3a8845f..bd02153 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -27,7 +27,7 @@ /* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */ // 云台参数 #define YAW_CHASSIS_ALIGN_ECD -3595 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改 -#define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度 +#define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度 #define PITCH_HORIZON_ECD 3412 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改 #define PITCH_MAX_ANGLE 30 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) #define PITCH_MIN_ANGLE -20 // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)