目录
前言
MPU6050%E9%9B%B6%E6%BC%82%E5%A4%84%E7%90%86-toc" style="margin-left:0px;">一、MPU6050零漂处理
MPU6050%E7%9A%84Yaw%EF%BC%88%C2%B1180%EF%BC%89%E8%AF%AF%E5%B7%AE%E5%A4%84%E7%90%86-toc" style="margin-left:0px;">二、MPU6050的Yaw（±180）误差处理
PID%E7%AE%97%E6%B3%95%EF%BC%88%E5%A2%9E%E9%87%8F%E5%BC%8F%2B%E4%BD%8D%E7%BD%AE%E5%BC%8F%EF%BC%89-toc" style="margin-left:0px;">三、PID算法（增量式+位置式）
四、灰度传感器（以8路为例）
1、获取黑线偏差
2、判断ABCD点（有无黑线交点）
五、总结
前言
小编分享了2024电赛H题参考方案（+视频演示＋核心控制代码）——自动行驶小车文章后，于是根据其中问题写了一些可能用到的参考代码，希望能帮助大家，有问题欢迎大家指出。
MPU6050%E9%9B%B6%E6%BC%82%E5%A4%84%E7%90%86">一、MPU6050零漂处理
在进行角度、转向控制中，大多采用MPU6050来进行姿态检测，但由于MPU6050的特性，上电运行时三个角度（Pitch、roll、yaw）存在一定的零漂，尤其时yaw角，有时上电就漂了20多度。
为此提出了一种解决方法就是：在进行MPU6050初始化之后，马上进行初始值确定，通过多次测量求平均值的算法求得新的yaw，后续就以这个值为yaw的初始值，然后再进行运动控制。此方法不一定有效，推荐尝试。
参考代码如下：
int i = 0;
float pitch,roll,yaw;
float start_yaw[50];
float base_yaw = 0;
float process_data(float data[],uint8_t NUM_SAMPLES);
while ( mpu_dmp_init())
{delay_ms(20);
}//等待初始化完成
while(1)
{mpu_dmp_get_data(&pitch, &roll, &yaw);start_yaw[i++] = yaw;if(i == 50) //采样50次{base_yaw = process_data(start_yaw,50);break;}delay_ms(5);
}
float process_data(float data[],uint8_t NUM_SAMPLES) //均值滤波算法
{// 找到最大值和最小值的索引float max_index = 0;float min_index = 0;for (int i = 1; i < NUM_SAMPLES; i++) {if (data[i] > data[max_index]) {max_index = i;}if (data[i] < data[min_index]) {min_index = i;}}// 去除最大值和最小值float sum = 0;for (int i = 0; i < NUM_SAMPLES; i++) {if (i != max_index && i != min_index) {sum += data[i];}}// 计算剩余数据的平均值（除去最大值和最小值后剩余NUM_SAMPLES-2个）float average = sum / (NUM_SAMPLES - 2);return average;
}MPU6050%E7%9A%84Yaw%EF%BC%88%C2%B1180%EF%BC%89%E8%AF%AF%E5%B7%AE%E5%A4%84%E7%90%86">二、MPU6050的Yaw（±180）误差处理
用过MPU6050的朋友都知道，经过DMP处理得到的三个角度的范围（如下代码），他们并不是连续的，在控制转向环等时，需要进行特殊处理后才可得到准确的角度误差。
//得到dmp处理后的数据(注意,本函数需要比较多堆栈,局部变量有点多)
//pitch:俯仰角 精度:0.1° 范围:-90.0° <---> +90.0°
//roll:横滚角 精度:0.1° 范围:-180.0°<---> +180.0°
//yaw:航向角 精度:0.1° 范围:-180.0°<---> +180.0°
//返回值:0,正常
// 其他,失败
u8 mpu_dmp_get_data(float *pitch,float *roll,float *yaw);处理方案一：根据特定的角度进行加减360°，但是需要判断什么时候该减，什么时候该加，有点麻烦。比如，小车当前角度为150°，如果要转到180°，这个时候肯定需要有大于180的值，但他会变成负值，所以这是需要将得到的负角度加上360°。
#define limit_180_Z(n) ((n>0)?(n-360):n)
#define limit_180_F(n) ((n<0)?(n+360):n)处理方案二：通过算法处理，无论陀螺仪测得的数据为多少，直接将其转化为与目标角度的偏差，直接得到误差，加入闭环控制算法即可。
#define My_abs(n) (n>0?n:(-n))//输入角度目标值，和当前读出yaw角度即可，自动算出最小偏差。
float Yaw_error(float Target, float Now){float error;if (Target >= 0) {if (Now <= 0) {if (My_abs(Now) < (180 - Target)) {error = My_abs(Now) + Target;} else {error = -(180 - Target) - (180 - My_abs(Now));}} else {if (Now > 0) {error = Target - Now;}}} else if (Target < 0){if (Now > 0) {if (Now > Target + 180){error = (180 - Now) + (180 - My_abs(Target));}else if (Now < Target + 180) {error = -(My_abs(Target) + Now);}} else if (Now < 0) {error = -(My_abs(Target) - My_abs(Now));}} return error;
}PID%E7%AE%97%E6%B3%95%EF%BC%88%E5%A2%9E%E9%87%8F%E5%BC%8F%2B%E4%BD%8D%E7%BD%AE%E5%BC%8F%EF%BC%89">三、PID算法（增量式+位置式）
但凡涉及到控制，一般选用PID控制算法来实现，其通过模拟PID离散化处理而来。在进行小车的速度环、转向环、巡线等控制中普遍使用。
下面就以C语言来实现其控制代码：
/*** @brief 增量式 PID and 位置式 PID* */
typedef enum
{POSITION_PID = 0,DELTA_PID = 1,
}PID_mode;
#define XIAN_FU 7000 //积分限幅
#define LIMIT(x,min,max) (x)=(((x)<=(min))?(min):(((x)>=(max))?(max):(x))) //限幅定义int pid_control(PID_mode mode,float get_speed, float set_Target,float P,float I,float D)
{static int Integral,Last_error,LLast_Error;int Error,pid_out; Error = set_Target - get_speed; if (mode == POSITION_PID) // position PID{Integral += Error; LIMIT(Integral,-XIAN_FU,XIAN_FU);//积分限幅pid_out = P*Error + I*Integral + D*(Error - Last_error); Last_error = Error; }else if (mode == DELTA_PID) // delta PID{pid_out += P* (Error - Last_error) + I * Error + D * (Error - 2*Last_error + LLast_Error); Last_error = Error;LLast_Error = Last_error; }return pid_out;
}1、积分分离；
2、变速积分；
3、微分先行；
4、。。。。。。
四、灰度传感器（以8路为例）
1、获取黑线偏差
在进行循迹或巡线的控制过程中，若我们需要采用闭环控制（PID控制），则需要获得黑线和小车中心线的偏差，然后以减小此偏差为目的去控制小车，从而实现稳定的循迹或巡线功能。而如果采取摄像头如（CCD、OpenMV等）获取次偏差的话，其获取的偏差数值相对灰度传感器而言，要连续得多，相同控制参数下，用摄像头要平稳得多，但是只要参数调得好，算法使用得精，用灰度传感器效果也是很不错得。
用灰度传感器获取偏差，就需要我们自行处理数据，相当于对不同得情况进行人为赋值，以此来规定误差，类似于模糊控制。这里以8路灰度传感器为例，以其中间的4、5路为中心，即当4、5路同时识别到，而其他路没有识别到时，就认为其偏差为零。设向右偏为正的偏差，则向左偏为负偏差。当然，需要考虑黑线宽度，传感器各路间隔等因素，情况分得越细越好。
以下代码只做参考（写得有点垃）：
//以STM32为例,默认扫到黑线为高电平
#define Get_Value1 GPIO_ReadInputDataBit(GPIOF,GPIO_Pin_7)
#define Get_Value2 GPIO_ReadInputDataBit(GPIOF,GPIO_Pin_6)
#define Get_Value3 GPIO_ReadInputDataBit(GPIOF,GPIO_Pin_5)
#define Get_Value4 GPIO_ReadInputDataBit(GPIOF,GPIO_Pin_4)
#define Get_Value5 GPIO_ReadInputDataBit(GPIOF,GPIO_Pin_3)
#define Get_Value6 GPIO_ReadInputDataBit(GPIOF,GPIO_Pin_2)
#define Get_Value7 GPIO_ReadInputDataBit(GPIOF,GPIO_Pin_1)
#define Get_Value8 GPIO_ReadInputDataBit(GPIOF,GPIO_Pin_0)
int Get_Err(void)
{int err = 0;//中间if(!Get_Value1 && !Get_Value2 && !Get_Value3 && Get_Value4 && Get_Value5 && !Get_Value6 && !Get_Value7 && !Get_Value8) err = 0;//右边else if(!Get_Value1 && !Get_Value2 && !Get_Value3 && !Get_Value4 && Get_Value5 && !Get_Value6 && !Get_Value7 && !Get_Value8) err = 1;else if(!Get_Value1 && !Get_Value2 && !Get_Value3 && !Get_Value4 && Get_Value5 && Get_Value6 && !Get_Value7 && !Get_Value8) err = 2;else if(!Get_Value1 && !Get_Value2 && !Get_Value3 && !Get_Value4 && !Get_Value5 && Get_Value6 && !Get_Value7 && !Get_Value8) err = 3;else if(!Get_Value1 && !Get_Value2 && !Get_Value3 && !Get_Value4 && !Get_Value5 && Get_Value6 && Get_Value7 && !Get_Value8) err = 4;else if(!Get_Value1 && !Get_Value2 && !Get_Value3 && !Get_Value4 && !Get_Value5 && !Get_Value6 && Get_Value7 && !Get_Value8) err = 5;else if(!Get_Value1 && !Get_Value2 && !Get_Value3 && !Get_Value4 && !Get_Value5 && !Get_Value6 && Get_Value7 && Get_Value8) err = 6;else if(!Get_Value1 && !Get_Value2 && !Get_Value3 && !Get_Value4 && !Get_Value5 && !Get_Value6 && !Get_Value7 && Get_Value8) err = 7;//左边else if(!Get_Value1 && !Get_Value2 && !Get_Value3 && Get_Value4 && !Get_Value5 && !Get_Value6 && !Get_Value7 && !Get_Value8) err = -1;else if(!Get_Value1 && !Get_Value2 && Get_Value3 && Get_Value4 && !Get_Value5 && !Get_Value6 && !Get_Value7 && !Get_Value8) err = -2;else if(!Get_Value1 && !Get_Value2 && Get_Value3 && !Get_Value4 && !Get_Value5 && !Get_Value6 && !Get_Value7 && !Get_Value8) err = -3;else if(!Get_Value1 && Get_Value2 && Get_Value3 && !Get_Value4 && !Get_Value5 && !Get_Value6 && !Get_Value7 && !Get_Value8) err = -4;else if(!Get_Value1 && Get_Value2 && !Get_Value3 && !Get_Value4 && !Get_Value5 && !Get_Value6 && !Get_Value7 && !Get_Value8) err = -5;else if(Get_Value1 && Get_Value2 && !Get_Value3 && !Get_Value4 && !Get_Value5 && !Get_Value6 && !Get_Value7 && !Get_Value8) err = -6;else if(Get_Value1 && !Get_Value2 && !Get_Value3 && !Get_Value4 && !Get_Value5 && !Get_Value6 && !Get_Value7 && !Get_Value8) err = -7; return err;
}2、判断ABCD点（有无黑线处）
本次黑线宽度为1.5cm，其宽度较大，所以可以只用灰度传感器便可以判断是否到达该点。
只要在一定时间内（不能只判断一次，与按键消抖差不多一个理），一次都没有检测到黑线，便可认为丢失黑线了，到了ABCD点处。
//N 自定
#define N 3
int i,j;//参数类型尽量大一些，防止溢出
uint8_t line_flag = 0;//判断是否在线上 0不在，1在
void get_ABCD(void) //每隔一定时间调用一次函数，进行判断是否在线上，需要结合N考虑
{if(!Get_Value1 && !Get_Value2 && !Get_Value3 && !Get_Value4 && !Get_Value5 && !Get_Value6 && !Get_Value7 && !Get_Value8){j = 0;if(++i >= N){line_flag = 0;}}else{i = 0;if(++j >= N){line_flag = 1;} }
}
根据以上代码，可以判断小车经过ABCD这样的点几次了，从而可以判断小车到达ABCD的具体点位。
uint8_t last_line_flag = 0;
uint8_t ABCD_count = 0if(last_line_flag != line_flag ) ABCD_count++;
last_line_flag = line_flag;
//每换一题，ABCD_count需要清零五、总结
代码仅作参考，有误之处多担待。有些代码可能写得累赘多余，如果有更好的方法还请不吝赐教！！！
控制参考方案请查看：2024电赛H题参考方案（+视频演示＋核心控制代码）——自动行驶小车-CSDN博客文章浏览阅读5k次，点赞37次，收藏125次。此次电赛的H题属于控制类题目，相较于往年较为简单，功能也算单一，四个题目的时间要求都不是很高，容易得分，其中主要难点可能是TI芯片了，但是资源丰富，那也就不是问题了。控制的难点就在与ABCD四点之间的丝滑连接，如何让小车又快又稳的运行，最后比拼的就是时间了，可能也是比赛现场的重点评判标准，毕竟选择该题的人多。https://blog.csdn.net/qq_67319052/article/details/140763678
