# 2021Infantry **Repository Path**: XMUT-PFA/infantry-robot ## Basic Information - **Project Name**: 2021Infantry - **Description**: old：初代步兵 remote：中期视频版本（遥控器控制） new_comp:中期视频后电脑操控版本 new_invantry：一号步兵位置环版本 invantry2：二号步兵位置环版本。以上两个版本大致一样 invantry1_v2:一号步兵二代，串级pid云台版本 invantry2_v2:二号步兵二代，串级pid（最新） invantry3:三号步兵,代码和2_v2都一样，就多了个狙击镜UI - **Primary Language**: C - **License**: Not specified - **Default Branch**: infantry2-v2 - **Homepage**: None - **GVP Project**: No ## Statistics - **Stars**: 1 - **Forks**: 1 - **Created**: 2021-03-25 - **Last Updated**: 2025-11-18 ## Categories & Tags **Categories**: Uncategorized **Tags**: None ## README # 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闭环射击） ## 其他 其他的诸如自瞄，返小陀螺这一类的，需要先把上面问题解决了再去考虑其他问题，因为陀螺仪数据的速度决定了步兵云台响应速度，没有一个快速的响应系统，做不了高级的自瞄打击这一类。