From 8bc4c0bb1ffde52653d614c62cc44b5a4b274317 Mon Sep 17 00:00:00 2001 From: guoxy06 <209087794@qq.com> Date: Sun, 12 Apr 2026 16:27:44 +0800 Subject: [PATCH] =?UTF-8?q?=E6=94=B9=E5=BA=95=E7=9B=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- application/chassis/chassis.c | 228 +++++++++++++--------------------- 1 file changed, 84 insertions(+), 144 deletions(-) diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index 471417e..267ddd7 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -1,3 +1,4 @@ + /** * @file chassis.c * @author NeoZeng neozng1@hnu.edu.cn @@ -56,15 +57,15 @@ static referee_info_t* referee_data; // 用于获取裁判系统的数据 static Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据传入此结构体的对应变量中，UI会自动检测是否变化，对应显示UI static SuperCapInstance *cap; // 超级电容 -static LKMotorInstance *motor_lb,*motor_rf; // 轮电机9015 -static DJIMotorInstance *motor_steering_lb,*motor_steering_rf; // 舵电机6020 +static LKMotorInstance *motor_lf,*motor_rb; // 轮电机9015 +static DJIMotorInstance *motor_steering_lf,*motor_steering_rb; // 舵电机6020 /* 用于自旋变速策略的时间变量 */ // static float t; /* 私有函数计算的中介变量,设为静态避免参数传递的开销 */ static float chassis_vx, chassis_vy; // 将云台系的速度投影到底盘 -static float vt_lb, vt_rf; // 底盘速度解算后的临时输出,待进行限幅 -static float at_lb, at_rf; // 底盘的角度解算后的临时输出,待进行限幅 +static float vt_lf, vt_rb; // 底盘速度解算后的临时输出,待进行限幅 +static float at_lf, at_rb; // 底盘的角度解算后的临时输出,待进行限幅 static PIDInstance chassis_follow_pid; // 底盘跟随云台的PID void ChassisInit() @@ -74,7 +75,7 @@ void ChassisInit() .can_init_config.can_handle = &hcan1, .controller_param_init_config = { .speed_PID = { - .Kp = 95, // 4.5 + .Kp = 80, // 4.5 .Ki = 0, // 0 .Kd = 0, // 0 .IntegralLimit = 8000, @@ -82,7 +83,7 @@ void ChassisInit() .MaxOut = 12000, }, .current_PID = { - .Kp = 0.4, // 0.4 + .Kp = 1, // 0.4 .Ki = 0, // 0 .Kd = 0, .IntegralLimit = 10500, @@ -99,14 +100,14 @@ void ChassisInit() .motor_type = LK9015, }; // @todo: 当前还没有设置电机的正反转,仍然需要手动添加reference的正负号,需要电机module的支持,待修改. - + chassis_motor_config.can_init_config.tx_id = 1; // chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; - motor_lb = LKMotorInit(&chassis_motor_config); - + motor_rb = LKMotorInit(&chassis_motor_config); + chassis_motor_config.can_init_config.tx_id = 2; // chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; - motor_rf = LKMotorInit(&chassis_motor_config); + motor_lf = LKMotorInit(&chassis_motor_config); // 舵电机6020初始化 Motor_Init_Config_s chassis_motor_steering_config = { @@ -144,40 +145,37 @@ void ChassisInit() .motor_type = GM6020, }; - 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 = 1; - motor_steering_lb = DJIMotorInit(&chassis_motor_steering_config); + motor_steering_rb = DJIMotorInit(&chassis_motor_steering_config); + + chassis_motor_steering_config.can_init_config.tx_id = 2; + motor_steering_lf = DJIMotorInit(&chassis_motor_steering_config); PID_Init_Config_s chassis_follow_pid_conf = { - .Kp = 1.2, // 6 - .Ki = 0, - .Kd = 0.1, // 0.495 - .DeadBand = 0.5f,// 启用死区，值为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 = 50, - //.Derivative_LPF_RC = 0.01, // 0.01 + .Kp = 12, // 6 + .Ki = 3.0f, + .Kd = 0.015f, // 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 }; PIDInit(&chassis_follow_pid, &chassis_follow_pid_conf); // 裁判系统初始化 - referee_data = UITaskInit(&huart1,&ui_data); // 裁判系统初始化,会同时初始化UI + referee_data = UITaskInit(&huart6,&ui_data); // 裁判系统初始化,会同时初始化UI SuperCap_Init_Config_s cap_conf = { .can_config = { - .can_handle = &hcan1, // 保持你原有的硬件CAN接口不变 - .tx_id = 0x001, // C板发给超级电容的ID - .rx_id = 0x100, // 超级电容发给C板的ID + .can_handle = &hcan2, + .tx_id = 0x302, // 超级电容默认接收id + .rx_id = 0x301, // 超级电容默认发送id,注意tx和rx在其他人看来是反的 }}; - cap = SuperCapInit(&cap_conf); // 超级电容初始化 todo 控制 - - // （新增）设置超级电容功率补偿，例如补偿5W，可根据实际需求调整 todo 控制 - SuperCapSetPowerOffset(cap, 5); + cap = SuperCapInit(&cap_conf); // 超级电容初始化 // 发布订阅初始化,如果为双板,则需要can comm来传递消息 #ifdef CHASSIS_BOARD @@ -225,104 +223,62 @@ void ChassisInit() */ static void MinmizeRotation(float *angle, const float *last_angle, float *speed) { - 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; + float rotation = *angle - *last_angle; + ANGLE_LIMIT_360_TO_180_ABS(rotation); + if (rotation > 90) { + *angle -= 180; *speed = -(*speed); - }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; + } else if (rotation < -90) { + *angle += 180; *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); + ANGLE_LIMIT_360_TO_180_ABS(*angle); } /** * @brief 舵轮电机角度解算 * */ - static void SteeringWheelCalculate() +static void SteeringWheelCalculate() { - float offset_lb, offset_rf; // 用于计算舵轮的角度 - float at_lb_last, at_rf_last; // 上次的角度 - at_lb_last = motor_steering_lb->measure.total_angle; - // at_lf_last = motor_steering_lf->measure.total_angle; - at_rf_last = motor_steering_rf->measure.total_angle; - // at_rb_last = motor_steering_rb->measure.total_angle; + float offset_lf, offset_rb; // 用于计算舵轮的角度 + float at_lf_last, at_rb_last; // 上次的角度 + at_lf_last = motor_steering_lf->measure.total_angle; + at_rb_last = motor_steering_rb->measure.total_angle; if (chassis_vx == 0 && chassis_vy == 0 && chassis_cmd_recv.wz == 0) { // at_lf = at_lf_last; // at_rf = at_rf_last; // at_lb = at_lb_last; // at_rb = at_rb_last; - vt_lb = 0, vt_rf = 0; + vt_lf = 0, vt_rb = 0; } else { // 生成预计算变量，减少计算量，空间换时间 float w = chassis_cmd_recv.wz * CHASSIS_WHEEL_OFFSET * SQRT2; 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;与上次相同因此注释 - arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_lb); // lb: y+ , x- - //temp_x = chassis_vx + w; - temp_y = chassis_vy + w;//与上次相同因此注释 - // arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_rb); // rb: y+ , x+ - // temp_y = chassis_vy - w; // temp_x = chassis_vy + w;与上次相同因此注释 - arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_rf); // rf: y- , x+ - 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;与上次相同因此注释 + // arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_lb); // lb: y+ , x- + // temp_x = chassis_vx + w; // temp_y = chassis_vx + w;与上次相同因此注释 + arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_rb); // rb: y+ , x+ + temp_y = chassis_vy - w; // temp_x = chassis_vy + w;与上次相同因此注释 + // arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_rf); // rf: y- , x+ // 计算角度偏移 - // offset_lf = atan2f(chassis_vy - w, chassis_vx - w) * RAD_2_DEGREE; // lf: y- , x- - offset_lb = atan2f(chassis_vy + w, chassis_vx - w) * RAD_2_DEGREE; // rf: y- , x+ - offset_rf = atan2f(chassis_vy - w, chassis_vx + w) * RAD_2_DEGREE; // lb: y+ , x-注意 - // offset_rb = atan2f(chassis_vy + w, chassis_vx + w) * RAD_2_DEGREE; // rb: y+ , x+ + offset_lf = atan2f(chassis_vy - w, chassis_vx - w) * RAD_2_DEGREE; // lf: y- , x- + offset_rb = atan2f(chassis_vy + w, chassis_vx + w) * RAD_2_DEGREE; // rb: y+ , x+ - // at_lf = STEERING_CHASSIS_ALIGN_ANGLE_LF + offset_lf; // 此处似乎不需要加上真实角度，因为我们需要的是绝对角度而不是相对角度 - at_lb = STEERING_CHASSIS_ALIGN_ANGLE_LB + offset_lb; - at_rf = STEERING_CHASSIS_ALIGN_ANGLE_RF + offset_rf; - // at_rb = STEERING_CHASSIS_ALIGN_ANGLE_RB + offset_rb; + at_lf = STEERING_CHASSIS_ALIGN_ANGLE_RF + offset_lf; // 此处似乎不需要加上真实角度，因为我们需要的是绝对角度而不是相对角度 + at_rb = STEERING_CHASSIS_ALIGN_ANGLE_LB + offset_rb; - // ANGLE_LIMIT_360_TO_180_ABS(at_lf); - ANGLE_LIMIT_360_TO_180_ABS(at_lb); - ANGLE_LIMIT_360_TO_180_ABS(at_rf); - // ANGLE_LIMIT_360_TO_180_ABS(at_rb); + ANGLE_LIMIT_360_TO_180_ABS(at_lf); + ANGLE_LIMIT_360_TO_180_ABS(at_rb); - // MinmizeRotation(&at_lf, &at_lf_last, &vt_lf); - MinmizeRotation(&at_lb, &at_lb_last, &vt_lb); - MinmizeRotation(&at_rf, &at_rf_last, &vt_rf); - // MinmizeRotation(&at_rb, &at_rb_last, &vt_rb); + MinmizeRotation(&at_lf, &at_lf_last, &vt_lf); + MinmizeRotation(&at_rb, &at_rb_last, &vt_rb); } - // DJIMotorSetRef(motor_steering_lf, at_lf); - DJIMotorSetRef(motor_steering_lb, at_lb); - DJIMotorSetRef(motor_steering_rf, at_rf); - // DJIMotorSetRef(motor_steering_rb, at_rb); + DJIMotorSetRef(motor_steering_lf, at_lf); + DJIMotorSetRef(motor_steering_rb, at_rb); } /** @@ -352,8 +308,8 @@ static void LimitChassisOutput() // LKMotorSetRef( motor_lf , vt_lf ); // LKMotorSetRef( motor_rb , vt_rb ); - LKMotorSetRef( motor_lb , vt_lb ); - LKMotorSetRef( motor_rf , vt_rf ); + LKMotorSetRef( motor_lf , vt_lf ); + LKMotorSetRef( motor_rb , vt_rb ); } /** @@ -369,8 +325,7 @@ static void EstimateSpeed() } /* 机器人底盘控制核心任务 */ -void ChassisTask() -{ +void ChassisTask() { // 后续增加没收到消息的处理(双板的情况) // 获取新的控制信息 #ifdef ONE_BOARD @@ -382,45 +337,34 @@ void ChassisTask() if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE) { // 如果出现重要模块离线或遥控器设置为急停,让电机停止 - LKMotorStop(motor_lb); - LKMotorStop(motor_rf); - DJIMotorStop(motor_steering_lb); - DJIMotorStop(motor_steering_rf); + LKMotorStop(motor_lf); + LKMotorStop(motor_rb); + DJIMotorStop(motor_steering_lf); + DJIMotorStop(motor_steering_rb); } else { // 正常工作 - LKMotorEnable(motor_lb); - LKMotorEnable(motor_rf); - DJIMotorEnable(motor_steering_lb); - DJIMotorEnable(motor_steering_rf); + LKMotorEnable(motor_lf); + LKMotorEnable(motor_rb); + DJIMotorEnable(motor_steering_lf); + DJIMotorEnable(motor_steering_rb); } // 根据控制模式设定旋转速度 switch (chassis_cmd_recv.chassis_mode) { - case CHASSIS_NO_FOLLOW: // 底盘不旋转,但维持全向机动,一般用于调整云台姿态 - chassis_cmd_recv.wz = 0; - break; - case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出 - // float single_round_offset = chassis_cmd_recv.offset_angle; - 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; - // chassis_cmd_recv.wz = PIDCalculate(&chassis_follow_pid, chassis_cmd_recv.offset_angle, 0)/100; - break; - case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略。 - chassis_cmd_recv.wz = 0.75; - break; - default: - break; + case CHASSIS_NO_FOLLOW: // 底盘不旋转,但维持全向机动,一般用于调整云台姿态 + 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); + break; + case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略 + chassis_cmd_recv.wz = 4000; + break; + default: + break; } // 根据云台和底盘的角度offset将控制量映射到底盘坐标系上 @@ -428,9 +372,8 @@ void ChassisTask() 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_vy = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta; - chassis_vx = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_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; // 根据控制模式进行正运动学解算,计算底盘输出 SteeringWheelCalculate(); @@ -442,9 +385,6 @@ void ChassisTask() // 根据电机的反馈速度和IMU(如果有)计算真实速度 EstimateSpeed(); - // 将robot_level传递给UI数据 - ui_data.robot_level = chassis_cmd_recv.robot_level; - // // 获取裁判系统数据 建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送 // // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red // chassis_feedback_data.enemy_color = referee_data->GameRobotState.robot_id > 7 ? 1 : 0;