# Infantry_Steer_2025

**Repository Path**: cod_-control/infantry_-steer

## Basic Information

- **Project Name**: Infantry_Steer_2025
- **Description**: 25赛季舵轮步兵
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: Infantry_Chassis
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 3
- **Forks**: 0
- **Created**: 2024-11-29
- **Last Updated**: 2025-09-07

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# Infantry_Steer_2025

## 介绍
25赛季舵轮步兵底盘代码

## 环境依赖

- 开发工具：Keil V5.38a，VsCode
- 软件环境：Window11
- 硬件环境：达妙H7(STM32H723VGT6)
- 编译工具：Arm Compiler V6.22，C/C++编译

## 目录结构
HAL-Template
  ├───Algorithm  
  ├───API  
  ├───BSP  
  ├───Device  
  ├───Drivers  
  ├───MDK-ARM  
  ├───Middlewares  
  ├───SystemView  
  ├───Task  
  ├───MDK-ARM  
  ├───Middlewares  
  └───USB_DEVICE  

## 使用说明  
开发的任务主要放置在文件夹**Task**内
底盘部分主要分为2个任务
依次为：
1.Comm_Task
2.Chassis_Task  

#### Comm_Task
分为两个函数Comm与Mode_Com具体执行其功能
1. Comm通过CAN通信发送Chassis_Task中计算出所需发送给电机的值。
2. Mode_Com则是通过读取遥控器拨杆值来判断当前底盘应处于怎样的模式。

#### Chassis_Task

1. **Chassis_Init**负责初始化功率控制模型参数，RLS参数初始化初始化，PID初始化，初始舵向角的初始化。  
##### 舵向角初始化:   
为了使舵轮舵轮在静态、在意外碰撞下保持稳定状态，特使舵向在常态保持±45°，以增加其稳定性。  
##### 功控参数初始化:    
初始化最大误差Err_Max和最小误差Err_Low为了计算出功率分配因子K，  
##### RLS参数初始化:  
用RLS_Init函数初始化了X矩阵为4行1列，P矩阵为4行4列，Y矩阵为1行1列。初始化Lamda遗忘因子，以及P（n-1）矩阵的数值。 
给W（n-1）矩阵赋予初始值。略  
部分代码如下：  
```
void Chassis_Init()
{
    Chassis_Info.Tune.Adjust_Angle[FL] = -45;
    Chassis_Info.Tune.Adjust_Angle[FB] = 45;
    Chassis_Info.Tune.Adjust_Angle[RB] = -45;
    Chassis_Info.Tune.Adjust_Angle[RL] = 45;  
//舵向角初始化
    Chassis_Info.Power_Max = 50;
    Chassis_Info.theta = atan(1.0 / 1.0);
    Chassis_Info.k = 0.0007;

    PowerCtrl_Info->PowerBuff_Max = 60;
    PowerCtrl_Info[0].K3 = 4;
    PowerCtrl_Info[0].Err_Lower = 0.01;
    PowerCtrl_Info[0].Err_Upper = 500;
    PowerCtrl_Info[1].K3 = 4.5;
    PowerCtrl_Info[1].Err_Lower = 0.001;
    PowerCtrl_Info[1].Err_Upper = 5000;  
 //功率控制参数初始化
    RLS_Init(&RLS_Power_Info, 4,1,0.99999,1e5);
    RLS_Power_Info.Data.W[0] = 0.270;
    RLS_Power_Info.Data.W[1] = 0.00201;
    RLS_Power_Info.Data.W[2] =40.0026436;
    RLS_Power_Info.Data.W[1] = 0.000260;

}

```

2. **Chassis_On**判断当前底盘处于的模式来将对应的Vx，Vy，Vw赋值，再对底盘进行运动学解算。对下台阶与常态移动进行了区别处理。  
##### 舵轮运动学解算：  

```
    Chassis_Info.Target.Angle[FL] = atan2f((Chassis_Info.Target.Vy + Chassis_Info.Target.Vw * arm_cos_f32(Chassis_Info.theta) * R),
                                           (Chassis_Info.Target.Vx + Chassis_Info.Target.Vw * arm_sin_f32(Chassis_Info.theta) * R)) *
                                    RadiansToDegrees;

    Chassis_Info.Target.Angle[RL] = atan2f((Chassis_Info.Target.Vy + Chassis_Info.Target.Vw * arm_cos_f32(Chassis_Info.theta) * R),
                                           (Chassis_Info.Target.Vx - Chassis_Info.Target.Vw * arm_sin_f32(Chassis_Info.theta) * R)) *
                                    RadiansToDegrees;

    Chassis_Info.Target.Angle[FB] = atan2f((Chassis_Info.Target.Vy - Chassis_Info.Target.Vw * arm_cos_f32(Chassis_Info.theta) * R),
                                           (Chassis_Info.Target.Vx + Chassis_Info.Target.Vw * arm_sin_f32(Chassis_Info.theta) * R)) *
                                    RadiansToDegrees;

    Chassis_Info.Target.Angle[RB] = atan2f((Chassis_Info.Target.Vy - Chassis_Info.Target.Vw * arm_cos_f32(Chassis_Info.theta) * R),
                                           (Chassis_Info.Target.Vx - Chassis_Info.Target.Vw * arm_sin_f32(Chassis_Info.theta) * R)) *
                                    RadiansToDegrees;
    Chassis_Info.Target.Wheelrpm[FL] = -sqrtf(powf(Chassis_Info.Target.Vx + Chassis_Info.Target.Vw * arm_sin_f32(Chassis_Info.theta), 2.f) + powf(Chassis_Info.Target.Vy + Chassis_Info.Target.Vw * arm_cos_f32(Chassis_Info.theta), 2.f));
    Chassis_Info.Target.Wheelrpm[RL] = -sqrtf(powf(Chassis_Info.Target.Vx - Chassis_Info.Target.Vw * arm_sin_f32(Chassis_Info.theta), 2.f) + powf(Chassis_Info.Target.Vy + Chassis_Info.Target.Vw * arm_cos_f32(Chassis_Info.theta), 2.f));
    Chassis_Info.Target.Wheelrpm[FB] = -sqrtf(powf(Chassis_Info.Target.Vx - Chassis_Info.Target.Vw * arm_sin_f32(Chassis_Info.theta), 2.f) + powf(Chassis_Info.Target.Vy - Chassis_Info.Target.Vw * arm_cos_f32(Chassis_Info.theta), 2.f));
    Chassis_Info.Target.Wheelrpm[RB] = -sqrtf(powf(Chassis_Info.Target.Vx + Chassis_Info.Target.Vw * arm_sin_f32(Chassis_Info.theta), 2.f) + powf(Chassis_Info.Target.Vy - Chassis_Info.Target.Vw * arm_cos_f32(Chassis_Info.theta), 2.f));
```  
##### 下台阶与飞坡特殊处理  
下台阶以及飞坡时，舵轮底盘舵向平行且同向为最稳定状态。
```
    if (fabs(INS_Info.Roll_Gyro) > 50 || fabs(INS_Info.Pitch_Gyro) > 50)
    {
        Chassis_Info.Tune.Adjust_Angle[FL] = 0;
        Chassis_Info.Tune.Adjust_Angle[FB] = 0;
        Chassis_Info.Tune.Adjust_Angle[RB] = 0;
        Chassis_Info.Tune.Adjust_Angle[RL] = 0;
    }
    else
    {
        for (int i = 0; i < 4; i++)
        {
            float a = -1;
            a = powf(a, i + 1);
            Chassis_Info.Tune.Adjust_Angle[i] = a * 45.f;
        }
    }
```


3. **Chassis_Off**失能底盘，并重置校准参数。  

```
    for (int i = 0; i < 4; i++)
    {
        Chassis_Info.Tune.Grab[i] = 0;
        Chassis_Info.Output.Angle[i] = 0;
        Chassis_Info.Output.Wheelrpm[i] = 0;
    }
```
4. **Chassis_PidCal**舵向上电初校准，以及对舵向的优劣弧判断，从而选择最短路径进行PID控制输出。  
##### 上电初校准  
将接近开关的每次电平变化用Grab记录，校准时将行进轮锁死以防底盘因机械公差挪动，判断接近开关处在是否处在低电平状态，  
若是则进行单次校准，即正转过接近开关，再反转过。取接近开关电平高变低与底边高的角度值取平均，记为零点。
若不是则进行双重校准，即出接近开关后正转再反转，此时我们会得到两组零点，将其取平均作为最后的零点。  
```
 for (int i = 0; i < 4; i++)
            if (Chassis_Info.Tune.Last_State[i] == 0 && Chassis_Info.Tune.Trigger[i] == 1 || Chassis_Info.Tune.Last_State[i] == 1 && Chassis_Info.Tune.Trigger[i] == 0)
                Chassis_Info.Tune.Grab[i]++;
        //
        for (int i = 0; i < 4; i++)
        {
            Chassis_Info.Output.Wheelrpm[i] = f_PID_Calculate(&Pid_Travel[i], 0, Motor_Travel[i].Data.Velocity);
            float a = -1;
            a = powf(a, i);
            if (Chassis_Info.Tune.Trigger[i] == 0)
            {
                if (Chassis_Info.Tune.Grab[i] < 1)
                {
                    Pid_Course_V[i].Output = 4000.f * a;
                    Chassis_Info.Output.Angle[i] = Pid_Course_V[i].Output;
                }
                else
                {
                    f_PID_Calculate(&Pid_Course_A[i], Chassis_Info.Tune.zero[0][i] + Chassis_Info.Tune.Adjust_Angle[i], Motor_Course[i].Data.Angle);
                    Chassis_Info.Output.Angle[i] = f_PID_Calculate(&Pid_Course_V[i], Pid_Course_A[i].Output, Motor_Course[i].Data.Velocity);
                }
            }
            else if (Chassis_Info.Tune.Trigger[i] == 1 || Chassis_Info.Tune.Last_State[i] == 1)
            {
                if (Chassis_Info.Tune.Grab[i] < 1)
                {
                    Pid_Course_V[i].Output = -4000 * a;
                    Chassis_Info.Output.Angle[i] = Pid_Course_V[i].Output;
                    Chassis_Info.Tune.zero[1][i] = Chassis_Info.Tune.zero[0][i];
                }
                else if (Chassis_Info.Tune.Grab[i] < 3)
                {
                    Pid_Course_V[i].Output = 4000 * a;
                    Chassis_Info.Output.Angle[i] = Pid_Course_V[i].Output;
                    Chassis_Info.Tune.zero[2][i] = (Chassis_Info.Tune.zero[0][i] + Chassis_Info.Tune.zero[1][i]) / 2;
                    Chassis_Info.Tune.zero[0][i] = Chassis_Info.Tune.zero[2][i];
                }
                else
                {
                    f_PID_Calculate(&Pid_Course_A[i], Chassis_Info.Tune.zero[2][i] + Chassis_Info.Tune.Adjust_Angle[i], Motor_Course[i].Data.Angle);
                    Chassis_Info.Output.Angle[i] = f_PID_Calculate(&Pid_Course_V[i], Pid_Course_A[i].Output, Motor_Course[i].Data.Velocity);
                }
            }
        }
```  
##### 优劣弧判断  
计算此次舵向期望位置与当前电机位置差值，若大于绝对值90则舵向期望值±180，行进轮反转。

```
            for (int i = 0; i < 4; i++)
            {

                if (fabsf(Chassis_Info.Tune.zero[0][i] - Chassis_Info.Target.Angle[i] - Motor_Course[i].Data.Angle) < 90.f)
                {
                    f_PID_Calculate(&Pid_Course_A[i], Chassis_Info.Tune.zero[0][i] - Chassis_Info.Target.Angle[i], Motor_Course[i].Data.Angle);
                    Chassis_Info.Output.Angle[i] = f_PID_Calculate(&Pid_Course_V[i], Pid_Course_A[i].Output, Motor_Course[i].Data.Velocity);
                    Chassis_Info.Output.Wheelrpm[i] = f_PID_Calculate(&Pid_Travel[i], Chassis_Info.Target.Wheelrpm[i], Motor_Travel[i].Data.Velocity);
                }
                else if (fabsf(Chassis_Info.Tune.zero[0][i] - Chassis_Info.Target.Angle[i] - Motor_Course[i].Data.Angle) >= 90.f)
                {
                    if (Chassis_Info.Target.Angle[i] >= 0)
                    {
                        f_PID_Calculate(&Pid_Course_A[i], Chassis_Info.Tune.zero[0][i] - Chassis_Info.Target.Angle[i] + 180, Motor_Course[i].Data.Angle);
                        Chassis_Info.Output.Angle[i] = f_PID_Calculate(&Pid_Course_V[i], Pid_Course_A[i].Output, Motor_Course[i].Data.Velocity);
                        Chassis_Info.Output.Wheelrpm[i] = f_PID_Calculate(&Pid_Travel[i], -Chassis_Info.Target.Wheelrpm[i], Motor_Travel[i].Data.Velocity);
                    }
                    if (Chassis_Info.Target.Angle[i] < 0)
                    {
                        f_PID_Calculate(&Pid_Course_A[i], Chassis_Info.Tune.zero[0][i] - Chassis_Info.Target.Angle[i] - 180, Motor_Course[i].Data.Angle);
                        Chassis_Info.Output.Angle[i] = f_PID_Calculate(&Pid_Course_V[i], Pid_Course_A[i].Output, Motor_Course[i].Data.Velocity);
                        Chassis_Info.Output.Wheelrpm[i] = f_PID_Calculate(&Pid_Travel[i], -Chassis_Info.Target.Wheelrpm[i], Motor_Travel[i].Data.Velocity);
                    }
                }
            }
        }
```


5. **Power_Ctrl**负责舵轮底盘功率控制  
##### 电机功率模型:  
参考了西交的功率控制模型，具体可参考[【RM2023-电机功率模型与功率控制开源】西交利物浦大学](http://github.com/MaxwellDemonLin/Motor-modeling-and-power-control)  
首先我们读取电机的电流值Current与转速Velocity（rpm），将电流映射为电机的转矩，将转速转换单位为（m/s）。  
通过西交的模型得出具体的预测底盘功率  
部分代码如下：  
```
    for (int i = 0; i < 4; i++)
    {
        PowerCtrl_Info[0].Target.Omiga[i] = (Motor_Course[i].Data.Velocity / 9.55f);
        PowerCtrl_Info[0].Target.Torque[i] = ((Motor_Course[i].Data.Current) * 1.907e-5);
        PowerCtrl_Info[0].Target.Omiga_2[i] = powf(PowerCtrl_Info[0].Target.Omiga[i], 2.f);
        PowerCtrl_Info[0].Target.Torque_2[i] = powf(PowerCtrl_Info[0].Target.Torque[i], 2.f);

        PowerCtrl_Info[0].Target.Power_In[i] = (PowerCtrl_Info[0].K1 * PowerCtrl_Info[0].Target.Torque_2[i] + PowerCtrl_Info[0].K2 * PowerCtrl_Info[0].Target.Omiga_2[i] + PowerCtrl_Info[0].Target.Omiga[i] * PowerCtrl_Info[0].Target.Torque[i]);
    }

```
##### RLS功率模型拟合:  
通过将初始化的常数Torque_2、Omiga_2、Power_In求和,得Torque2_Sum、Omiga2_Sum、Power_Allin...进行功率拟合，  
通过RLS_Update对计算出的功率与实际功率进行递归最小二乘拟合，将模型中的参数K1，K2...进行更新。  
舵向与行进的功率分配：通过Chassis_Info.Power_Max设定底盘最大输出功率，对舵向优先分配80%的功率，使其优先提供转向，将剩余的功率分配给行进向。  
部分代码如下：  

```  
    PowerCtrl_Info[0].Sum.Torque2_Sum = PowerCtrl_Info[0].Target.Torque_2[0] + PowerCtrl_Info[0].Target.Torque_2[1] + PowerCtrl_Info[0].Target.Torque_2[2] + PowerCtrl_Info[0].Target.Torque_2[3];
    PowerCtrl_Info[0].Sum.Omiga2_Sum = PowerCtrl_Info[0].Target.Omiga_2[0] + PowerCtrl_Info[0].Target.Omiga_2[1] + PowerCtrl_Info[0].Target.Omiga_2[2] + PowerCtrl_Info[0].Target.Omiga_2[3];
    PowerCtrl_Info[0].Sum.Power_Sum = PowerCtrl_Info[0].Target.Power_In[0] + PowerCtrl_Info[0].Target.Power_In[1] + PowerCtrl_Info[0].Target.Power_In[2] + PowerCtrl_Info[0].Target.Power_In[3] + PowerCtrl_Info[0].K3;
    PowerCtrl_Info[0].Power_Allin = PowerCtrl_Info[0].Sum.Power_Sum;
    PowerCtrl_Info[0].Power_Max = 0.8 * Chassis_Info.Power_Max;
    VAL_LIMIT(PowerCtrl_Info[0].Power_Allin, -PowerCtrl_Info[0].Power_Max, PowerCtrl_Info[0].Power_Max);
    RLS_Power_Info.Data.U[0] = PowerCtrl_Info[0].Power_Allin + PowerCtrl_Info[1].Power_Allin;
    RLS_Power_Info.Data.Y[0] = Referee_System_Info.Power_Heat_Data.Chassis_Power;
    RLS_Power_Info.Data.X[0] = PowerCtrl_Info[0].Sum.Torque2_Sum;
    RLS_Power_Info.Data.X[1] = PowerCtrl_Info[0].Sum.Omiga2_Sum;
    RLS_Power_Info.Data.X[2] = PowerCtrl_Info[1].Sum.Torque2_Sum;
    RLS_Power_Info.Data.X[3] = PowerCtrl_Info[1].Sum.Omiga2_Sum;
    RLS_Update(&RLS_Power_Info);  
    PowerCtrl_Info[0].K1 = RLS_Power_Info.Data.W[0];
    PowerCtrl_Info[0].K2 = RLS_Power_Info.Data.W[1];
    PowerCtrl_Info[1].K1 = RLS_Power_Info.Data.W[2];
    PowerCtrl_Info[1].K2 = RLS_Power_Info.Data.W[3];
```

##### 功率分配：  
读取PID误差，将误差累加，根据单电机PID误差的占比和预测功率的占总输出功率的占比运用分配因子，通过平滑函数将其化为一个隶属度。
部分代码如下：  
```
for (int i = 0; i < 4; i++)
        PowerCtrl_Info[0].Err[i] = Pid_Course_V[i].Err[0];
    // 输入PID误差
    PowerCtrl_Info[0].Sum.Err_Sum = fabsf(PowerCtrl_Info[0].Err[FL]) + fabsf(PowerCtrl_Info[0].Err[FB]) + fabsf(PowerCtrl_Info[0].Err[RB]) + fabsf(PowerCtrl_Info[0].Err[RL]);
    // 计算PID误差和
    if (PowerCtrl_Info[0].Sum.Err_Sum > PowerCtrl_Info[0].Err_Upper)
        PowerCtrl_Info[0].K = 1;
    else if (PowerCtrl_Info[0].Sum.Err_Sum < PowerCtrl_Info[0].Err_Lower)
        PowerCtrl_Info[0].K = 0;
    else
        PowerCtrl_Info[0].K = (PowerCtrl_Info[0].Sum.Err_Sum - PowerCtrl_Info[0].Err_Lower) / (PowerCtrl_Info[0].Err_Upper - PowerCtrl_Info[0].Err_Lower);
    for (int i = 0; i < 4; i++)
    {
        PowerCtrl_Info[0].Menbership[i] = (PowerCtrl_Info[0].K * (fabs(PowerCtrl_Info[0].Err[i]) / PowerCtrl_Info[0].Sum.Err_Sum) + (1 - PowerCtrl_Info[0].K) * (fabs(PowerCtrl_Info[0].Target.Power_In[i]) / PowerCtrl_Info[0].Sum.Power_Sum));
        VAL_LIMIT(PowerCtrl_Info[0].Menbership[i], 0, 1);
        PowerCtrl_Info[0].Power_Limit[i] = PowerCtrl_Info[0].Menbership[i] * PowerCtrl_Info[0].Power_Allin; // 港科大P分配
    }
```
##### 求解受限力矩电流：  
如果拟合出的功率Power_Sum大于被分配的Power_Max，
则将根据上述分配的Power_Limit进行功率限制的求解，运用求根公式，求出限制后的力矩电流值。  
如若所拟合的功率Power_Sum并未超过我们所给定的Power_Max,则直接沿用PID计算所得输出即可。  
部分代码如下：  
```
  if (PowerCtrl_Info[0].Sum.Power_Sum >= PowerCtrl_Info[0].Power_Max)
    {

        for (int i = 0; i < 4; i++)
        {
            PowerCtrl_Info[0].A = PowerCtrl_Info[0].K1;
            PowerCtrl_Info[0].B = PowerCtrl_Info[0].Target.Omiga[i];
            PowerCtrl_Info[0].C = PowerCtrl_Info[0].Target.Omiga_2[i] * PowerCtrl_Info[0].K2 + PowerCtrl_Info[0].K3 - PowerCtrl_Info[0].Power_Limit[i];
            PowerCtrl_Info[0].Delta = powf(PowerCtrl_Info[0].B, 2.f) - 4 * PowerCtrl_Info[0].A * PowerCtrl_Info[0].C;
            if (isnan(PowerCtrl_Info[0].Delta) == 1 || isinf(PowerCtrl_Info[0].Delta) == 1)
                PowerCtrl_Info[0].Delta = 0;
            if (PowerCtrl_Info[0].Delta >= 0)
            {
                PowerCtrl_Info[0].Sqrt = sqrtf(PowerCtrl_Info[0].Delta);
                if (Chassis_Info.Output.Angle[i] >= 0)
                {
                    PowerCtrl_Info[0].Torque[i] = (-PowerCtrl_Info[0].B + PowerCtrl_Info[0].Sqrt) / (2 * PowerCtrl_Info[0].A);
                    Chassis_Info.Output.Angle[i] = (PowerCtrl_Info[0].Torque[i] / 1.907e-5);
                }
                else
                {
                    PowerCtrl_Info[0].Torque[i] = (-PowerCtrl_Info[0].B - PowerCtrl_Info[0].Sqrt) / (2 * PowerCtrl_Info[0].A);
                    Chassis_Info.Output.Angle[i] = (PowerCtrl_Info[0].Torque[i] / 1.907e-5);
                }
            }
            else
            {
                if (Chassis_Info.Output.Angle[i] >= 0)
                {
                    PowerCtrl_Info[0].Torque[i] = (-PowerCtrl_Info[0].B) / (2 * PowerCtrl_Info[0].A);
                    Chassis_Info.Output.Angle[i] = (PowerCtrl_Info[0].Torque[i] / 1.907e-5);
                }
                else
                {
                    PowerCtrl_Info[0].Torque[i] = (-PowerCtrl_Info[0].B) / (2 * PowerCtrl_Info[0].A);
                    Chassis_Info.Output.Angle[i] = (PowerCtrl_Info[0].Torque[i] / 1.907e-5);
                }
            }
        }
    }
    else
        for (int i = 0; i < 4; i++)
            Chassis_Info.Output.Angle[i] = Pid_Course_V[i].Output;  
```


## 参考开源  
1. [【RM2024-功率控制算法开源（含舵轮，轮腿，全自动调参）】香港科技大学ENTERPRIZE战队](http://github.com/hkustenterprize/RM2024-PowerModule?tab=readme-ov-file)
2. [【RM2023-电机功率模型与功率控制开源】西交利物浦大学](http://github.com/MaxwellDemonLin/Motor-modeling-and-power-control)  
## 联系方式  
QQ:647915770