Infantry robot2021: old：初代步兵 remote：中期视频版本（遥控器控制） new_comp:中期视频后电脑操控版本 new_invantry：一号步兵位置环版本 invantry2：二号步兵位置环版本。以上两个版本大致一样 invantry1_v2:一号步兵二代，串级pid云台版本 invantry2_v2:二号步兵二代，串级pid（最新） invantry3:三号步兵,代码和2_v2都一样，就多了个狙击镜UI

# 2021-Robomaster-步兵机器人
## 补充
2024.3.26：
PFA2024届已经采用了一套全新的框架，2021年的框架不具备参考意义，
希望PFA技术一直迭代下去，越来越好！

## 基本情况

由于2020年疫情导致软件代码断代（只传下来控制电机的方法），2021基本从0开始。

开发板：A板

底盘电机：3508

云台电机：6020

摩擦轮：大疆精灵4电机（开环控制，掉速严重，不好控制，建议后面用3508闭环控制射击）

陀螺仪：维特智能串口陀螺仪（传输速度有点慢但是精度高，建议以后用i2c陀螺仪，传输速率会快点，精度要求不高）

作者：张粤炫

## 软件框架与程序模块

### 多线程控制-freRTOS操作系统

#### 裁判系统数据交互线程

```cpp
  referee_unpack_fifo_data();//裁判系统返回参数线程
```

##### 说明：

这个线程主要用于读取裁判系统的数据，裁判系统会实时检测机器人状态，因此，本线程的数据往往用来做闭环的反馈，也可以发送串口数据给裁判系统，用来画UI界面。

#### 云台电机实时控制线程

```cpp
set_motor_voltage(&hcan1,0, motor_pid[4].output,motor_pid[5].output,motor_pid[6].output,0);//6020   205
```

##### 说明：

云台用的串级闭环，所以云台控制必须要控制的及时，所以我单独开了一个线程

#### 模式选择线程

```cpp
main_mode_switch();

```

##### 说明：

分为 1比赛模式 2遥控调试模式 3备用无陀螺仪模式（当时陀螺仪容易坏，备用一套程序）

### 闭环控制

```cpp
void RM_limit(void)
{

//if(switch_R==3) v0;
chasis_limit();//超级电容  底盘闭环，功率限制
fire_heat_limit();//发射热量限制
fire_limit();//射速限制

}
```

#### 云台闭环（中断2ms运行一次串级PID）

```cpp
	motor_pid[4].f_pid_reset(&motor_pid[4],in_XP,in_XI,in_XD,PID_Speed);//设置pid
        taget_Yaw=taget_Yaw-mouseX*mouseX_speed;//读取鼠标数据，得到目标角度

if     (taget_Yaw>max_angle)taget_Yaw=taget_Yaw-max_angle; //鼠标x轴移动  限值
	else if (taget_Yaw<0)taget_Yaw=max_angle+taget_Yaw; //鼠标x轴移动  限值
//以下是串级PID
	Xnum++;
	if(Xnum>n){Xnum=0;
        Xerr=-Xyaw_to_tageyaw(Yaw,taget_Yaw);}
		ch[4]=Xerr;
			motor_pid[4].target =ch[4];
      motor_pid[4].f_cal_pid(&motor_pid[4],moto_chassis[4].speed_rpm);    //6020+6020Y_moto_chassis[5].anglech[6]
```

Xnum：内外环权重比，一般情况下内：外=3：1，内环控制更为重要

Xyaw_to_tageyaw：外环位置控制环

motor_pid[4].f_cal_pid(&motor_pid[4],moto_chassis[4].speed_rpm) ：内环速度环控制

调参经验：

> [(169条消息) PID参数解析+调参经验笔记（经验法）_pid调参_Xuan-ZY的博客-CSDN博客](https://blog.csdn.net/yunddun/article/details/107720644)

自动控制理论，频域分析PID作用：

> [(169条消息) PID自控理论（频域bode图理论分析）_Xuan-ZY的博客-CSDN博客](https://blog.csdn.net/yunddun/article/details/129599811)

#### 功率闭环

```cpp
void chasis_limit(void)
{
chassi_power_bufer=	power_heat_data_t.chassis_power_buffer;//缓冲
now_chassi_power=power_heat_data_t.chassis_power;//当前底盘功率
chasi_limi=robot_state.chassis_power_limit;//底盘功率上限
//if     (   chasi_limi> 0&&chasi_limi<=45     ){v0;limit=v0_limit;} //2500  v    7000

if     (   chasi_limi> 0&&chasi_limi<=45     ){v1;SPEED_limit=2000;} //2500  v    7000
else if(   chasi_limi>45&&chasi_limi<=50     ){v2;SPEED_limit=2500;}//3000
else if(   chasi_limi>50&&chasi_limi<=55     ){v2;SPEED_limit=2500;} // 3100
else if(   chasi_limi>55&&chasi_limi<=60     ){v3;SPEED_limit=3000;}//3200
else if(   chasi_limi>60&&chasi_limi<=65     ){v4;}//3300
else if(   chasi_limi>65&&chasi_limi<=70     ){v5;}//3700
else if(   chasi_limi>70&&chasi_limi<=75     ){v6;}//
else if(   chasi_limi>75&&chasi_limi<=82     ){v7;SPEED_limit=4000;}//
else if(   chasi_limi>82&&chasi_limi<=87     ){v8;SPEED_limit=8000;}//
else if(   chasi_limi>87)                     {v9;SPEED_limit=8000;}//
chassi_powe_err=now_chassi_power-chasi_limi;//期望小于0
bufer_err  = chassi_power_bufer-60;//期望大于0
if(chassi_powe_err>0||bufer_err<0)//
{
limit_speed=-chassi_powe_err*1+bufer_err*2;
}
else   limit_speed=0;

}
```

##### 说明

chasi_limi：裁判系统规定的功率上限

SPEED_limit：限制的电机速度（其实功率闭环并没有做好，这里控制量不应该是电机速度，应该是控制电机的功率，也就是电流值×24v电压）

##### 功率闭环流程（p比例控制，需要的精度不高）

1. 控制量（输入量）：now_chassi_power（当前底盘功率）
2. 目标量：chasi_limi（极限值，不能高于这个值，否则扣血）
3. 输出量：电机的速度值（这个地方其实有问题，应该是输出功率，直接发给can控制，但是这样很不稳定，电机力气变得小，所以代码中我控制的电机速度，内环再加一个速度环）

##### 超级电容工作流程

* 裁判系统会给出一个上限功率chasi_limi
* 超级电容通过串口和A板互联，超级电容根据chasi_limi进行“能量缓存”操作

1. 刚开机的状态，电容里面消耗光的情况下，超级电容会先给电容充电，继电器为直通后级，也就是此时如果底盘超功率是直接会被裁判系统检测到并且扣血，所以一定要有反馈信号，就是起码要能区别此时是否是直通后级，如果是的话用ui画个标记，体现操作手此时要慢速。如果充满电，则关闭直通后级模式，打开电容模式、
2. 经过第一步，此时电容电压是够的，所以读取裁判系统此时的最大输入功率，并控制超级电容以这个功率继续给电容充电，，其中Taget_V就是裁判系统的闭环参数，如果检测到此时电容被消耗的差不多了，就跳入上面的if中，直通后级，关闭充电，准备进入第三个步骤。
3. 当时比赛来不及改，大致思路是用一个io口的0和1状态表示此时是否是直通后级的状态，然后flag标志位更新为1，又从第一步开始循环。

### 云台自稳功能

```cpp
		motor_pid[5].f_pid_reset(&motor_pid[5],in_YP,in_YI,in_YD,PID_Speed);
	taget_Roll=taget_Roll-mouseY*mouseY_speed;

if     (taget_Roll>max_angle)taget_Roll=taget_Roll-max_angle; //鼠标y轴移动  限值
	else if (taget_Roll<0)taget_Roll=max_angle+taget_Roll; //鼠标y轴移动  限值
	taget_Roll_limit();//限制二号步兵俯仰角电机不会超过物理限位

Ynum++;
	if(Ynum>nY){Ynum=0;	Yerr=-Yroll_to_tageroll(Roll,taget_Roll);}
	ch[5]=Yerr;

motor_pid[5].target = ch[5]; 																			
      motor_pid[5].f_cal_pid(&motor_pid[5],moto_chassis[5].speed_rpm);  
```

#### 俯仰角串级pd控制（自稳功能）

##### 外环位置环

```cpp
float Yroll_to_tageroll(float now,float target)
{
	if(target<0)target=max_angleY+target;
	 static int err=0;

float out_put;
	 err=target-now;                //计算偏差

if( err>(max_angleY/2)){  err= err-max_angleY;}//解决360度问题
	else if( err<-(max_angleY/2)){ err= err+max_angleY;}

out_put=Yp/100*err+Yd/100*(moto_chassis[5].speed_rpm);   //增量式PI控制器

if(out_put>max_outputY)out_put=max_outputY;
	else if(out_put<-max_outputY)out_put=-max_outputY;
	return out_put;                         //增量输出

}
```

* 输入量（反馈量）：陀螺仪返回的roll角度值（俯仰角）
* 目标量：鼠标控制的角度（用户想要到达的角度）
* 输出量：内环速度环的目标值（作为内环的目标量）

###### 外环PD控制器优化

```cpp
 out_put=Yp/100*err+Yd/100*(moto_chassis[5].speed_rpm);   //增量式PD控制器  
```

此处的微分部分进行了优化，非常关键，大幅度提高了系统的稳定性，可以通过计算公式去理解

原来是这样，可以比较一下

```cpp
 out_put=Yp/100*err+Yd/100*(err-last_err);   //增量式PD控制器   
```

ps:这个地方一定要弄懂

##### 内环速度环

```cpp
	ch[5]=Yerr;
		  motor_pid[5].target = ch[5]; 																			
      motor_pid[5].f_cal_pid(&motor_pid[5],moto_chassis[5].speed_rpm); 
```

* 输入量（反馈量）：电机can返回的速度值
* 目标量：外环的输出量
* 输出量：can发送给电机的电流值（单独开了个线程发送这个值）

#### 功能

机器人在不平整的路面行走时，能够进行俯仰角自稳，操作时候不会上下晃动

### 底盘全向运动算法

```cpp
void chassis_move(void)
{
	      chassis_buffer();
	      rotor_Angle=(moto_chassis[4].angle-Bchang)*360/8192;//开机时由于装配导致的补偿角度就可以通过减去Bchang值得到0，所以此时底盘朝前
		
				Vx=cos(rotor_Angle/180*My_Pi)*(key_D*SPEED-key_A*SPEED)+sin(rotor_Angle/180*My_Pi)*(key_W*SPEED-key_S*SPEED);
				Vy=cos(rotor_Angle/180*My_Pi)*(key_W*SPEED-key_S*SPEED)-sin(rotor_Angle/180*My_Pi)*(key_D*SPEED-key_A*SPEED); 
}
```

说明：大致的思路就是把速度分解到坐标系xy轴上，需要在草稿纸上面算明白

### 底盘模式选择

```cpp
	 void Dipan_Mode (void)    //////////////////////底盘归位
	 {
			if(go_mode==0){
			chassis_Follow(Follow_angle) ;
			}   //模式二：正常运动								E								
	      else 
					{
//						if(switch_R==3)Vz=0;
						if(Vz_mode%1==0){Vz=1000;					  }
						if(Vz_mode%2==0){Vz=2000;        }
						if(Vz_mode%3==0){Vz=3000;        }
				   }

}
```

说明：小陀螺模式，可选择三种模式

其实以后其他学校自瞄系统很强的时候，小陀螺基本没用

所以最好不要固定速度去旋转，最好是呈现某种波形去旋转（大概效果就是速度忽快忽慢，让他没法跟踪，但是前提是自己本身的云台稳定性过硬，不然会把自己晃晕了...）

```cpp
void  chassis_Follow(uint16_t taget_ang)//底盘跟随
{
		if(moto_chassis[4].angle<(taget_ang-DeadBand)||moto_chassis[4].angle>(taget_ang+DeadBand))		//100是允许的误差，否则容易左右震荡

Vz=dipan_pid(moto_chassis[4].angle,taget_ang);

else Vz=0;

}
```

说明：底盘跟随模式，底盘跟随云台

缺陷：底盘跟过来的时候，容易影响到云台的稳定性，所以这边最好设置过阻尼模式（就是不要有超调量）去跟随。

### 发射机构

发射机构21年的时候到最后比赛时候，都是开环控制，吃了太多的亏，所以这边没有可以借鉴的地方。

## 重要文件

### Gimble

说明：云台控制文件库

### stm32f4xx_it

说明：中断回调函数

### KM

说明：卡尔曼滤波函数库

### key

说明：操作按键控制文件

### chassis

说明：底盘控制文件

### Fire

说明：射击函数文件

### rc

说明：裁判系统信息交互文件

### focus

说明：自瞄闭环控制

### RMmaster

说明：裁判系统闭环

# 问题总结

## 陀螺仪

### 说明

陀螺仪对于步兵来说相当重要，是接下来一段时间需要攻克的重点，原因如下

1. 串口陀螺仪虽然精度高，但是响应速度非常慢（步兵陀螺仪不太需要精度，需要的是响应速度，确定了采用iic（spi）协议陀螺仪方案的正确性）
2. 步兵不管是自瞄，全向运动，俯仰角自稳，都存在响应慢的问题，这些都与陀螺仪有关。（当时比赛的时候就有感觉，咱们的步兵在响应速度上差点意思）

补充：最新版本代码采用哈工程ekf算法解算
## 功率闭环

### 说明

这个问题归根结底就是要找对被控量，到底是要控制功率还是串级控制速度。（有待实验）
补充：功率闭环于2024届解决

## 射击闭环

### 说明

21年用的是开环射击
补充：该问题已于22年解决（3508闭环射击）

## 其他

其他的诸如自瞄，返小陀螺这一类的，需要先把上面问题解决了再去考虑其他问题，因为陀螺仪数据的速度决定了步兵云台响应速度，没有一个快速的响应系统，做不了高级的自瞄打击这一类。

Robomaster_XMUT/ Infantry robot2021

简介

发行版

贡献者

近期动态

Robomaster_XMUT/ Infantry robot2021 .gitee-modal { width: 500px !important; }

简介

发行版

贡献者

近期动态

搜索帮助

Robomaster_XMUT/ Infantry robot2021