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:
guoxy06
2026-04-12 16:27:44 +08:00
parent be8f3f38c8
commit 47aba369d8
1 changed files with 84 additions and 144 deletions
Download Patch File Download Diff File Expand all files Collapse all files

208
application/chassis/chassis.c
View File

@@ -1,3 +1,4 @@
/** /**
* @file chassis.c * @file chassis.c
* @author NeoZeng neozng1@hnu.edu.cn * @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 Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据传入此结构体的对应变量中，UI会自动检测是否变化，对应显示UI
static SuperCapInstance *cap; // 超级电容 static SuperCapInstance *cap; // 超级电容
static LKMotorInstance *motor_lb,*motor_rf; // 轮电机9015 static LKMotorInstance *motor_lf,*motor_rb; // 轮电机9015
static DJIMotorInstance *motor_steering_lb,*motor_steering_rf; // 舵电机6020 static DJIMotorInstance *motor_steering_lf,*motor_steering_rb; // 舵电机6020
/* 用于自旋变速策略的时间变量 */ /* 用于自旋变速策略的时间变量 */
// static float t; // static float t;
/* 私有函数计算的中介变量,设为静态避免参数传递的开销 */ /* 私有函数计算的中介变量,设为静态避免参数传递的开销 */
static float chassis_vx, chassis_vy; // 将云台系的速度投影到底盘 static float chassis_vx, chassis_vy; // 将云台系的速度投影到底盘
static float vt_lb, vt_rf; // 底盘速度解算后的临时输出,待进行限幅 static float vt_lf, vt_rb; // 底盘速度解算后的临时输出,待进行限幅
static float at_lb, at_rf; // 底盘的角度解算后的临时输出,待进行限幅 static float at_lf, at_rb; // 底盘的角度解算后的临时输出,待进行限幅
static PIDInstance chassis_follow_pid; // 底盘跟随云台的PID static PIDInstance chassis_follow_pid; // 底盘跟随云台的PID
void ChassisInit() void ChassisInit()
@@ -74,7 +75,7 @@ void ChassisInit()
.can_init_config.can_handle = &hcan1, .can_init_config.can_handle = &hcan1,
.controller_param_init_config = { .controller_param_init_config = {
.speed_PID = { .speed_PID = {
.Kp = 95, // 4.5 .Kp = 80, // 4.5
.Ki = 0, // 0 .Ki = 0, // 0
.Kd = 0, // 0 .Kd = 0, // 0
.IntegralLimit = 8000, .IntegralLimit = 8000,
@@ -82,7 +83,7 @@ void ChassisInit()
.MaxOut = 12000, .MaxOut = 12000,
}, },
.current_PID = { .current_PID = {
.Kp = 0.4, // 0.4 .Kp = 1, // 0.4
.Ki = 0, // 0 .Ki = 0, // 0
.Kd = 0, .Kd = 0,
.IntegralLimit = 10500, .IntegralLimit = 10500,
@@ -102,11 +103,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_lb = LKMotorInit(&chassis_motor_config); motor_rb = 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_rf = LKMotorInit(&chassis_motor_config); motor_lf = LKMotorInit(&chassis_motor_config);
// 舵电机6020初始化 // 舵电机6020初始化
Motor_Init_Config_s chassis_motor_steering_config = { Motor_Init_Config_s chassis_motor_steering_config = {
@@ -144,40 +145,37 @@ void ChassisInit()
.motor_type = GM6020, .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; 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 = { PID_Init_Config_s chassis_follow_pid_conf = {
.Kp = 1.2, // 6 .Kp = 12, // 6
.Ki = 0, .Ki = 3.0f,
.Kd = 0.1, // 0.495 .Kd = 0.015f, // 0.5
.DeadBand = 0.5f,// 启用死区，值为0.5度 .DeadBand = 0.5,
//.CoefA = 0.2, .CoefA = 0.2,
//.CoefB = 0.3, .CoefB = 0.3,
//.Improve = PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_DerivativeFilter | PID_Derivative_On_Measurement | PID_Integral_Limit | PID_Derivative_On_Measurement, .Improve = PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_DerivativeFilter | PID_Derivative_On_Measurement | PID_Integral_Limit | PID_Derivative_On_Measurement,
//.IntegralLimit = 50, // 200 .IntegralLimit = 50, // 200
.MaxOut = 50, .MaxOut = 1200,
//.Derivative_LPF_RC = 0.01, // 0.01 .Derivative_LPF_RC = 0.01, // 0.01
}; };
PIDInit(&chassis_follow_pid, &chassis_follow_pid_conf); 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 = { SuperCap_Init_Config_s cap_conf = {
.can_config = { .can_config = {
.can_handle = &hcan1, // 保持你原有的硬件CAN接口不变 .can_handle = &hcan2,
.tx_id = 0x001, // C板发给超级电容的ID .tx_id = 0x302, // 超级电容默认接收id
.rx_id = 0x100, // 超级电容发给C板的ID .rx_id = 0x301, // 超级电容默认发送id,注意tx和rx在其他人看来是反的
}}; }};
cap = SuperCapInit(&cap_conf); // 超级电容初始化 todo 控制 cap = SuperCapInit(&cap_conf); // 超级电容初始化
// （新增）设置超级电容功率补偿，例如补偿5W，可根据实际需求调整 todo 控制
SuperCapSetPowerOffset(cap, 5);
// 发布订阅初始化,如果为双板,则需要can comm来传递消息 // 发布订阅初始化,如果为双板,则需要can comm来传递消息
#ifdef CHASSIS_BOARD #ifdef CHASSIS_BOARD
@@ -225,104 +223,62 @@ void ChassisInit()
*/ */
static void MinmizeRotation(float *angle, const float *last_angle, float *speed) static void MinmizeRotation(float *angle, const float *last_angle, float *speed)
{ {
float target_angle = *angle; float rotation = *angle - *last_angle;
float actual_angle = *last_angle; ANGLE_LIMIT_360_TO_180_ABS(rotation);
float rotation = target_angle - actual_angle; if (rotation > 90) {
float norm_rotation = rotation; *angle -= 180;
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); *speed = -(*speed);
}else if(norm_rotation<-threshold){ } else if (rotation < -90) {
int32_t round_diff = (int32_t)((target_angle-actual_angle) / 360.0f); *angle += 180;
*angle = actual_angle + norm_rotation+180.0f+round_diff*360.0f;
*speed = -(*speed); *speed = -(*speed);
} }
// ANGLE_LIMIT_360_TO_180_ABS(*angle); 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);
} }
/** /**
* @brief 舵轮电机角度解算 * @brief 舵轮电机角度解算
* *
*/ */
static void SteeringWheelCalculate() static void SteeringWheelCalculate()
{ {
float offset_lb, offset_rf; // 用于计算舵轮的角度 float offset_lf, offset_rb; // 用于计算舵轮的角度
float at_lb_last, at_rf_last; // 上次的角度 float at_lf_last, at_rb_last; // 上次的角度
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_rb_last = motor_steering_rb->measure.total_angle;
at_rf_last = motor_steering_rf->measure.total_angle;
// at_rb_last = motor_steering_rb->measure.total_angle;
if (chassis_vx == 0 && chassis_vy == 0 && chassis_cmd_recv.wz == 0) { if (chassis_vx == 0 && chassis_vy == 0 && chassis_cmd_recv.wz == 0) {
// at_lf = at_lf_last; // at_lf = at_lf_last;
// at_rf = at_rf_last; // at_rf = at_rf_last;
// at_lb = at_lb_last; // at_lb = at_lb_last;
// at_rb = at_rb_last; // at_rb = at_rb_last;
vt_lb = 0, vt_rf = 0; vt_lf = 0, vt_rb = 0;
} else { } else {
// 生成预计算变量，减少计算量，空间换时间 // 生成预计算变量，减少计算量，空间换时间
float w = chassis_cmd_recv.wz * CHASSIS_WHEEL_OFFSET * SQRT2; float w = chassis_cmd_recv.wz * CHASSIS_WHEEL_OFFSET * SQRT2;
float temp_x = chassis_vx - w, temp_y = chassis_vy - w; 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- 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_vx + 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_lb = atan2f(chassis_vy + w, chassis_vx - w) * RAD_2_DEGREE; // rf: y- , x+ offset_rb = atan2f(chassis_vy + w, chassis_vx + w) * RAD_2_DEGREE; // rb: 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_lf = STEERING_CHASSIS_ALIGN_ANGLE_RF + offset_lf; // 此处似乎不需要加上真实角度，因为我们需要的是绝对角度而不是相对角度
at_lb = STEERING_CHASSIS_ALIGN_ANGLE_LB + offset_lb; at_rb = STEERING_CHASSIS_ALIGN_ANGLE_LB + offset_rb;
at_rf = STEERING_CHASSIS_ALIGN_ANGLE_RF + 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_lf);
ANGLE_LIMIT_360_TO_180_ABS(at_lb); ANGLE_LIMIT_360_TO_180_ABS(at_rb);
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_lf, &at_lf_last, &vt_lf);
MinmizeRotation(&at_lb, &at_lb_last, &vt_lb); MinmizeRotation(&at_rb, &at_rb_last, &vt_rb);
MinmizeRotation(&at_rf, &at_rf_last, &vt_rf);
// MinmizeRotation(&at_rb, &at_rb_last, &vt_rb);
} }
// DJIMotorSetRef(motor_steering_lf, at_lf); DJIMotorSetRef(motor_steering_lf, at_lf);
DJIMotorSetRef(motor_steering_lb, at_lb); DJIMotorSetRef(motor_steering_rb, at_rb);
DJIMotorSetRef(motor_steering_rf, at_rf);
// DJIMotorSetRef(motor_steering_rb, at_rb);
} }
/** /**
@@ -352,8 +308,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_lb , vt_lb ); LKMotorSetRef( motor_lf , vt_lf );
LKMotorSetRef( motor_rf , vt_rf ); LKMotorSetRef( motor_rb , vt_rb );
} }
/** /**
@@ -369,8 +325,7 @@ static void EstimateSpeed()
} }
/* 机器人底盘控制核心任务 */ /* 机器人底盘控制核心任务 */
void ChassisTask() void ChassisTask() {
{
// 后续增加没收到消息的处理(双板的情况) // 后续增加没收到消息的处理(双板的情况)
// 获取新的控制信息 // 获取新的控制信息
#ifdef ONE_BOARD #ifdef ONE_BOARD
@@ -382,18 +337,18 @@ void ChassisTask()
if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE) if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
{ // 如果出现重要模块离线或遥控器设置为急停,让电机停止 { // 如果出现重要模块离线或遥控器设置为急停,让电机停止
LKMotorStop(motor_lb); LKMotorStop(motor_lf);
LKMotorStop(motor_rf); LKMotorStop(motor_rb);
DJIMotorStop(motor_steering_lb); DJIMotorStop(motor_steering_lf);
DJIMotorStop(motor_steering_rf); DJIMotorStop(motor_steering_rb);
} }
else else
{ {
// 正常工作 // 正常工作
LKMotorEnable(motor_lb); LKMotorEnable(motor_lf);
LKMotorEnable(motor_rf); LKMotorEnable(motor_rb);
DJIMotorEnable(motor_steering_lb); DJIMotorEnable(motor_steering_lf);
DJIMotorEnable(motor_steering_rf); DJIMotorEnable(motor_steering_rb);
} }
// 根据控制模式设定旋转速度 // 根据控制模式设定旋转速度
@@ -403,21 +358,10 @@ void ChassisTask()
chassis_cmd_recv.wz = 0; chassis_cmd_recv.wz = 0;
break; break;
case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出 case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出
// float single_round_offset = chassis_cmd_recv.offset_angle; 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;
// chassis_cmd_recv.wz = PIDCalculate(&chassis_follow_pid, chassis_cmd_recv.offset_angle, 0)/100;
break; break;
case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略。 case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
chassis_cmd_recv.wz = 0.75; chassis_cmd_recv.wz = 4000;
break; break;
default: default:
break; break;
@@ -428,9 +372,8 @@ void ChassisTask()
static float sin_theta, cos_theta; static float sin_theta, cos_theta;
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_vy = 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_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_vy = chassis_cmd_recv.vx * sin_theta - chassis_cmd_recv.vy * cos_theta;
// 根据控制模式进行正运动学解算,计算底盘输出 // 根据控制模式进行正运动学解算,计算底盘输出
SteeringWheelCalculate(); SteeringWheelCalculate();
@@ -442,9 +385,6 @@ void ChassisTask()
// 根据电机的反馈速度和IMU(如果有)计算真实速度 // 根据电机的反馈速度和IMU(如果有)计算真实速度
EstimateSpeed(); EstimateSpeed();
// 将robot_level传递给UI数据
ui_data.robot_level = chassis_cmd_recv.robot_level;
// // 获取裁判系统数据 建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送 // // 获取裁判系统数据 建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送
// // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red // // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red
// chassis_feedback_data.enemy_color = referee_data->GameRobotState.robot_id > 7 ? 1 : 0; // chassis_feedback_data.enemy_color = referee_data->GameRobotState.robot_id > 7 ? 1 : 0;