电磁组-19届智能车电磁组电感处理与循迹代码带元素处理+讲解（开源） ...

0、说明：本文章所用代码开源，开源链接放在文章末端

一、电磁循迹的实现-----理论

1、电磁车模的构成

很多小伙伴由于没有打过雷同的比赛，刚开始接触智能车的时候大概会不知所措，会浪费很多时间，我也是19届第一次到场智能车，由于当时还是小白，以是选择了比力简单、比力底子的电磁组，经过这一届智能车比赛也让我对智能车有了一定的相识，本着技能共享、共同进步的理念，接下来我会只管用通俗易懂的方式帮助各人入门智能车（针对电磁组，纯纯是个人理解）。首先附上一张车模照片：

这是我的电磁车的车模，图中圈出来的几个板块电磁模块（电感），运放模块（这里作为模块插在主控板上了），电机驱动模块，主控板皆必要各人自己用嘉立创EDA画板打板焊接，而车模、tof测距、TFT显示屏、编码器、锂电池都是必要我们到淘宝店肆进行自行购买的，接下来我将一一向各人介绍一下各个模块的作用：
电感（电磁模块）：产生感应电动势，用于循迹时计算毛病
碳杆：固定电感，质量小，强度高，各人都这么用
tof测距：距离检测，一般用于检测障碍物
TFT显示屏：用于调试时打印调试信息
主控模块：整个体系的控制
电机驱动：驱动直流电机
编码器：用于电机转速信息的反馈
航模锂电池：整个体系的供电
F车模：必要购买官方指定车模
2、电磁循迹的实现原理

   先讲一下电感的采集，赛道中心会铺设漆包线，就是被漆包起来的铜导线，漆可以绝缘，在导线中通有20KHz，100毫安的互换电，通过电磁感应原理，我们的电磁模块排布了工字电感，电感中会产生感应电动势即电压，由于产生的电压很小，必要经过运放模块进行放大，放大后的电压就可以被我们单片机的ADC读取了，如许就完成了电感采集。
  电磁模块通过杜邦线连接到主控板上的电磁运放模块，运放再连接到焦点板上的ADC口，电磁模块上的电感与电磁线作用产生电压经过放大后通过ADC转化成数字信号大概原始值是几千多，当我们把采集到的值显示到屏幕上以后，可以调节运放模块上面的可调电阻来改变采集到的电感值，一般几千的电感值较大我们都会把电感值归一化到0—100以内来作为循迹利用，归一化的方法就是把车子摆放在赛道上中间，旋转可调电阻找到每个电感值的最大和最小值给他进行归一化（下面会继续讲）。
完成电感的采集后必要我们将电感值进行进一步处理作用到电机上实现小车的循迹运动。先看一下下面这张图：

可以看到在赛道中会有这三种情况，车身在中间、车身偏左、车身偏右，在中间时我们必要保持直线循迹，如上图一，车身偏左必要右转，如上图二、车身偏右必要左转，如上图三，而电感产生的电压是越靠近漆包线越大的，那么，我们只必要利用差比和：毛病值 = 左边电感值 - 右边电感值，假如毛病值 > 0,则左转；毛病值 <0,则右转。
至于如何实现左右转，在19届以前我们电磁车模利用的是四轮车模，只必要控制舵机的的角度来控制转向就可以了，但是19届利用的是三轮车模，前面一个万向轮，后面两个电机驱动轮，我们必要进行差速转向，即左转时，左边电机转的慢大概反转，右边电机转的快；右转时，右边电机转的慢大概反转，左边电机转的快，我们只必要将毛病值乘以一定的转向系数加到电机的PWM上即可。
二、电感的处理

1、电感的采集

我们一般接纳12位ADC进行电感采集，其数字阈值位4095，也就是采集到的最大数字值是4095，下面是逐飞库封装的电感采集函数：

1. `在这里插入//  @brief      ADC转换一次
2. //  @param      adcn            选择ADC通道
3. //  @param      resolution      分辨率
4. //  @return     void
5. //  Sample usage:               adc_convert(ADC_P10, ADC_10BIT);
6. //-------------------------------------------------------------------------------------------------------------------
7. uint16 adc_once(ADCN_enum adcn,ADCRES_enum resolution)
8. {
9.         uint16 adc_value;
10.        
11.         ADC_CONTR &= (0xF0);                        //清除ADC_CHS[3:0] ： ADC 模拟通道选择位
12.         ADC_CONTR |= adcn;
13.        
14.         ADC_CONTR |= 0x40;                          // 启动 AD 转换
15.         while (!(ADC_CONTR & 0x20));          // 查询 ADC 完成标志
16.         ADC_CONTR &= ~0x20;                          // 清完成标志
17.        
18.         adc_value = ADC_RES;                          //存储 ADC 的 12 位结果的高 4 位
19.         adc_value <<= 8;
20.         adc_value |= ADC_RESL;                  //存储 ADC 的 12 位结果的低 8 位
21.        
22.         ADC_RES = 0;
23.         ADC_RESL = 0;
24.        
25.         adc_value >>= resolution;                //取多少位
26.        
28.         return adc_value;
29. }

复制代码

2、电感的的归一化处理与差比和算法

小车在运行时车身会抖动、电感会阶跃、电感采集具有偶尔性因素等导致采集到的电感禁绝确，会对控制产生影响，而为了低沉干扰，我们会对采集到的电感值进行一些简单的滤波处理，使得电感采集变得不那么不规律，让它过分的平滑一些，如许利于小车的运行安稳，下面来讲解电感的处理。
2.1 电感的滤波处理—去极值求平均滤波

有一些特殊情况， 一瞬间电感值突然很大大概很小。 由于运行时间很快， 一瞬间的很大大概很小大概导致整个体系严重震荡， 以是我们引入去极值滤波方法， 所谓去极值， 便是将最大和最小去除，可以很好的解决电感偶尔阶跃的问题。
采集电感是时难免会出现禁绝的情况，以是，去除最大最小值后，为了过滤掉这些禁绝的地方， 我们选择再进行一次均值滤波方式来将误差只管缩小。
   代码实现逻辑是，封装一个去极值求平均的函数，传入一个采集了很多次电感值的数组，分别找到数组的最大值，最小值，把每一电感值求和，再减去最大值和最小值，最后求平均值，其中代码块如下

1. /**
2.   * @brief   取极值滤波函数               
3.   * @param   读取到的电感数组
4.   * @param   无
5.   * @retval  返回去除极值后的平均值        
6.   */
7. int16 ADC_Del_MaxMin_Average_Filter(int16 *ADC) //去掉最大值和最小值（传进来的是数组，存储的是电感值）
8. {
9.         uint8 i;
10.     int16 max,min,average= 0;  //定义极值以及和
11.     uint16 sum = 0;
13.     max = ADC[0]; //初始化最小值和最大值
14.     min = ADC[0];
16.     for(i=0;i<sizeof(ADC);i++) // sizeof(ADC)是传进来的电感数组的长度
17.     {
18.         if(max<ADC[i])        max = ADC[i];  //找出最大的电感
19.         if(min>ADC[i])        min = ADC[i];  //找出最小的电感
20.         sum += ADC[i];  //累加
21.     }
22.    
23.     average =(sum-max-min)/(sizeof(ADC)-2);    //电感的累加值减去最大值和最小值 再求平均
24.     return average;       //将去极值的值传回原函数
25. }

复制代码

2.2 电感的归一化与差比和

(1）为什么要归一化
  经过滤波，我们得到了较为精准的左右电感采集值，横向的电感值得到了优化， 但是纵向的电感值却没有得到很好的改正。 举一个很简单的例子： 当比赛的园地和你练习的园地的电感的电源不同的时，园地的电感值就会变，大概就会导致你原来调的参数并不能很好的驱策车子运作。而且大概你左右电感的特性大概不同，也许左边电感更灵敏大概更容易变大一点，如许也会导致电感值测量的误差。以是我们可以对采集到的电感值做归一化处理。所谓归一化，便是给左右电感一个统一的尺度（0-100）之间，无论其中一个电感多灵敏多大，两侧的电感值都在这个范围之内变化，雷同于给尺子标上了刻度以及零刻度。固然用差比和法也能做到，但是在差比和之进步行归一化，可以有用低沉误差。由此可得归一化可以有用低沉电感放大倍率的影响，使得小车对不同的情况顺应性更强，而且非常有用的使得中线位置更加稳固。再进行差比和就是对电感值进一步做处理，将电感值再一次限制在了0-100之间。
（2）电感归一化和差比和的实现
  对于电感归一化的实现，首先我们必要测量出小车在运动过程中的最小值和最大值，再根据最小值和最大值对其进行归一化处理，得到一个0-1的数，再将其乘以100便得到了归一化后的电感值，最小值一般是0，而最大值必要在园地内里在屏幕里上打印裸电感值读取。差比和就是： 电感的差值 / 电感的和，并限定他们的范围，为了方便起见，我们仍然将其封装成一个函数。最后还要对电感值进行限幅处理，代码如下。

1. /**
2.   * @brief   电感值的最后处理函数，包括读取、去极值、求平均值、 归一化、差比和              
3.   * @param   无
4.   * @param   无
5.   * @retval  无      
6.   */
7. void ADC_Final_Read_Deal()
8. {
9.         uint8 i;
10.         //采集电感数组
11.         int16 filter_buf_L1[FILTER_N];  //左第一个电感储存数组  
12.         int16 filter_buf_R1[FILTER_N];  //右第一个电感储存数组
13.         int16 filter_buf_R2[FILTER_N];
14.         int16 filter_buf_R3[FILTER_N];
15.         int16 filter_buf_M[FILTER_N];   //中间电感存储数组  
16.         //--------采样--------------
17.         for(i = 0; i <FILTER_N; i++)   //采值，采样5次
18.         {
19.                 filter_buf_L1[i]  = adc_once(ADC_P17,ADC_12BIT);  //左一电感数组
20.                 filter_buf_R1[i]  = adc_once(ADC_P05,ADC_12BIT);  //右一电感数组
21.                 filter_buf_M[i]   = adc_once(ADC_P06,ADC_12BIT);  //中间电感
22.                 filter_buf_R2[i]  = adc_once(ADC_P01,ADC_12BIT);  //右二电感数组
23.                 filter_buf_R3[i]  = adc_once(ADC_P00,ADC_12BIT);  //右三电感数组
24.         }
25.         //--------去极值求平均---------
26.         adc_deal_last[LEFT_1]= ADC_Del_MaxMin_Average_Filter(filter_buf_L1);  //左一电感最终值      
27.         adc_deal_last[RIGHT_1] =ADC_Del_MaxMin_Average_Filter(filter_buf_R1); //右一电感最终值
28.         adc_deal_last[MIDDLE] =ADC_Del_MaxMin_Average_Filter(filter_buf_M);   //中间电感最终值
29.         adc_deal_last[RIGHT_2] =ADC_Del_MaxMin_Average_Filter(filter_buf_R2); //右二电感最终值
30.         adc_deal_last[RIGHT_3] =ADC_Del_MaxMin_Average_Filter(filter_buf_R3);   //右三电感最终值
31.        
32.         //归一化电感值
33.         Left_Adc   = (adc_deal_last[LEFT_1]*100) / adc_max[0];
34.         Right_Adc  = (adc_deal_last[RIGHT_1]*100)/ adc_max[1];
35.         Middle_Adc = (adc_deal_last[MIDDLE]*100) / adc_max[2];
36.         Right_Adc2 = (adc_deal_last[RIGHT_2]*100)/ adc_max[3];
37.         Right_Adc3 = (adc_deal_last[RIGHT_3]*100)/ adc_max[4];
38.        
39.         //电感限幅处理
40.         Left_Adc   = ADC_Limit(Left_Adc,ADC_MAX,ADC_MIN);
41.         Right_Adc  = ADC_Limit(Right_Adc,ADC_MAX,ADC_MIN);
42.         Middle_Adc = ADC_Limit(Middle_Adc,ADC_MAX,ADC_MIN);
43.         Right_Adc2 = ADC_Limit(Right_Adc2,ADC_MAX,ADC_MIN);
44.         Right_Adc3 = ADC_Limit(Right_Adc3,ADC_MAX_R3,ADC_MIN);
46.         //三电感融合
47.         AD_Bias = ((Left_Adc-Middle_Adc)*100/(Left_Adc+ Middle_Adc)) -    \
48.               ((Right_Adc-Middle_Adc)*100/(Right_Adc+ Middle_Adc));
49. //        AD_Bias =(Left_Adc-Right_Adc)*100/(Left_Adc+Right_Adc);       //两电感融合
50. }
52. /**
53.   * @brief  电感归一化限幅函数,见电感最终值限幅在0-100范围内               
54.   * @param   输入的ADC值
55.   * @param   限幅最大值
56.   * @retval  最小值   
57.   */
58. int16 ADC_Limit(int16 in_adc,int8 max,int8 min)
59. {
60.         uint16 Adc_Input;
61.         Adc_Input = in_adc;
62.         if(Adc_Input >= max)       Adc_Input = max;
63.         else if(Adc_Input <= min)  Adc_Input = min;
64.         return Adc_Input;
65. }

复制代码

可以看到，由于我们总共有五组电感，以是相同的操作实行了五次（实际七个电感，利用了三个），读者可以进行优化，制止大量重复代码，这里为了低沉调试成本，利用了比力笨拙的办法。其中，电感的差比和可以有简单的两电感差比和、三电感差比和，假如要加速，就可以尝试四电感融合大概其他的循迹算法，差比和比力简单，用的人多。经实行，两电感靠中性（车身靠赛道中心行驶）差一点，不容易出赛道，三电感靠中性好一点，但容易出赛道，其他由于硬件原因没有尝试。
三、电机的控制

1、电机控制与编码器反馈

1.1 PID控制

PID控制有位置式和增量式，这里利用位置式，他们各有优缺点，读者自行分析（csdn大概bilibili有很多讲授讲解），这里主要讲解运用，同时PID控制电机还有速度环、位置环、电流环等，这里只是用速度环，其他读者自行学习相识。
位置式Pid就是位置闭环控制，位置闭环控制就是根据编码器的脉冲累加，测量电机的位置信息，并与目标值进行比力得到一个控制毛病，然后我们对毛病进行比例积分、微分的控制，使毛病趋近于0的一个过程。
速度环：速度反馈给电机，来达到我们想要的结果。
速度环一般是P、I控制就够用，位置环一般是P、D控制也就够用。
长处：
（1）快速响应：快速到达设定的目标值，减小惯性的作用。
（2）速度控制（准、稳）：带负载速度也不改变
通过以上分析，我们在赛场上要求小车响应够快，速度够快，那就不得不利用速度闭环来控制电机了。
更多PID知识可参考：
PID讲解
1.2 电机编码值的获取

为了进行电机闭环控制，我们还必要获取电机当前转速信息，这就不得不利用编码器了，编码器有很多种，光栅编码器有正交编码器、方向编码器等，此外还有霍尔编码器等，这里利用的是逐飞官方的带方向的编码器。
普通光栅编码器原理：将一个码盘一圈匀称打上很多孔，孔相联两端都有一段未打孔的码盘部门，将码盘固定在电机轴上大概外接齿轮上，码盘两边带有光栅，电机转动时，会存在挡住光栅、透光的连续操作，如许只要遮住光栅时编码器返回1给MCU，透光时返回0给MCU，就可以返回不同频率的脉冲值，根据一定时间内脉冲值的多少再联合电机的减速比、编码器一圈的光栅数即可得到电机的实施转速信息（在这里没有进行标量化处理，长处：控制简单，缺点：不知道自己小车的实际速度；标量化处理长处：可以知道小车跑到几米的速度，缺点：计算比力麻烦，容易算错），另外方向编码器还可以反馈方向信息。
编码器更多可参考：
编码器讲解
给出逐飞带方向编码器获取编码值的代码实现：

1. int16 temp_left_pluse = 0;  //int16
2. int16 temp_right_pluse = 0;
4. //读编码器数据
5. void encoderValue_get(void)
6. {
7.         temp_left_pluse =  ctimer_count_read(SPEEDL_PLUSE);   //获取计数值
8.         temp_right_pluse = ctimer_count_read(SPEEDR_PLUSE);
10.         //计数器清零
11.         ctimer_count_clean(SPEEDL_PLUSE);
12.         ctimer_count_clean(SPEEDR_PLUSE);
14.         //采集方向信息
15.     if(1 == SPEEDL_DIR)              //左边电机   
16.         {
17.                 temp_left_pluse = temp_left_pluse;   //正转
18.         }
19.         else                  
20.         {
21.                 temp_left_pluse = -temp_left_pluse ;  //反转
22.         }
23.         if(1 == SPEEDR_DIR)           //右边电机
24.         {
25.            temp_right_pluse  =  -temp_right_pluse ;
26.         }
27.         else                  
28.         {
29.            temp_right_pluse =  temp_right_pluse ;
30.         }                   
31. }

复制代码

1.3 PID算法的实现

为了方便起见，在PID算法中，我们会将控制的很多东西，写进一个结构体内里，必要计算时进行数据存入和读出即可，同时P、I、D三项系数我们可以利用一个数组存储，如许只必要在PID初始化时进行写入即可。PID初始化我们会初始化P、I、D三项系数、初始化输出限幅、初始化积分限幅，初始化其他信息为0，下面给出PID算法的代码框架：

1. //这个PID结构体一般放在头文件中
2. typedef struct
3. {
4.         float speed_kp;
5.         float speed_ki;
6.         float speed_kd;
7.        
8.         float I_out;
9.         float P_out;
10.         float D_out;
11.         float MAX_Iout;
12.         float MAX_out;
13.         float out;
14.        
15.         float error[2];
16.         float feedback_value;
17.         float goal_value;
18. }Motor_pid_t;
21. /**
22.   * @brief 电机PID初始化                 
23.   * @param 电机结构体
24.   * @param 无
25.   * @retval  无         
26.   */
27. void PID_Initialize(Motor_pid_t *motor_pid,const float pid_array[3],float max_out,float max_iout)
28. {
29.         motor_pid->speed_kp = pid_array[0];
30.         motor_pid->speed_ki = pid_array[1];
31.         motor_pid->speed_kd = pid_array[2];
32.         motor_pid->P_out = 0;
33.         motor_pid->I_out = 0;
34.         motor_pid->D_out = 0;
35.         motor_pid->MAX_Iout = max_iout;
36.         motor_pid->MAX_out = max_out;
37.         motor_pid->out = 0;
38.         motor_pid->feedback_value = 0;
39.         motor_pid->goal_value = 0;
40.         motor_pid->error[0] = motor_pid->error[1] = 0;
41. }
43. /**
44.   * @brief  电机PID计算               
45.   * @param  电机结构体数组
46.   * @param  当前值
47.   * @param  目标值
48.   * @retval  当前值         
49.   */
50. float PID_Calculate(Motor_pid_t *Motor_pid,float get,float set)
51. {
52.         Motor_pid->error[0] = set - get;
53.        
54.         Motor_pid->P_out = Motor_pid->speed_kp * Motor_pid->error[0];
55.         Motor_pid->I_out += Motor_pid->speed_ki * Motor_pid->error[0];
56.         Motor_pid->D_out = Motor_pid->speed_kd * (Motor_pid->error[0] - Motor_pid->error[1]);
57.        
58.         PID_LimitMax(Motor_pid->I_out,Motor_pid->MAX_Iout);
59.        
60.         Motor_pid->out = Motor_pid->P_out + Motor_pid->I_out + Motor_pid->D_out;
61.         PID_LimitMax(Motor_pid->out, Motor_pid->MAX_out);
63.         Motor_pid->error[1] = Motor_pid->error[0];
64.         return Motor_pid->out;
65. }
66. /**
67.   * @brief  限幅函数               
68.   * @param  输入的数值
69.   * @param  限幅的最大值
70.   * @retval 限幅后的输出值
71.   */
72. float PID_LimitMax(float intput,float max)
73. {
74.         if(intput>max)             intput = max;
75.         else if(intput<-max)  intput = -max;
76.         return intput;
77. }

复制代码

1.4 电机的控制

在智能车比赛中，直流电机驱动模块一般是有一个电机PWM引脚，有一个电机方向引脚，PWM控制电机转速，方向引脚控制电机转向，0和1分别对应正反转，正反转是相对的，这里给出电机控制代码：

1. /**
2.   * @brief  左电机控制               
3.   * @param  左电机计算出来的占空比
4.   * @param  无
5.   * @retval 无      
6.   */
7. void Motor_Left_Command(float PWM_Left)
8. {
9.         if(PWM_Left>=0)  
10.         {
11.                 pwm_init( PWMA_CH4P_P66 ,17000, 0);   //左边电机正转
12.                 pwm_duty(PWMA_CH3P_P64 ,(int)PWM_Left);
13.         }
14.         else if(PWM_Left<0)
15.         {
16.                 pwm_init( PWMA_CH4P_P66 ,17000, PWM_DUTY_MAX);   //右边电机正转
17.                 pwm_duty(PWMA_CH3P_P64 ,-(int)PWM_Left);  //左边电机
18.         }
19. }
21. /**
22.   * @brief  右电机控制               
23.   * @param  右电机计算出来的PWM
24.   * @param  无
25.   * @retval 无        
26.   */
27. void Motor_Right_Command(float PWM_Right)
28. {
29.         if(PWM_Right>=0)  
30.         {
31.                 pwm_init( PWMA_CH2P_P62 ,17000, 0);   //左边电机正转
32.                 pwm_duty(PWMA_CH1P_P60 ,(int)PWM_Right);
33.         }
34.         else if(PWM_Right<0)
35.         {
36.                 pwm_init( PWMA_CH2P_P62 ,17000, PWM_DUTY_MAX);   //右边电机正转
37.                 pwm_duty(PWMA_CH1P_P60 ,-(int)PWM_Right);  //右边电机
38.         }                                               
39. }

复制代码

这里封装了两个函数，便于调用和调试，假如是一个函数必要多个参数，在后期调试时不好观察。
1.5 小车循迹的实现

• 循迹电机目标值的计算
  通过差比和计算出来的毛病值，我们必要进行再一次计算，也就是乘以一个系数然后作用到电机的目标值上，使得电机存在差速，用于循迹，其次，还有一些其他处理，后面会讲到，比如直角目标值、坡道目标值、直道加速等，这里直道加速逻辑是处在直道上而且超过一定时间，增大电机目标速度。**这里主要展示如何计算电机的目标值，元素细节后面再讲。**计算电机目标值代码如下：
  

1. typedef struct
2. {
3.         float set_encoder_speed;
4.         float get_encoder_speed;
5.         float Turn_bias_speed;         //未使用
6.         float Motor_PWM;
7. }Motor_control; //这个结构体用于保存电机控制信息，放在头文件中
10. /**
11. * @brief   电机需要增加的偏差值计算               
12. * @param   左电机控制结构体
13. * @param   右电机控制结构体
14. * @retval  无
15.   */
16. void Bias_Turn_Control(Motor_control *motor_control_L, Motor_control *motor_control_R)
17. {
18.         int16 bias = 0;           //零时保存电感偏差值
19.         int16 bias_differential = 0;   //保存电感偏差 - 上一次偏差
20.         float bias_speed = 0;
21.         AD_Bias_last = AD_Bias;
22.         bias = AD_Bias;             //从差比和得到的偏差值（全局变量）
23.         bias_differential = AD_Bias - AD_Bias_last;   //计算两次偏差
24.        
25.         bias_speed = BIAS_KP * bias + BIAS_KD * bias_differential;    //计算给到电机的偏差，电感偏差乘以一定的系数
26.         bias_speed = fbs_output(bias_speed);
27.        
28.         motor_control_L->get_encoder_speed = (float)temp_left_pluse;       //读取电机编码值
29.         motor_control_R->get_encoder_speed = (float)temp_right_pluse;
30.        
31.         //下面是设置电机目标值
32.         if(Straight_Angle_flag==1 && Slope_Flag==0)         //检测到直角
33.         {
34.                 if(Left_Adc>Right_Adc)       //左直角
35.                 {
36.                         motor_control_L->set_encoder_speed = STRAIGHT_ANGLE_LEFT_L;     //直角的速度---固定值
37.                         motor_control_R->set_encoder_speed = STRAIGHT_ANGLE_LEFT_R;
38.                 }
39.                 else if(Left_Adc<Right_Adc)  //右直角
40.                 {
41.                         motor_control_L->set_encoder_speed = STRAIGHT_ANGLE_RIGHT_L;
42.                         motor_control_R->set_encoder_speed = STRAIGHT_ANGLE_RIGHT_R;
43.                 }
44.         }
45.         else if(Straight_Angle_flag==0 && Slope_Flag==1)    //坡道处理
46.         {
47.                 motor_control_L->set_encoder_speed = SLOPE_SPEED;    //坡道提速
48.                 motor_control_R->set_encoder_speed = SLOPE_SPEED;
49.         }
50.         else if(Straight_Angle_flag==0 && Slope_Flag==0)       //无特殊元素
51.         {
52.                 if(bias<=5 && bias>= -5 && Accelerate_Count<75)     //判断是否在直道
53.                 {
54.                         Accelerate_Count++;      //计数判断是否直道，一段时间还在直道则加速
55.                         if(bias>=0)
56.                         {
57.                                 motor_control_L->set_encoder_speed = GO_AHEAD_SPEED - bias_speed;
58.                                 motor_control_R->set_encoder_speed = GO_AHEAD_SPEED + bias_speed;
59.                         }
60.                         else if(bias<0)
61.                         {
62.                                 motor_control_L->set_encoder_speed = GO_AHEAD_SPEED + bias_speed;
63.                                 motor_control_R->set_encoder_speed = GO_AHEAD_SPEED - bias_speed;
64.                         }
65.                 }
66.                 else if(bias>5 || bias< -5)     //不在直道，适当降速
67.                 {
68.                         Accelerate_Count = 0;
69.                         if(bias>=0)
70.                         {
71.                                 motor_control_L->set_encoder_speed = GO_AHEAD_SPEED - bias_speed;
72.                                 motor_control_R->set_encoder_speed = GO_AHEAD_SPEED + bias_speed;
73.                         }
74.                         else if(bias<0)
75.                         {
76.                                 motor_control_L->set_encoder_speed = GO_AHEAD_SPEED + bias_speed;
77.                                 motor_control_R->set_encoder_speed = GO_AHEAD_SPEED - bias_speed;
78.                         }
79.                 }
80.                 else if(Accelerate_Count>=75 && bias<=5 && bias>= -5) //计时到一定值，还在直道上，直接判定为直道，开启加速
81.                 {
82.                         if(bias>=0)
83.                         {
84.                                 motor_control_L->set_encoder_speed = STRAIGHT_HIGH_SPEED - bias_speed;  
85.                                 motor_control_R->set_encoder_speed = STRAIGHT_HIGH_SPEED + bias_speed;
86.                         }
87.                         else if(bias<0)
88.                         {
89.                                 motor_control_L->set_encoder_speed = STRAIGHT_HIGH_SPEED + bias_speed;
90.                                 motor_control_R->set_encoder_speed = STRAIGHT_HIGH_SPEED - bias_speed;
91.                         }
92.                 }       
93.         }
94. }

复制代码

• 刚刚已经设置了目标值，下面将进行PID计算，并将计算出来的值给电机
  这里主要展示如何进行电机PID计算并控制电机，元素判断细节后面再讲。PID计算和控制代码：
  

1. /**
2.   * @brief   闭环计算电机需要输出的PWM值               
3.   * @param   无
4.   * @param   无
5.   * @retval  无         
6.   */
7. void Motor_PWM_Final_Control()
8. {
9.         static float Round_PWM_L,Round_PWM_R;
10.         Bias_Turn_Control(&Left_motor, &Right_motor);
11.         if(outtrack_flag==1 && turn_avoid_flag==0 )   //出赛道停车防止撞坏电感和损坏电机
12.         {
13.                 Motor_Left_Command(0);        //停车
14.                 Motor_Right_Command(0);
15.         }
16.         else if(turn_avoid_flag !=0 && (outtrack_flag ==0 || outtrack_flag ==1) && One_avoid_flag!=1)  //过障碍物处理
17.         {
18.                 if(turn_avoid_flag==1)   //状态1，电机固定偏差驶离赛道
19.                 {
20.                 Round_PWM_L = PID_Calculate(&pid_motor_left,Left_motor.get_encoder_speed,AVOID_SPEED_QUIT_L);
21.                 Round_PWM_R = PID_Calculate(&pid_motor_right,Right_motor.get_encoder_speed,AVOID_SPEED_QUIT_R);
22.                 Motor_Left_Command(Round_PWM_L);
23.                 Motor_Right_Command(Round_PWM_R);
24.                 }
25.                 else if(turn_avoid_flag==2)   //状态2，驶回赛道
26.                 {
27.                 Round_PWM_L = PID_Calculate(&pid_motor_left,Left_motor.get_encoder_speed,AVOID_SPEED_BACK_L);
28.                 Round_PWM_R = PID_Calculate(&pid_motor_right,Right_motor.get_encoder_speed,AVOID_SPEED_BACK_R);
29.                 Motor_Left_Command(Round_PWM_L);
30.                 Motor_Right_Command(Round_PWM_R);
31.                 }
32.         }
33.         else if(outtrack_flag==0 )    //未出赛道
34.         {
35.                 Left_motor.Motor_PWM = PID_Calculate(&pid_motor_left,Left_motor.get_encoder_speed,Left_motor.set_encoder_speed);
36.                 Right_motor.Motor_PWM= PID_Calculate(&pid_motor_right,Right_motor.get_encoder_speed,Right_motor.set_encoder_speed);
37.        
38.                 if(Left_motor.Motor_PWM > MAX_OUT)         Left_motor.Motor_PWM =  MAX_OUT;   //保险起见计算出的PWM再次限幅
39.                 else if(Left_motor.Motor_PWM < -MAX_OUT)   Left_motor.Motor_PWM =  -MAX_OUT;   //限幅可以使用PID里定义的限幅函数
40.                 if(Right_motor.Motor_PWM > MAX_OUT)         Right_motor.Motor_PWM =  MAX_OUT;
41.                 else if(Right_motor.Motor_PWM < -MAX_OUT)   Right_motor.Motor_PWM =  -MAX_OUT;
42.                
43.                 if(into_island_flag==1 && out_island_flag==0 && Straight_Angle_flag==0)   //入环岛检测
44.                 {
45.                         P52 = 1;             //调试使用的LED不用管
46.                         Round_PWM_L = PID_Calculate(&pid_motor_left,Left_motor.get_encoder_speed,IN_ROUND_SPEED_L);   
47.                         Round_PWM_R =                  PID_Calculate(&pid_motor_right,Right_motor.get_encoder_speed,IN_ROUND_SPEED_R);  
48.                        
49.                         Motor_Left_Command(Round_PWM_L);    //电机输出
50.                         Motor_Right_Command(Round_PWM_R);
51.                 }
52.                 else if(out_island_flag==1&& Straight_Angle_flag==0 &&(into_island_flag==1 || into_island_flag==0))  //出环岛检测
53.                 {
54.                         P52 = 0;
55.                         Round_PWM_L = PID_Calculate(&pid_motor_left,Left_motor.get_encoder_speed,OUT_ROUND_SPEED_L);  
56.                         Round_PWM_R = PID_Calculate(&pid_motor_right,Right_motor.get_encoder_speed,OUT_ROUND_SPEED_R);  
57.                        
58.                         Motor_Left_Command(Round_PWM_L);    //电机输出
59.                         Motor_Right_Command(Round_PWM_R);
60.                 }
61.                 else if(into_island_flag==0 && out_island_flag==0)      //普通循迹
62.                 {
63.                         P52 = 1;
64.                         Motor_Left_Command(Left_motor.Motor_PWM);           //将输出信号给电机
65.                         Motor_Right_Command(Right_motor.Motor_PWM);
66.                 }
67.         }
68. }

复制代码

四、特殊元素的判断与处理

特殊元素处理可以说是电磁组程序上可以最大优化的地方，每个人都有不同的方式，在这里我大概讲解一下我的方式：我的每一个特殊元素的通过都是利用累计编码值的方式来实现的，比如收支环岛、过障碍、过坡道等。
1、环岛判断和处理

判断环岛每个人的方式都不太一样，没有比力固定的方式，基本上都是在经过环岛时，有些电感会比力非常就以为是环岛，环岛处理有很多方式，比如常见的陀螺仪积分（出环比力容易）、陀螺仪积分固定毛病（可以作弊减小旅程）、入环后循迹做出环检测（出环单独处理）等。我利用的是第三种，当观察到有一些电感非常，而且已经非常超过一段时间时，置位环岛标志，给固定电机目标值并进行PID计算，再发送给电机，在环岛标志位下以固定姿态进入环岛，当旅程累计到达设定值，清空环岛标志位。出环岛也是一样的，检测到非常电感并经过一段时间，置位出环岛标志，给固定电机目标值并进行PID计算，再发送给电机，在出环岛标志位下以固定姿态出环岛，当旅程到达一定值时，以为已经出环，清空出环标志。
入环判断代码（出环岛雷同）如下：

1. 左环岛为例
2. /************ 1.入环岛标志处理 ***********/
3.         if( Left_Adc ==100 && Right_Adc>75)   //Left_Adc>95 && Left_Adc<=100
4.         {
5.                 in_island_cnt++;         //计数一定值认为有环岛
6.                 if(in_island_cnt>=2)
7.                 {
8.                         in_island_cnt=0;
9.                         into_island_flag = 1;  //置位环岛标志
10.                 }
11.         }
13. //确定环岛后通过累计编码值来清空标志位，
14.         if(into_island_flag==1)  IN_island_encoder += temp_right_pluse;  //累计编码值
15.         if(IN_island_encoder >= IN_ISLAND_ENCODER_MAX)     //路程够了，代表已经进入环岛
16.         {
17.                 into_island_flag = 0;         //清空环岛标志位
18.                 IN_island_encoder = 0;
19.         }

复制代码

在代码中，我们可以看到在靠近左环岛时，左边的电感会变成最大值，右边电感也会变得超过一个阈值，只必要在这个状态下保持一小段时间，我们就可以以为这是一个环岛元素，必要进入环岛了。
判断完环岛元素后，还必要将控制值加到电机上，入环岛电机控制代码如下：

1. //入环岛电机控制
2. if(into_island_flag==1 && out_island_flag==0 && Straight_Angle_flag==0)   //入环岛检测
3. {
4.         P52 = 1;             //调试使用的LED不用管
5.         //PID计算
6.         Round_PWM_L = PID_Calculate(&pid_motor_left,Left_motor.get_encoder_speed,IN_ROUND_SPEED_L);   
7.         Round_PWM_R = PID_Calculate(&pid_motor_right,Right_motor.get_encoder_speed,IN_ROUND_SPEED_R);  
8.         //给电机控制值       
9.         Motor_Left_Command(Round_PWM_L);
10.         Motor_Right_Command(Round_PWM_R);
11. }

复制代码

2、十字路口判断和处理

假如我们电感是对称排布，而且利用差比和算法，那么在经过式子路口时我们计算出来的转向毛病应该靠近0，以是不必要做特殊处理，这里我就当成普通元素处理了。
3、直角判断和处理

直角也是一样的，我们会在经过直角时发现有电感出现非常，而且假如是左直角转弯，左边电感值会大一些，右直角反之，只必要出现如许的非常，我们就可以以为这是一个直角元素，置位直角标志位，可以做出直角处理。下面是直角判断代码：

1. /**** 2.直角处理 *****/
2.         //判断直角
3.         if((Left_Adc<50&& Right_Adc<40 && Middle_Adc>25)|| \
4.                 (Right_Adc<50&& Left_Adc<40 && Middle_Adc>25))
5.                 Straight_Angle_flag = 1;                             //直角标志位
6.         if(Straight_Angle_flag==1)  Straight_Angle_encoder += (temp_left_pluse+temp_right_pluse)/2;  //累计编码
7.         if(Straight_Angle_encoder >= STRAIGHT_ANGLE_ENCODER)      //编码记到一定值认为已经转过直角
8.         {
9.                 Straight_Angle_flag = 0;             //标志位清零
10.                 Straight_Angle_encoder = 0;          //累计编码值清零
11.         }
12.         /*****  2.直角处理 ****/

复制代码

刚刚进行了记载直角标志位，如今进行直角处理，我的方法是给电机一个固定的目标值用于转过直角，并在这期间累计编码值，累计（左边电机编码值 + 右边电机编码值）/ 2，如许做是由于左右两边走过的旅程不一样，取平均值能将编码值累计值近似为车在走直线时的编码值累计值，设置固定电机目标值后还必要在控制函数中进行PID计算，当累计编码值大于某一个阈值时，以为车已经通过直角了，可以清空标志位，回归到正常状态。

1. //直角电机目标值设置
2. if(Straight_Angle_flag==1 && Slope_Flag==0)         //检测到直角
3.         {
4.                 if(Left_Adc>Right_Adc)       //左直角
5.                 {
6.                         motor_control_L->set_encoder_speed = STRAIGHT_ANGLE_LEFT_L;     //直角的速度---固定值
7.                         motor_control_R->set_encoder_speed = STRAIGHT_ANGLE_LEFT_R;
8.                 }
9.                 else if(Left_Adc<Right_Adc)  //右直角
10.                 {
11.                         motor_control_L->set_encoder_speed = STRAIGHT_ANGLE_RIGHT_L;
12.                         motor_control_R->set_encoder_speed = STRAIGHT_ANGLE_RIGHT_R;
13.                 }
14.         }

复制代码

4、障碍物判断和处理

过障碍物各人都是利用tof测距大概超声波测距，当发现测距值小于某一阈值时，以为这是一个障碍物，做出相应的处理。但是，比赛过程中有很多时候会误判，将园地里其他的道具误以为是障碍物，如锥桶、坡道侧面、园地边缘等，好在障碍物一般会放在一个比力长的直道上，解决方式是加一些限定条件，我的处理方式是：当检测到障碍物距离小于600毫米，而且如今车在走直道，判断方式多样，可以是差比和小于一个很小的阈值，大概中间电感很小（取决于中间电感的放置方式，竖着放的话，直道为0），而且设置一个标志位用于检测是否通过了一次障碍，过了一次障碍之后，将这个标志位置1，表现再也不会再判断障碍物，只有这个标志为0才可以做出障碍物处理。下面给出判断处理代码：

1. /***** 3.障碍物处理 ******/
2.         //距离小于阈值，并且在走直线
3.         if(dl1b_distance_mm<AVOID_DIS_MIN && Middle_Adc>-5 && Middle_Adc<5)   turn_avoid_flag = 1;  //置障碍标志
4.         if(turn_avoid_flag==1 || turn_avoid_flag==2)  
5.         {
6.                 Aviod_encoder +=(temp_left_pluse+temp_right_pluse)/2;        //累计编码值
7.                 if(Aviod_encoder>= AVOID_TURN_QUIT && turn_avoid_flag==1) turn_avoid_flag=2; //编码值大于驶离障碍阈值转到状态2
8.                 else if(Aviod_encoder>AVIOD_TURN_BUCK && turn_avoid_flag==2)         //状态2
9.                 {
10.                         Aviod_encoder = 0;         
11.                         turn_avoid_flag = 0;   //清空标志位
12.                         Avoid_Count+=1;
13.                         if(Avoid_Count>=1)      
14.                         One_avoid_flag = 1;     //只过一次障碍标志位置1，以后也不会再进行环岛处理
15.                 }  
16.         }
17.         /*****  3.障碍物处理 *****/

复制代码

刚刚只是给出了障碍的判断，并没有进行通过处理，我的处理方式是，有障碍物时是状态1，控制电机差速先往一个方向驶离障碍物（我利用右转驶离），并累计旅程，旅程到一定值时跳转到状态2，状态2下控制电机差速左转驶回赛道（左转驶回赛道），并在期间累计旅程，旅程到达一定值时，清空标志位，并置位只过一次障碍的标志位，这一步在置位标志位时已经进行。下面给出过障碍代码：

1. else if(turn_avoid_flag !=0 && (outtrack_flag ==0 || outtrack_flag ==1) && One_avoid_flag!=1)  //过障碍物处理
2.         {
3.                 if(turn_avoid_flag==1)   //状态1，电机固定偏差驶离赛道
4.                 {
5.                         Round_PWM_L = PID_Calculate(&pid_motor_left,Left_motor.get_encoder_speed,AVOID_SPEED_QUIT_L);
6.                         Round_PWM_R = PID_Calculate(&pid_motor_right,Right_motor.get_encoder_speed,AVOID_SPEED_QUIT_R);
7.                         Motor_Left_Command(Round_PWM_L);
8.                         Motor_Right_Command(Round_PWM_R);
9.                 }
10.                 else if(turn_avoid_flag==2)   //状态2，驶回赛道
11.                 {
12.                         Round_PWM_L = PID_Calculate(&pid_motor_left,Left_motor.get_encoder_speed,AVOID_SPEED_BACK_L);
13.                         Round_PWM_R = PID_Calculate(&pid_motor_right,Right_motor.get_encoder_speed,AVOID_SPEED_BACK_R);
14.                         Motor_Left_Command(Round_PWM_L);
15.                         Motor_Right_Command(Round_PWM_R);
16.                 }
17.         }

复制代码

5、坡道判断和处理

坡道也是和障碍物差不多，坡道也会摆在一个比力长的直道上，为了不和障碍物处理辩说，在比赛前应该将车放在一个先过障碍物，再过坡道的方向上发车，如许比力好处理，处理方式：假如车在直道上（中间电感大概差比和毛病判断），测距值小于某一阈值，大概说左右两边电感同时达到一个很大的值（由于在进入坡道时，由于电磁前瞻，电感和电磁线的距离会比其他元素下要小），那么可以以为这是一个坡道，坡道上，为了防止速度太小上不去坡道，我们必要得当提速并累计编码值，当编码值大于某一阈值时，清空标志位，如许就完成了坡道的通过。下面是代码：

1. /** 坡道处理 ***/
2.         //一次坡道标志为1，距离小于阈值，或者两边电感很大，并且在直道上
3.         if((One_avoid_flag==1 && dl1b_distance_mm<=SLOPE_DISTANCE ||(Left_Adc==100&&Right_Adc==100)) && \
4.                 (Middle_Adc>-5&&Middle_Adc<5))
5.                 Slope_Flag = 1;              //坡道标志位
6.         if(Slope_Flag==1)  
7.         {
8.                 Slope_Encoder+=(temp_left_pluse+temp_right_pluse)/2;      //累计编码值
9.                 if(Slope_Encoder>STROUGH_SLOPE_RNCODER)   Slope_Flag = 0;   //编码值大于阈值，清空标志
10.         }
11.         /** 坡道处理 ***/

复制代码

提速控制，给电机一个固定目标值，代码如下：

1. else if(Straight_Angle_flag==0 && Slope_Flag==1)    //坡道处理
2.         {
3.                 motor_control_L->set_encoder_speed = SLOPE_SPEED;    //坡道提速
4.                 motor_control_R->set_encoder_speed = SLOPE_SPEED;
5.         }

复制代码

五、总结和结果展示

对于上面利用到的代码，我将所有的代码放在了gitee上进行开源，感兴趣的小伙伴可以直接去下载源码
1、gitee简介

Gitee（码云）是开源中国于 2013 年推出的基于 Git 的代码托管平台、企业级研发效能平台，提供中国本土化的代码托管服务。gitee和github功能基本一样，只不过github是国外的代码托管平台，而gitee是国内的平台，由于在国内很难对Github进行访问，很多人利用gitee代替，我如今也特别喜好利用这个工具，它可以生存你的代码、下载别人的开源代码、对自己的代码进行版本回退等，方便了很多开发工作。
只必要在欣赏器搜索 gitee,然后注册一个账号就可以利用了。
2、本代码开源链接

打开欣赏器粘贴以下链接进行下载（确保有一个gitee账号）：
   https://gitee.com/fantasy-q/19_-intelligent-car_-eleltronic-team
  3、总结与展示

3.1总结

本次分享内容大概存在一些问题，接待各人在评论区留言，假如还有源码不懂的地方也可以在评论区进行提问，我也会热心的回答各人的问题。
另外，在源码中还有其他的一些处理，比如直道加速、出赛道检测等，各人可以下载源码查看。
3.2 结果展示

19届电磁组，我到场了安徽赛区的比赛，现场差点没完赛，侥幸混了一个省二，没有拍到现场完赛视频，但是确实可以跑全元素，这里给出一个调试的视频（没有障碍和坡道）：

     电磁视频
  

免责声明：如果侵犯了您的权益，请联系站长，我们会及时删除侵权内容，谢谢合作！更多信息从访问主页：qidao123.com:ToB企服之家，中国第一个企服评测及商务社交产业平台。