diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index 1c64f51..7109ad5 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -145,10 +145,10 @@ void ChassisInit() .motor_type = GM6020, }; - chassis_motor_steering_config.can_init_config.tx_id = 1; + chassis_motor_steering_config.can_init_config.tx_id = 2; motor_steering_rf = DJIMotorInit(&chassis_motor_steering_config); - chassis_motor_steering_config.can_init_config.tx_id = 2; + chassis_motor_steering_config.can_init_config.tx_id = 1; motor_steering_lb = DJIMotorInit(&chassis_motor_steering_config); PID_Init_Config_s chassis_follow_pid_conf = { @@ -172,11 +172,14 @@ void ChassisInit() SuperCap_Init_Config_s cap_conf = { .can_config = { .can_handle = &hcan2, - .tx_id = 0x302, // 超级电容默认接收id - .rx_id = 0x301, // 超级电容默认发送id,注意tx和rx在其他人看来是反的 + .tx_id = 0x001, // 超级电容默认接收id + .rx_id = 0x100, // 超级电容默认发送id,注意tx和rx在其他人看来是反的 }}; cap = SuperCapInit(&cap_conf); // 超级电容初始化 + // (新增)设置超级电容功率补偿,例如补偿5W,可根据实际需求调整 + SuperCapSetPowerOffset(cap, 5); + // 发布订阅初始化,如果为双板,则需要can comm来传递消息 #ifdef CHASSIS_BOARD Chassis_IMU_data = INS_Init(); // 底盘IMU初始化 @@ -223,16 +226,43 @@ void ChassisInit() */ static void MinmizeRotation(float *angle, const float *last_angle, float *speed) { - float rotation = *angle - *last_angle; - ANGLE_LIMIT_360_TO_180_ABS(rotation); - if (rotation > 90) { - *angle -= 180; + float target_angle = *angle; + float actual_angle = *last_angle; + float rotation = target_angle - actual_angle; + float norm_rotation = rotation; + while (norm_rotation > 180.0f) { + norm_rotation -= 360.0f; + } + while (norm_rotation < -180.0f) { + norm_rotation += 360.0f; + } + float threshold = 90.0f; + float target_single=fmodf(target_angle,360.0f); + if(target_single<0.0f) target_single+=360.0f; + + float current_single=fmodf(actual_angle,360.0f); + if(current_single<0.0f) current_single+=360.0f; + + + if (norm_rotation > threshold) { + int32_t round_diff = (int32_t)((target_angle-actual_angle) / 360.0f); + *angle = actual_angle + norm_rotation-180.0f+round_diff*360.0f; *speed = -(*speed); - } else if (rotation < -90) { - *angle += 180; + }else if(norm_rotation<-threshold) { + int32_t round_diff = (int32_t)((target_angle-actual_angle) / 360.0f); + *angle = actual_angle + norm_rotation+180.0f+round_diff*360.0f; *speed = -(*speed); } - ANGLE_LIMIT_360_TO_180_ABS(*angle); + // float rotation = *angle - *last_angle; + // ANGLE_LIMIT_360_TO_180_ABS(rotation); + // if (rotation > 90) { + // *angle -= 180; + // *speed = -(*speed); + // } else if (rotation < -90) { + // *angle += 180; + // *speed = -(*speed); + // } + // ANGLE_LIMIT_360_TO_180_ABS(*angle); } /** @@ -367,7 +397,17 @@ void ChassisTask() { chassis_cmd_recv.wz = 0; break; case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出 - chassis_cmd_recv.wz = PIDCalculate(&chassis_follow_pid, chassis_cmd_recv.offset_angle, 0); + //chassis_cmd_recv.wz = PIDCalculate(&chassis_follow_pid, chassis_cmd_recv.offset_angle, 0); + float single_round_offset = chassis_cmd_recv.offset_angle; + if(single_round_offset > 180.0f) + { + single_round_offset -= 360.0f; + } + else if(single_round_offset < -180.0f) + { + single_round_offset += 360.0f; + } + chassis_cmd_recv.wz = PIDCalculate(&chassis_follow_pid, single_round_offset, 0)/90; break; case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略 chassis_cmd_recv.wz = 0.75;