diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index 17d2726..ce227cf 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -60,9 +60,6 @@ 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; @@ -202,7 +199,6 @@ 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 } @@ -374,7 +370,6 @@ 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); @@ -416,68 +411,17 @@ 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.95;//实验版 break; default: break; } - // 根据云台和底盘的角度offset将控制量映射到底盘坐标系上 - // 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东.) static float sin_theta, cos_theta; - 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; - - - 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;//实验版/ + 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(); @@ -489,7 +433,7 @@ void ChassisTask() { // 根据电机的反馈速度和IMU(如果有)计算真实速度 EstimateSpeed(); - // // 获取裁判系统数据 建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送 + // // 获取裁判系统数据 建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送/ // // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red // chassis_feedback_data.enemy_color = referee_data->GameRobotState.robot_id > 7 ? 1 : 0; // // 当前只做了17mm热量的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况 diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index cf77f1c..fdf2002 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -87,9 +87,17 @@ void RobotCMDInit() static void CalcOffsetAngle() { static float last_offset_angle = 0; - const float smooth_factor = 0.3f; // 平滑因子,0~1之间,值越大平滑效果越明显 + //const float smooth_factor = 0.3f; // 平滑因子, - const float deadband = 3.0f; // 死区值 + float smooth_factor; + // 小陀螺模式下使用更大的平滑因子，让offset_angle更快跟随云台 + if (chassis_cmd_send.chassis_mode == CHASSIS_ROTATE) { + smooth_factor = 0.9f; // 快速响应 + } else { + smooth_factor = 0.3f; // 平滑过渡 0~1之间,值越大平滑效果越明显 + } + + //const float deadband = 3.0f; // 死区值 // 新增：跟踪模式变化，用于重置基准角度 // static chassis_mode_e last_chassis_mode = CHASSIS_NO_FOLLOW;// // @@ -111,14 +119,14 @@ static void CalcOffsetAngle() else if (angle_diff < -180.0f) new_offset += 360.0f; ///// - // 应用死区 - if (fabs(new_offset) < deadband) { - new_offset = 0.0f; - } else if (new_offset > 0) { - new_offset -= deadband; - } else { - new_offset += deadband; - }////// + // // 应用死区 + // 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; // // 更新模式状态 diff --git a/modules/remote/remote_control.c b/modules/remote/remote_control.c index dd2507b..a57e655 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_+0.5; + rc_ctrl[TEMP].rc.rocker_l_= rc_ctrl[TEMP].rc.rocker_l_+1.5; // 左侧拨轮 RectifyRCjoystick(); // 开关,0左1右