From e66dbb8052615e8782ff1374b897868d027af4df Mon Sep 17 00:00:00 2001 From: Zorua <3540226929@qq.com> Date: Sat, 11 Apr 2026 16:22:34 +0800 Subject: [PATCH] =?UTF-8?q?=E5=8F=98=E9=87=8F=E5=90=8D=E6=9B=B4=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- application/chassis/chassis.c | 42 ++++++++++++++++++----------------- 1 file changed, 22 insertions(+), 20 deletions(-) diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index 94477b9..5b15561 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -102,11 +102,11 @@ void ChassisInit() chassis_motor_config.can_init_config.tx_id = 1; // chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; - motor_rf = LKMotorInit(&chassis_motor_config); + motor_lb = 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_lb = LKMotorInit(&chassis_motor_config); + motor_rf = LKMotorInit(&chassis_motor_config); // 舵电机6020初始化 Motor_Init_Config_s chassis_motor_steering_config = { @@ -271,8 +271,8 @@ static void MinmizeRotation(float *angle, const float *last_angle, float *speed) */ static void SteeringWheelCalculate() { - float offset_rf, offset_lb; // 用于计算舵轮的角度 - float at_rf_last, at_lb_last; // 上次的角度 + 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; @@ -290,37 +290,38 @@ static void MinmizeRotation(float *angle, const float *last_angle, float *speed) // 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_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; // 计算角度偏移 // offset_lf = atan2f(chassis_vy - w, chassis_vx - w) * RAD_2_DEGREE; // lf: y- , x- - offset_rf = atan2f(chassis_vy - w, chassis_vx - w) * RAD_2_DEGREE; // rf: y- , x+ - offset_lb = atan2f(chassis_vy + w, chassis_vx + w) * RAD_2_DEGREE; // lb: 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+ // at_lf = STEERING_CHASSIS_ALIGN_ANGLE_LF + offset_lf; // 此处似乎不需要加上真实角度，因为我们需要的是绝对角度而不是相对角度 - at_rf = STEERING_CHASSIS_ALIGN_ANGLE_RF + offset_rf; - at_lb = STEERING_CHASSIS_ALIGN_ANGLE_LB + offset_lb; + at_lb = STEERING_CHASSIS_ALIGN_ANGLE_RF + offset_lb; + at_rf = STEERING_CHASSIS_ALIGN_ANGLE_LB + offset_rf; // at_rb = STEERING_CHASSIS_ALIGN_ANGLE_RB + offset_rb; // ANGLE_LIMIT_360_TO_180_ABS(at_lf); - ANGLE_LIMIT_360_TO_180_ABS(at_rf); ANGLE_LIMIT_360_TO_180_ABS(at_lb); + ANGLE_LIMIT_360_TO_180_ABS(at_rf); // ANGLE_LIMIT_360_TO_180_ABS(at_rb); // MinmizeRotation(&at_lf, &at_lf_last, &vt_lf); - MinmizeRotation(&at_rf, &at_rf_last, &vt_rf); - MinmizeRotation(&at_lb, &at_lb_last, &vt_lb); + MinmizeRotation(&at_lb, &at_rf_last, &vt_lb); + MinmizeRotation(&at_rf, &at_lb_last, &vt_rf); // MinmizeRotation(&at_rb, &at_rb_last, &vt_rb); } // DJIMotorSetRef(motor_steering_lf, at_lf); - DJIMotorSetRef(motor_steering_rf, at_rf); DJIMotorSetRef(motor_steering_lb, at_lb); + DJIMotorSetRef(motor_steering_rf, at_rf); // DJIMotorSetRef(motor_steering_rb, at_rb); } @@ -351,8 +352,8 @@ static void LimitChassisOutput() // LKMotorSetRef( motor_lf , vt_lf ); // LKMotorSetRef( motor_rb , vt_rb ); - LKMotorSetRef( motor_rf , vt_rf ); - LKMotorSetRef( motor_lb , vt_lb ); + LKMotorSetRef( motor_lb , vt_rf ); + LKMotorSetRef( motor_rf , vt_lb ); } /** @@ -381,18 +382,18 @@ void ChassisTask() if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE) { // 如果出现重要模块离线或遥控器设置为急停,让电机停止 - LKMotorStop(motor_rf); LKMotorStop(motor_lb); - DJIMotorStop(motor_steering_rf); + LKMotorStop(motor_rf); DJIMotorStop(motor_steering_lb); + DJIMotorStop(motor_steering_rf); } else { // 正常工作 - LKMotorEnable(motor_rf); LKMotorEnable(motor_lb); - DJIMotorEnable(motor_steering_rf); + LKMotorEnable(motor_rf); DJIMotorEnable(motor_steering_lb); + DJIMotorEnable(motor_steering_rf); } // 根据控制模式设定旋转速度 @@ -428,7 +429,8 @@ void ChassisTask() 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_vy = 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; // 根据控制模式进行正运动学解算,计算底盘输出 SteeringWheelCalculate();