From 194047605d182161087ea8e960879cc916cabc78 Mon Sep 17 00:00:00 2001 From: guoxy06 <209087794@qq.com> Date: Wed, 22 Apr 2026 20:43:29 +0800 Subject: [PATCH] =?UTF-8?q?=E5=B0=8F=E9=99=80=E8=9E=BA=E5=89=8D=E7=9E=BB?= =?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 | 76 +++++++++++++++++++++++++++------ application/cmd/robot_cmd.c | 4 +- modules/remote/remote_control.c | 2 +- 3 files changed, 67 insertions(+), 15 deletions(-) diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index 6601cb2..338218a 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -31,7 +31,7 @@ #define HALF_TRACK_WIDTH (TRACK_WIDTH / 2.0f) // 半轮距 #define PERIMETER_WHEEL (RADIUS_WHEEL * 2 * PI) // 轮子周长 #if defined(CHASSIS_MCNAMEE_WHEEL) -#define CHASSIS_WHEEL_OFFSET 1.0f // 机器人底盘轮子修正偏移量 +//#define CHASSIS_WHEEL_OFFSET 1.0f // 机器人底盘轮子修正偏移量 #elif defined(CHASSIS_STEERING_WHEEL) #define CHASSIS_WHEEL_OFFSET 30.0f // 机器人底盘轮子修正偏移量 //#define CHASSIS_WHEEL_OFFSET 2.0f @@ -60,6 +60,9 @@ static Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据 static SuperCapInstance *cap; // 超级电容 static LKMotorInstance *motor_rf,*motor_lb; // 轮电机9015 static DJIMotorInstance *motor_steering_rf,*motor_steering_lb; // 舵电机6020 +static Gimbal_Upload_Data_s gimbal_feed_data; // 云台反馈数据 +static float last_imu_yaw = 0.0f; // IMU角度上一帧值 +static Subscriber_t *gimbal_feed_sub; // 新增,用于订阅云台的反馈数据 /* 用于自旋变速策略的时间变量 */ // static float t; @@ -157,12 +160,12 @@ void ChassisInit() .Ki = 0, .Kd = 0.1f, // 0.5 .DeadBand = 0.5, - .CoefA = 0.2, - .CoefB = 0.3, - .Improve = PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_DerivativeFilter | PID_Derivative_On_Measurement | PID_Integral_Limit | PID_Derivative_On_Measurement, - .IntegralLimit = 50, // 200 - .MaxOut = 1200, - .Derivative_LPF_RC = 0.01, // 0.01 + // .CoefA = 0.2, + // .CoefB = 0.3, + // .Improve = PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_DerivativeFilter | PID_Derivative_On_Measurement | PID_Integral_Limit | PID_Derivative_On_Measurement, + // .IntegralLimit = 50, // 200 + .MaxOut = 50, + // .Derivative_LPF_RC = 0.01, // 0.01 }; PIDInit(&chassis_follow_pid, &chassis_follow_pid_conf); @@ -199,6 +202,7 @@ void ChassisInit() #ifdef ONE_BOARD // 单板控制整车,则通过pubsub来传递消息 chassis_sub = SubRegister("chassis_cmd", sizeof(Chassis_Ctrl_Cmd_s)); + gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s)); // 新增 chassis_pub = PubRegister("chassis_feed", sizeof(Chassis_Upload_Data_s)); #endif // ONE_BOARD } @@ -370,6 +374,7 @@ void ChassisTask() { // 获取新的控制信息 #ifdef ONE_BOARD SubGetMessage(chassis_sub, &chassis_cmd_recv); + SubGetMessage(gimbal_feed_sub, &gimbal_feed_data); #endif #ifdef CHASSIS_BOARD chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(chasiss_can_comm); @@ -411,7 +416,7 @@ void ChassisTask() { chassis_cmd_recv.wz = -PIDCalculate(&chassis_follow_pid, single_round_offset, 0)/70.0f; break; case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略 - chassis_cmd_recv.wz = -0.35;//实验版 + chassis_cmd_recv.wz = 0.35;//实验版 break; default: break; @@ -420,12 +425,59 @@ void ChassisTask() { // 根据云台和底盘的角度offset将控制量映射到底盘坐标系上 // 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东.) 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); + static float rotate_offset_angle = 0.0f; // 小陀螺模式下的旋转角度补偿 + static float offset_angle_base = 0.0f; // 进入小陀螺模式时的offset_angle基准 + static uint8_t last_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; // chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta; - chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta;//实验版 - chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta; + + + if (chassis_cmd_recv.chassis_mode == CHASSIS_ROTATE) { + // 小陀螺模式：使用角速度积分来计算旋转补偿 + if (last_chassis_mode != CHASSIS_ROTATE) { + // 刚进入小陀螺模式时，初始化补偿角度/ + rotate_offset_angle = 0.0f; + offset_angle_base = chassis_cmd_recv.offset_angle; + last_imu_yaw = gimbal_feed_data.gimbal_imu_data.YawTotalAngle; // 添加这行,初始化IMU角度 + + } + float offset_angle_delta = chassis_cmd_recv.offset_angle - offset_angle_base; + // 使用角速度积分（累加），而不是角速度变化量 + //rotate_offset_angle -= chassis_cmd_recv.wz * 0.005f * RAD_2_DEGREE; + + // 使用IMU实际角度变化补偿 + float current_imu_yaw = gimbal_feed_data.gimbal_imu_data.YawTotalAngle; + float imu_delta = current_imu_yaw - last_imu_yaw; + if (imu_delta > 180.0f) imu_delta -= 360.0f; + else if (imu_delta < -180.0f) imu_delta += 360.0f; + rotate_offset_angle += imu_delta; + last_imu_yaw = current_imu_yaw; + + // 限制补偿角度范围，防止积分饱和 + if (rotate_offset_angle > 180.0f) rotate_offset_angle -= 360.0f; + if (rotate_offset_angle < -180.0f) rotate_offset_angle += 360.0f; + + // 使用补偿后的角度进行坐标变换 + //float effective_angle = chassis_cmd_recv.offset_angle - rotate_offset_angle;/ + float effective_angle = offset_angle_delta - rotate_offset_angle; + cos_theta = arm_cos_f32(effective_angle * DEGREE_2_RAD); + sin_theta = arm_sin_f32(effective_angle * DEGREE_2_RAD); + } else { + // 非小陀螺模式：正常使用offset_angle + rotate_offset_angle = 0.0f; // 重置补偿角度 + offset_angle_base = chassis_cmd_recv.offset_angle; // 添加这行,更新基准角度 + 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); + } + + last_chassis_mode = chassis_cmd_recv.chassis_mode; + + 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(); diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 9893389..cf77f1c 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -187,12 +187,12 @@ static void RemoteControlSet() // around = 0; chassis_cmd_send.chassis_mode = CHASSIS_ROTATE; gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; - around= chassis_cmd_send.offset_angle/360; + around= chassis_cmd_send.offset_angle/(-360); } else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘和云台分离,底盘保持不转动 { //gimbal_fetch_data.yaw_motor_total_angle = gimbal_fetch.yaw_motor_total_angle-around*360; - chassis_cmd_send.offset_angle = chassis_cmd_send.offset_angle - 360*around; + chassis_cmd_send.offset_angle = chassis_cmd_send.offset_angle + 360*around; chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; } diff --git a/modules/remote/remote_control.c b/modules/remote/remote_control.c index 8bc32b1..dd2507b 100644 --- a/modules/remote/remote_control.c +++ b/modules/remote/remote_control.c @@ -40,7 +40,7 @@ static void sbus_to_rc(const uint8_t *sbus_buf) rc_ctrl[TEMP].rc.rocker_l_ = (((sbus_buf[2] >> 6) | (sbus_buf[3] << 2) | (sbus_buf[4] << 10)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 2 rc_ctrl[TEMP].rc.rocker_l1 = (((sbus_buf[4] >> 1) | (sbus_buf[5] << 7)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 3 rc_ctrl[TEMP].rc.dial = ((sbus_buf[16] | (sbus_buf[17] << 8)) & 0x07FF) - RC_CH_VALUE_OFFSET; - rc_ctrl[TEMP].rc.rocker_l_= rc_ctrl[TEMP].rc.rocker_l_+2.5; + rc_ctrl[TEMP].rc.rocker_l_= rc_ctrl[TEMP].rc.rocker_l_+0.5; // 左侧拨轮 RectifyRCjoystick(); // 开关,0左1右