TuxMonkey/half-steering-basic-framework
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/half-steering-basic-framework synced 2026-10-24 01:52:20 +08:00
Code Issues Packages Projects Releases Wiki Activity

变量名更正

Browse Source
This commit is contained in:
Zorua
2026-04-11 16:22:34 +08:00
parent 0029810204
commit a4b8d0db84
1 changed files with 22 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

42
application/chassis/chassis.c
View File

@@ -102,11 +102,11 @@ void ChassisInit()
chassis_motor_config.can_init_config.tx_id = 1; chassis_motor_config.can_init_config.tx_id = 1;
// chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; // 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.can_init_config.tx_id = 2;
// chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; // 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初始化 // 舵电机6020初始化
Motor_Init_Config_s chassis_motor_steering_config = { 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() static void SteeringWheelCalculate()
{ {
float offset_rf, offset_lb; // 用于计算舵轮的角度 float offset_lb, offset_rf; // 用于计算舵轮的角度
float at_rf_last, at_lb_last; // 上次的角度 float at_lb_last, at_rf_last; // 上次的角度
at_lb_last = motor_steering_lb->measure.total_angle; at_lb_last = motor_steering_lb->measure.total_angle;
// at_lf_last = motor_steering_lf->measure.total_angle; // at_lf_last = motor_steering_lf->measure.total_angle;
at_rf_last = motor_steering_rf->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- // 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;与上次相同因此注释 // 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- 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;//与上次相同因此注释 temp_y = chassis_vy + w;//与上次相同因此注释
// arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_rb); // rb: y+ , x+ // 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;与上次相同因此注释 // 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+ 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_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; // rf: y- , x+
offset_lb = atan2f(chassis_vy + w, chassis_vx + w) * RAD_2_DEGREE; // lb: 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_rb = atan2f(chassis_vy + w, chassis_vx + w) * RAD_2_DEGREE; // rb: y+ , x+
// at_lf = STEERING_CHASSIS_ALIGN_ANGLE_LF + offset_lf; // 此处似乎不需要加上真实角度，因为我们需要的是绝对角度而不是相对角度 // at_lf = STEERING_CHASSIS_ALIGN_ANGLE_LF + offset_lf; // 此处似乎不需要加上真实角度，因为我们需要的是绝对角度而不是相对角度
at_rf = STEERING_CHASSIS_ALIGN_ANGLE_RF + offset_rf; at_lb = STEERING_CHASSIS_ALIGN_ANGLE_RF + offset_lb;
at_lb = STEERING_CHASSIS_ALIGN_ANGLE_LB + offset_lb; at_rf = STEERING_CHASSIS_ALIGN_ANGLE_LB + offset_rf;
// at_rb = STEERING_CHASSIS_ALIGN_ANGLE_RB + offset_rb; // at_rb = STEERING_CHASSIS_ALIGN_ANGLE_RB + offset_rb;
// ANGLE_LIMIT_360_TO_180_ABS(at_lf); // 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_lb);
ANGLE_LIMIT_360_TO_180_ABS(at_rf);
// ANGLE_LIMIT_360_TO_180_ABS(at_rb); // ANGLE_LIMIT_360_TO_180_ABS(at_rb);
// MinmizeRotation(&at_lf, &at_lf_last, &vt_lf); // MinmizeRotation(&at_lf, &at_lf_last, &vt_lf);
MinmizeRotation(&at_rf, &at_rf_last, &vt_rf); MinmizeRotation(&at_lb, &at_rf_last, &vt_lb);
MinmizeRotation(&at_lb, &at_lb_last, &vt_lb); MinmizeRotation(&at_rf, &at_lb_last, &vt_rf);
// MinmizeRotation(&at_rb, &at_rb_last, &vt_rb); // MinmizeRotation(&at_rb, &at_rb_last, &vt_rb);
} }
// DJIMotorSetRef(motor_steering_lf, at_lf); // DJIMotorSetRef(motor_steering_lf, at_lf);
DJIMotorSetRef(motor_steering_rf, at_rf);
DJIMotorSetRef(motor_steering_lb, at_lb); DJIMotorSetRef(motor_steering_lb, at_lb);
DJIMotorSetRef(motor_steering_rf, at_rf);
// DJIMotorSetRef(motor_steering_rb, at_rb); // DJIMotorSetRef(motor_steering_rb, at_rb);
} }
@@ -351,8 +352,8 @@ static void LimitChassisOutput()
// LKMotorSetRef( motor_lf , vt_lf ); // LKMotorSetRef( motor_lf , vt_lf );
// LKMotorSetRef( motor_rb , vt_rb ); // LKMotorSetRef( motor_rb , vt_rb );
LKMotorSetRef( motor_rf , vt_rf ); LKMotorSetRef( motor_lb , vt_rf );
LKMotorSetRef( motor_lb , vt_lb ); LKMotorSetRef( motor_rf , vt_lb );
} }
/** /**
@@ -381,18 +382,18 @@ void ChassisTask()
if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE) if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
{ // 如果出现重要模块离线或遥控器设置为急停,让电机停止 { // 如果出现重要模块离线或遥控器设置为急停,让电机停止
LKMotorStop(motor_rf);
LKMotorStop(motor_lb); LKMotorStop(motor_lb);
DJIMotorStop(motor_steering_rf); LKMotorStop(motor_rf);
DJIMotorStop(motor_steering_lb); DJIMotorStop(motor_steering_lb);
DJIMotorStop(motor_steering_rf);
} }
else else
{ {
// 正常工作 // 正常工作
LKMotorEnable(motor_rf);
LKMotorEnable(motor_lb); LKMotorEnable(motor_lb);
DJIMotorEnable(motor_steering_rf); LKMotorEnable(motor_rf);
DJIMotorEnable(motor_steering_lb); 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); 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); 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_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(); SteeringWheelCalculate();