电磁组-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，下面是逐飞库封装的电感采集函数：
`在这里插入// @brief ADC转换一次
// @param adcn 选择ADC通道
// @param resolution 分辨率
// @return void
// Sample usage: adc_convert(ADC_P10, ADC_10BIT);
//-------------------------------------------------------------------------------------------------------------------
uint16 adc_once(ADCN_enum adcn,ADCRES_enum resolution)
{
uint16 adc_value;
ADC_CONTR &= (0xF0); //清除ADC_CHS[3:0] ： ADC 模拟通道选择位
ADC_CONTR |= adcn;
ADC_CONTR |= 0x40; // 启动 AD 转换
while (!(ADC_CONTR & 0x20)); // 查询 ADC 完成标志
ADC_CONTR &= ~0x20; // 清完成标志
adc_value = ADC_RES; //存储 ADC 的 12 位结果的高 4 位
adc_value <<= 8;
adc_value |= ADC_RESL; //存储 ADC 的 12 位结果的低 8 位
ADC_RES = 0;
ADC_RESL = 0;
adc_value >>= resolution; //取多少位
return adc_value;
}
2、电感的的归一化处理与差比和算法
小车在运行时车身会抖动、电感会阶跃、电感采集具有偶然性因素等导致采集到的电感不准确，会对控制产生影响，而为了降低干扰，我们会对采集到的电感值进行一些简单的滤波处理，使得电感采集变得不那么不规律，让它过度的平滑一些，这样利于小车的运行平稳，下面来讲解电感的处理。
2.1 电感的滤波处理—去极值求平均滤波
有一些特殊情况， 一瞬间电感值突然很大或者很小。 由于运行时间很快， 一瞬间的很大或者很小可能导致整个系统严重震荡， 所以我们引入去极值滤波方法， 所谓去极值， 便是将最大和最小去除，可以很好的解决电感偶然阶跃的问题。
采集电感是时难免会出现不准的情况，所以，去除最大最小值后，为了过滤掉这些不准的地方， 我们选择再进行一次均值滤波方式来将误差尽量缩小。
代码实现逻辑是，封装一个去极值求平均的函数，传入一个采集了很多次电感值的数组，分别找到数组的最大值，最小值，把每一电感值求和，再减去最大值和最小值，最后求平均值，其中代码块如下
/**
* @brief 取极值滤波函数
* @param 读取到的电感数组
* @param 无
* @retval 返回去除极值后的平均值
*/
int16 ADC_Del_MaxMin_Average_Filter(int16 *ADC) //去掉最大值和最小值（传进来的是数组，存储的是电感值）
{
uint8 i;
int16 max,min,average= 0; //定义极值以及和
uint16 sum = 0;
max = ADC[0]; //初始化最小值和最大值
min = ADC[0];
for(i=0;i<sizeof(ADC);i++) // sizeof(ADC)是传进来的电感数组的长度
{
if(max<ADC[i]) max = ADC[i]; //找出最大的电感
if(min>ADC[i]) min = ADC[i]; //找出最小的电感
sum += ADC[i]; //累加
}
average =(sum-max-min)/(sizeof(ADC)-2); //电感的累加值减去最大值和最小值 再求平均
return average; //将去极值的值传回原函数
}
2.2 电感的归一化与差比和
(1）为什么要归一化
经过滤波，我们得到了较为精准的左右电感采集值，横向的电感值得到了优化， 但是纵向的电感值却没有得到很好的矫正。 举一个很简单的例子： 当比赛的场地和你训练的场地的电感的电源不同的时，场地的电感值就会变，可能就会导致你原来调的参数并不能很好的驱使车子运作。并且或许你左右电感的特性可能不同，也许左边电感更灵敏或者更容易变大一点，这样也会导致电感值测量的误差。所以我们可以对采集到的电感值做归一化处理。所谓归一化，便是给左右电感一个统一的标准（0-100）之间，无论其中一个电感多灵敏多大，两侧的电感值都在这个范围之内变化，类似于给尺子标上了刻度以及零刻度。当然用差比和法也能做到，但是在差比和之前进行归一化，可以有效降低误差。由此可得归一化可以有效降低电感放大倍率的影响，使得小车对不同的环境适应性更强，并且非常有效的使得中线位置更加稳定。再进行差比和就是对电感值进一步做处理，将电感值再一次限制在了0-100之间。
（2）电感归一化和差比和的实现
对于电感归一化的实现，首先我们需要测量出小车在运动过程中的最小值和最大值，再根据最小值和最大值对其进行归一化处理，得到一个0-1的数，再将其乘以100便得到了归一化后的电感值，最小值一般是0，而最大值需要在场地里面在屏幕里上打印裸电感值读取。差比和就是： 电感的差值 / 电感的和，并限定他们的范围，为了方便起见，我们仍然将其封装成一个函数。最后还要对电感值进行限幅处理，代码如下。
/**
* @brief 电感值的最后处理函数，包括读取、去极值、求平均值、 归一化、差比和
* @param 无
* @param 无
* @retval 无
*/
void ADC_Final_Read_Deal()
{
uint8 i;
//采集电感数组
int16 filter_buf_L1[FILTER_N]; //左第一个电感储存数组
int16 filter_buf_R1[FILTER_N]; //右第一个电感储存数组
int16 filter_buf_R2[FILTER_N];
int16 filter_buf_R3[FILTER_N];
int16 filter_buf_M[FILTER_N]; //中间电感存储数组
//--------采样--------------
for(i = 0; i <FILTER_N; i++) //采值，采样5次
{
filter_buf_L1[i] = adc_once(ADC_P17,ADC_12BIT); //左一电感数组
filter_buf_R1[i] = adc_once(ADC_P05,ADC_12BIT); //右一电感数组
filter_buf_M[i] = adc_once(ADC_P06,ADC_12BIT); //中间电感
filter_buf_R2[i] = adc_once(ADC_P01,ADC_12BIT); //右二电感数组
filter_buf_R3[i] = adc_once(ADC_P00,ADC_12BIT); //右三电感数组
}
//--------去极值求平均---------
adc_deal_last[LEFT_1]= ADC_Del_MaxMin_Average_Filter(filter_buf_L1); //左一电感最终值
adc_deal_last[RIGHT_1] =ADC_Del_MaxMin_Average_Filter(filter_buf_R1); //右一电感最终值
adc_deal_last[MIDDLE] =ADC_Del_MaxMin_Average_Filter(filter_buf_M); //中间电感最终值
adc_deal_last[RIGHT_2] =ADC_Del_MaxMin_Average_Filter(filter_buf_R2); //右二电感最终值
adc_deal_last[RIGHT_3] =ADC_Del_MaxMin_Average_Filter(filter_buf_R3); //右三电感最终值
//归一化电感值
Left_Adc = (adc_deal_last[LEFT_1]*100) / adc_max[0];
Right_Adc = (adc_deal_last[RIGHT_1]*100)/ adc_max[1];
Middle_Adc = (adc_deal_last[MIDDLE]*100) / adc_max[2];
Right_Adc2 = (adc_deal_last[RIGHT_2]*100)/ adc_max[3];
Right_Adc3 = (adc_deal_last[RIGHT_3]*100)/ adc_max[4];
//电感限幅处理
Left_Adc = ADC_Limit(Left_Adc,ADC_MAX,ADC_MIN);
Right_Adc = ADC_Limit(Right_Adc,ADC_MAX,ADC_MIN);
Middle_Adc = ADC_Limit(Middle_Adc,ADC_MAX,ADC_MIN);
Right_Adc2 = ADC_Limit(Right_Adc2,ADC_MAX,ADC_MIN);
Right_Adc3 = ADC_Limit(Right_Adc3,ADC_MAX_R3,ADC_MIN);
//三电感融合
AD_Bias = ((Left_Adc-Middle_Adc)*100/(Left_Adc+ Middle_Adc)) - \
((Right_Adc-Middle_Adc)*100/(Right_Adc+ Middle_Adc));
// AD_Bias =(Left_Adc-Right_Adc)*100/(Left_Adc+Right_Adc); //两电感融合
}
/**
* @brief 电感归一化限幅函数,见电感最终值限幅在0-100范围内
* @param 输入的ADC值
* @param 限幅最大值
* @retval 最小值
*/
int16 ADC_Limit(int16 in_adc,int8 max,int8 min)
{
uint16 Adc_Input;
Adc_Input = in_adc;
if(Adc_Input >= max) Adc_Input = max;
else if(Adc_Input <= min) Adc_Input = min;
return Adc_Input;
}
可以看到，由于我们总共有五组电感，所以相同的操作执行了五次（实际七个电感，使用了三个），读者可以进行优化，避免大量重复代码，这里为了降低调试成本，使用了比较笨拙的办法。其中，电感的差比和可以有简单的两电感差比和、三电感差比和，如果要加速，就可以尝试四电感融合或者其他的循迹算法，差比和比较简单，用的人多。经实验，两电感靠中性（车身靠赛道中心行驶）差一点，不容易出赛道，三电感靠中性好一点，但容易出赛道，其他由于硬件原因没有尝试。
三、电机的控制
1、电机控制与编码器反馈
1.1 PID控制
PID控制有位置式和增量式，这里使用位置式，他们各有优缺点，读者自行分析（csdn或者bilibili有很多教学讲解），这里主要讲解运用，同时PID控制电机还有速度环、位置环、电流环等，这里只是用速度环，其他读者自行学习了解。
位置式Pid就是位置闭环控制，位置闭环控制就是根据编码器的脉冲累加，测量电机的位置信息，并与目标值进行比较得到一个控制偏差，然后我们对偏差进行比例积分、微分的控制，使偏差趋近于0的一个过程。
速度环：速度反馈给电机，来达到我们想要的效果。
速度环一般是P、I控制就够用，位置环一般是P、D控制也就够用。
优点：
（1）快速响应：快速到达设定的目标值，减小惯性的作用。
（2）速度控制（准、稳）：带负载速度也不改变
通过以上分析，我们在赛场上要求小车响应够快，速度够快，那就不得不使用速度闭环来控制电机了。
更多PID知识可参考：
PID讲解
1.2 电机编码值的获取
为了进行电机闭环控制，我们还需要获取电机当前转速信息，这就不得不使用编码器了，编码器有很多种，光栅编码器有正交编码器、方向编码器等，此外还有霍尔编码器等，这里使用的是逐飞官方的带方向的编码器。
普通光栅编码器原理：将一个码盘一圈均匀打上很多孔，孔相联两端都有一段未打孔的码盘部分，将码盘固定在电机轴上或者外接齿轮上，码盘两边带有光栅，电机转动时，会存在挡住光栅、透光的连续操作，这样只要遮住光栅时编码器返回1给MCU，透光时返回0给MCU，就可以返回不同频率的脉冲值，根据一定时间内脉冲值的多少再结合电机的减速比、编码器一圈的光栅数即可得到电机的实施转速信息（在这里没有进行标量化处理，优点：控制简单，缺点：不知道自己小车的实际速度；标量化处理优点：可以知道小车跑到几米的速度，缺点：计算比较麻烦，容易算错），另外方向编码器还可以反馈方向信息。
编码器更多可参考：
编码器讲解
给出逐飞带方向编码器获取编码值的代码实现：
int16 temp_left_pluse = 0; //int16
int16 temp_right_pluse = 0;
//读编码器数据
void encoderValue_get(void)
{
temp_left_pluse = ctimer_count_read(SPEEDL_PLUSE); //获取计数值
temp_right_pluse = ctimer_count_read(SPEEDR_PLUSE);
//计数器清零
ctimer_count_clean(SPEEDL_PLUSE);
ctimer_count_clean(SPEEDR_PLUSE);
//采集方向信息
if(1 == SPEEDL_DIR) //左边电机
{
temp_left_pluse = temp_left_pluse; //正转
}
else
{
temp_left_pluse = -temp_left_pluse ; //反转
}
if(1 == SPEEDR_DIR) //右边电机
{
temp_right_pluse = -temp_right_pluse ;
}
else
{
temp_right_pluse = temp_right_pluse ;
}
}
1.3 PID算法的实现
为了方便起见，在PID算法中，我们会将控制的很多东西，写进一个结构体里面，需要计算时进行数据存入和读出即可，同时P、I、D三项系数我们可以使用一个数组存储，这样只需要在PID初始化时进行写入即可。PID初始化我们会初始化P、I、D三项系数、初始化输出限幅、初始化积分限幅，初始化其他信息为0，下面给出PID算法的代码框架：
//这个PID结构体一般放在头文件中
typedef struct
{
float speed_kp;
float speed_ki;
float speed_kd;
float I_out;
float P_out;
float D_out;
float MAX_Iout;
float MAX_out;
float out;
float error[2];
float feedback_value;
float goal_value;
}Motor_pid_t;
/**
* @brief 电机PID初始化
* @param 电机结构体
* @param 无
* @retval 无
*/
void PID_Initialize(Motor_pid_t *motor_pid,const float pid_array[3],float max_out,float max_iout)
{
motor_pid->speed_kp = pid_array[0];
motor_pid->speed_ki = pid_array[1];
motor_pid->speed_kd = pid_array[2];
motor_pid->P_out = 0;
motor_pid->I_out = 0;
motor_pid->D_out = 0;
motor_pid->MAX_Iout = max_iout;
motor_pid->MAX_out = max_out;
motor_pid->out = 0;
motor_pid->feedback_value = 0;
motor_pid->goal_value = 0;
motor_pid->error[0] = motor_pid->error[1] = 0;
}
/**
* @brief 电机PID计算
* @param 电机结构体数组
* @param 当前值
* @param 目标值
* @retval 当前值
*/
float PID_Calculate(Motor_pid_t *Motor_pid,float get,float set)
{
Motor_pid->error[0] = set - get;
Motor_pid->P_out = Motor_pid->speed_kp * Motor_pid->error[0];
Motor_pid->I_out += Motor_pid->speed_ki * Motor_pid->error[0];
Motor_pid->D_out = Motor_pid->speed_kd * (Motor_pid->error[0] - Motor_pid->error[1]);
PID_LimitMax(Motor_pid->I_out,Motor_pid->MAX_Iout);
Motor_pid->out = Motor_pid->P_out + Motor_pid->I_out + Motor_pid->D_out;
PID_LimitMax(Motor_pid->out, Motor_pid->MAX_out);
Motor_pid->error[1] = Motor_pid->error[0];
return Motor_pid->out;
}
/**
* @brief 限幅函数
* @param 输入的数值
* @param 限幅的最大值
* @retval 限幅后的输出值
*/
float PID_LimitMax(float intput,float max)
{
if(intput>max) intput = max;
else if(intput<-max) intput = -max;
return intput;
}
1.4 电机的控制
在智能车比赛中，直流电机驱动模块一般是有一个电机PWM引脚，有一个电机方向引脚，PWM控制电机转速，方向引脚控制电机转向，0和1分别对应正反转，正反转是相对的，这里给出电机控制代码：
/**
* @brief 左电机控制
* @param 左电机计算出来的占空比
* @param 无
* @retval 无
*/
void Motor_Left_Command(float PWM_Left)
{
if(PWM_Left>=0)
{
pwm_init( PWMA_CH4P_P66 ,17000, 0); //左边电机正转
pwm_duty(PWMA_CH3P_P64 ,(int)PWM_Left);
}
else if(PWM_Left<0)
{
pwm_init( PWMA_CH4P_P66 ,17000, PWM_DUTY_MAX); //右边电机正转
pwm_duty(PWMA_CH3P_P64 ,-(int)PWM_Left); //左边电机
}
}
/**
* @brief 右电机控制
* @param 右电机计算出来的PWM
* @param 无
* @retval 无
*/
void Motor_Right_Command(float PWM_Right)
{
if(PWM_Right>=0)
{
pwm_init( PWMA_CH2P_P62 ,17000, 0); //左边电机正转
pwm_duty(PWMA_CH1P_P60 ,(int)PWM_Right);
}
else if(PWM_Right<0)
{
pwm_init( PWMA_CH2P_P62 ,17000, PWM_DUTY_MAX); //右边电机正转
pwm_duty(PWMA_CH1P_P60 ,-(int)PWM_Right); //右边电机
}
}
这里封装了两个函数，便于调用和调试，如果是一个函数需要多个参数，在后期调试时不好观察。
1.5 小车循迹的实现
- 循迹电机目标值的计算
通过差比和计算出来的偏差值，我们需要进行再一次计算，也就是乘以一个系数然后作用到电机的目标值上，使得电机存在差速，用于循迹，其次，还有一些其他处理，后面会讲到，比如直角目标值、坡道目标值、直道加速等，这里直道加速逻辑是处在直道上并且超过一定时间，增大电机目标速度。**这里主要展示如何计算电机的目标值，元素细节后面再讲。**计算电机目标值代码如下：
typedef struct
{
float set_encoder_speed;
float get_encoder_speed;
float Turn_bias_speed; //未使用
float Motor_PWM;
}Motor_control; //这个结构体用于保存电机控制信息，放在头文件中
/**
* @brief 电机需要增加的偏差值计算
* @param 左电机控制结构体
* @param 右电机控制结构体
* @retval 无
*/
void Bias_Turn_Control(Motor_control *motor_control_L, Motor_control *motor_control_R)
{
int16 bias = 0; //零时保存电感偏差值
int16 bias_differential = 0; //保存电感偏差 - 上一次偏差
float bias_speed = 0;
AD_Bias_last = AD_Bias;
bias = AD_Bias; //从差比和得到的偏差值（全局变量）
bias_differential = AD_Bias - AD_Bias_last; //计算两次偏差
bias_speed = BIAS_KP * bias + BIAS_KD * bias_differential; //计算给到电机的偏差，电感偏差乘以一定的系数
bias_speed = fbs_output(bias_speed);
motor_control_L->get_encoder_speed = (float)temp_left_pluse; //读取电机编码值
motor_control_R->get_encoder_speed = (float)temp_right_pluse;
//下面是设置电机目标值
if(Straight_Angle_flag==1 && Slope_Flag==0) //检测到直角
{
if(Left_Adc>Right_Adc) //左直角
{
motor_control_L->set_encoder_speed = STRAIGHT_ANGLE_LEFT_L; //直角的速度---固定值
motor_control_R->set_encoder_speed = STRAIGHT_ANGLE_LEFT_R;
}
else if(Left_Adc<Right_Adc) //右直角
{
motor_control_L->set_encoder_speed = STRAIGHT_ANGLE_RIGHT_L;
motor_control_R->set_encoder_speed = STRAIGHT_ANGLE_RIGHT_R;
}
}
else if(Straight_Angle_flag==0 && Slope_Flag==1) //坡道处理
{
motor_control_L->set_encoder_speed = SLOPE_SPEED; //坡道提速
motor_control_R->set_encoder_speed = SLOPE_SPEED;
}
else if(Straight_Angle_flag==0 && Slope_Flag==0) //无特殊元素
{
if(bias<=5 && bias>= -5 && Accelerate_Count<75) //判断是否在直道
{
Accelerate_Count++; //计数判断是否直道，一段时间还在直道则加速
if(bias>=0)
{
motor_control_L->set_encoder_speed = GO_AHEAD_SPEED - bias_speed;
motor_control_R->set_encoder_speed = GO_AHEAD_SPEED + bias_speed;
}
else if(bias<0)
{
motor_control_L->set_encoder_speed = GO_AHEAD_SPEED + bias_speed;
motor_control_R->set_encoder_speed = GO_AHEAD_SPEED - bias_speed;
}
}
else if(bias>5 || bias< -5) //不在直道，适当降速
{
Accelerate_Count = 0;
if(bias>=0)
{
motor_control_L->set_encoder_speed = GO_AHEAD_SPEED - bias_speed;
motor_control_R->set_encoder_speed = GO_AHEAD_SPEED + bias_speed;
}
else if(bias<0)
{
motor_control_L->set_encoder_speed = GO_AHEAD_SPEED + bias_speed;
motor_control_R->set_encoder_speed = GO_AHEAD_SPEED - bias_speed;
}
}
else if(Accelerate_Count>=75 && bias<=5 && bias>= -5) //计时到一定值，还在直道上，直接判定为直道，开启加速
{
if(bias>=0)
{
motor_control_L->set_encoder_speed = STRAIGHT_HIGH_SPEED - bias_speed;
motor_control_R->set_encoder_speed = STRAIGHT_HIGH_SPEED + bias_speed;
}
else if(bias<0)
{
motor_control_L->set_encoder_speed = STRAIGHT_HIGH_SPEED + bias_speed;
motor_control_R->set_encoder_speed = STRAIGHT_HIGH_SPEED - bias_speed;
}
}
}
}
- 刚刚已经设置了目标值，下面将进行PID计算，并将计算出来的值给电机
这里主要展示如何进行电机PID计算并控制电机，元素判断细节后面再讲。PID计算和控制代码：
/**
* @brief 闭环计算电机需要输出的PWM值
* @param 无
* @param 无
* @retval 无
*/
void Motor_PWM_Final_Control()
{
static float Round_PWM_L,Round_PWM_R;
Bias_Turn_Control(&Left_motor, &Right_motor);
if(outtrack_flag==1 && turn_avoid_flag==0 ) //出赛道停车防止撞坏电感和损坏电机
{
Motor_Left_Command(0); //停车
Motor_Right_Command(0);
}
else if(turn_avoid_flag !=0 && (outtrack_flag ==0 || outtrack_flag ==1) && One_avoid_flag!=1) //过障碍物处理
{
if(turn_avoid_flag==1) //状态1，电机固定偏差驶离赛道
{
Round_PWM_L = PID_Calculate(&pid_motor_left,Left_motor.get_encoder_speed,AVOID_SPEED_QUIT_L);
Round_PWM_R = PID_Calculate(&pid_motor_right,Right_motor.get_encoder_speed,AVOID_SPEED_QUIT_R);
Motor_Left_Command(Round_PWM_L);
Motor_Right_Command(Round_PWM_R);
}
else if(turn_avoid_flag==2) //状态2，驶回赛道
{
Round_PWM_L = PID_Calculate(&pid_motor_left,Left_motor.get_encoder_speed,AVOID_SPEED_BACK_L);
Round_PWM_R = PID_Calculate(&pid_motor_right,Right_motor.get_encoder_speed,AVOID_SPEED_BACK_R);
Motor_Left_Command(Round_PWM_L);
Motor_Right_Command(Round_PWM_R);
}
}
else if(outtrack_flag==0 ) //未出赛道
{
Left_motor.Motor_PWM = PID_Calculate(&pid_motor_left,Left_motor.get_encoder_speed,Left_motor.set_encoder_speed);
Right_motor.Motor_PWM= PID_Calculate(&pid_motor_right,Right_motor.get_encoder_speed,Right_motor.set_encoder_speed);
if(Left_motor.Motor_PWM > MAX_OUT) Left_motor.Motor_PWM = MAX_OUT; //保险起见计算出的PWM再次限幅
else if(Left_motor.Motor_PWM < -MAX_OUT) Left_motor.Motor_PWM = -MAX_OUT; //限幅可以使用PID里定义的限幅函数
if(Right_motor.Motor_PWM > MAX_OUT) Right_motor.Motor_PWM = MAX_OUT;
else if(Right_motor.Motor_PWM < -MAX_OUT) Right_motor.Motor_PWM = -MAX_OUT;
if(into_island_flag==1 && out_island_flag==0 && Straight_Angle_flag==0) //入环岛检测
{
P52 = 1; //调试使用的LED不用管
Round_PWM_L = PID_Calculate(&pid_motor_left,Left_motor.get_encoder_speed,IN_ROUND_SPEED_L);
Round_PWM_R = PID_Calculate(&pid_motor_right,Right_motor.get_encoder_speed,IN_ROUND_SPEED_R);
Motor_Left_Command(Round_PWM_L); //电机输出
Motor_Right_Command(Round_PWM_R);
}
else if(out_island_flag==1&& Straight_Angle_flag==0 &&(into_island_flag==1 || into_island_flag==0)) //出环岛检测
{
P52 = 0;
Round_PWM_L = PID_Calculate(&pid_motor_left,Left_motor.get_encoder_speed,OUT_ROUND_SPEED_L);
Round_PWM_R = PID_Calculate(&pid_motor_right,Right_motor.get_encoder_speed,OUT_ROUND_SPEED_R);
Motor_Left_Command(Round_PWM_L); //电机输出
Motor_Right_Command(Round_PWM_R);
}
else if(into_island_flag==0 && out_island_flag==0) //普通循迹
{
P52 = 1;
Motor_Left_Command(Left_motor.Motor_PWM); //将输出信号给电机
Motor_Right_Command(Right_motor.Motor_PWM);
}
}
}
四、特殊元素的判断与处理
特殊元素处理可以说是电磁组程序上可以最大优化的地方，每个人都有不同的方式，在这里我大概讲解一下我的方式：我的每一个特殊元素的通过都是使用累计编码值的方式来实现的，比如出入环岛、过障碍、过坡道等。
1、环岛判断和处理
判断环岛每个人的方式都不太一样，没有比较固定的方式，基本上都是在经过环岛时，有些电感会比较异常就认为是环岛，环岛处理有很多方式，比如常见的陀螺仪积分（出环比较容易）、陀螺仪积分固定偏差（可以作弊减小路程）、入环后循迹做出环检测（出环单独处理）等。我使用的是第三种，当观察到有一些电感异常，并且已经异常超过一段时间时，置位环岛标志，给固定电机目标值并进行PID计算，再发送给电机，在环岛标志位下以固定姿态进入环岛，当路程累计到达设定值，清空环岛标志位。出环岛也是一样的，检测到异常电感并经过一段时间，置位出环岛标志，给固定电机目标值并进行PID计算，再发送给电机，在出环岛标志位下以固定姿态出环岛，当路程到达一定值时，认为已经出环，清空出环标志。
入环判断代码（出环岛类似）如下：
左环岛为例
/************ 1.入环岛标志处理 ***********/
if( Left_Adc ==100 && Right_Adc>75) //Left_Adc>95 && Left_Adc<=100
{
in_island_cnt++; //计数一定值认为有环岛
if(in_island_cnt>=2)
{
in_island_cnt=0;
into_island_flag = 1; //置位环岛标志
}
}
//确定环岛后通过累计编码值来清空标志位，
if(into_island_flag==1) IN_island_encoder += temp_right_pluse; //累计编码值
if(IN_island_encoder >= IN_ISLAND_ENCODER_MAX) //路程够了，代表已经进入环岛
{
into_island_flag = 0; //清空环岛标志位
IN_island_encoder = 0;
}
在代码中，我们可以看到在接近左环岛时，左边的电感会变成最大值，右边电感也会变得超过一个阈值，只需要在这个状态下保持一小段时间，我们就可以认为这是一个环岛元素，需要进入环岛了。
判断完环岛元素后，还需要将控制值加到电机上，入环岛电机控制代码如下：
//入环岛电机控制
if(into_island_flag==1 && out_island_flag==0 && Straight_Angle_flag==0) //入环岛检测
{
P52 = 1; //调试使用的LED不用管
//PID计算
Round_PWM_L = PID_Calculate(&pid_motor_left,Left_motor.get_encoder_speed,IN_ROUND_SPEED_L);
Round_PWM_R = PID_Calculate(&pid_motor_right,Right_motor.get_encoder_speed,IN_ROUND_SPEED_R);
//给电机控制值
Motor_Left_Command(Round_PWM_L);
Motor_Right_Command(Round_PWM_R);
}
2、十字路口判断和处理
如果我们电感是对称排布，并且使用差比和算法，那么在经过式子路口时我们计算出来的转向偏差应该接近0，所以不需要做特殊处理，这里我就当成普通元素处理了。
3、直角判断和处理
直角也是一样的，我们会在经过直角时发现有电感出现异常，并且如果是左直角转弯，左边电感值会大一些，右直角反之，只需要出现这样的异常，我们就可以认为这是一个直角元素，置位直角标志位，可以做出直角处理。下面是直角判断代码：
/**** 2.直角处理 *****/
//判断直角
if((Left_Adc<50&& Right_Adc<40 && Middle_Adc>25)|| \
(Right_Adc<50&& Left_Adc<40 && Middle_Adc>25))
Straight_Angle_flag = 1; //直角标志位
if(Straight_Angle_flag==1) Straight_Angle_encoder += (temp_left_pluse+temp_right_pluse)/2; //累计编码
if(Straight_Angle_encoder >= STRAIGHT_ANGLE_ENCODER) //编码记到一定值认为已经转过直角
{
Straight_Angle_flag = 0; //标志位清零
Straight_Angle_encoder = 0; //累计编码值清零
}
/***** 2.直角处理 ****/
刚刚进行了记录直角标志位，现在进行直角处理，我的方法是给电机一个固定的目标值用于转过直角，并在这期间累计编码值，累计（左边电机编码值 + 右边电机编码值）/ 2，这样做是因为左右两边走过的路程不一样，取平均值能将编码值累计值近似为车在走直线时的编码值累计值，设置固定电机目标值后还需要在控制函数中进行PID计算，当累计编码值大于某一个阈值时，认为车已经通过直角了，可以清空标志位，回归到正常状态。
//直角电机目标值设置
if(Straight_Angle_flag==1 && Slope_Flag==0) //检测到直角
{
if(Left_Adc>Right_Adc) //左直角
{
motor_control_L->set_encoder_speed = STRAIGHT_ANGLE_LEFT_L; //直角的速度---固定值
motor_control_R->set_encoder_speed = STRAIGHT_ANGLE_LEFT_R;
}
else if(Left_Adc<Right_Adc) //右直角
{
motor_control_L->set_encoder_speed = STRAIGHT_ANGLE_RIGHT_L;
motor_control_R->set_encoder_speed = STRAIGHT_ANGLE_RIGHT_R;
}
}
4、障碍物判断和处理
过障碍物大家都是使用tof测距或者超声波测距，当发现测距值小于某一阈值时，认为这是一个障碍物，做出相应的处理。但是，比赛过程中有很多时候会误判，将场地里其他的道具误认为是障碍物，如锥桶、坡道侧面、场地边缘等，好在障碍物一般会放在一个比较长的直道上，解决方式是加一些限定条件，我的处理方式是：当检测到障碍物距离小于600毫米，并且现在车在走直道，判断方式多样，可以是差比和小于一个很小的阈值，或者中间电感很小（取决于中间电感的放置方式，竖着放的话，直道为0），并且设置一个标志位用于检测是否通过了一次障碍，过了一次障碍之后，将这个标志位置1，表示再也不会再判断障碍物，只有这个标志为0才可以做出障碍物处理。下面给出判断处理代码：
/***** 3.障碍物处理 ******/
//距离小于阈值，并且在走直线
if(dl1b_distance_mm<AVOID_DIS_MIN && Middle_Adc>-5 && Middle_Adc<5) turn_avoid_flag = 1; //置障碍标志
if(turn_avoid_flag==1 || turn_avoid_flag==2)
{
Aviod_encoder +=(temp_left_pluse+temp_right_pluse)/2; //累计编码值
if(Aviod_encoder>= AVOID_TURN_QUIT && turn_avoid_flag==1) turn_avoid_flag=2; //编码值大于驶离障碍阈值转到状态2
else if(Aviod_encoder>AVIOD_TURN_BUCK && turn_avoid_flag==2) //状态2
{
Aviod_encoder = 0;
turn_avoid_flag = 0; //清空标志位
Avoid_Count+=1;
if(Avoid_Count>=1)
One_avoid_flag = 1; //只过一次障碍标志位置1，以后也不会再进行环岛处理
}
}
/***** 3.障碍物处理 *****/
刚刚只是给出了障碍的判断，并没有进行通过处理，我的处理方式是，有障碍物时是状态1，控制电机差速先往一个方向驶离障碍物（我使用右转驶离），并累计路程，路程到一定值时跳转到状态2，状态2下控制电机差速左转驶回赛道（左转驶回赛道），并在期间累计路程，路程到达一定值时，清空标志位，并置位只过一次障碍的标志位，这一步在置位标志位时已经进行。下面给出过障碍代码：
else if(turn_avoid_flag !=0 && (outtrack_flag ==0 || outtrack_flag ==1) && One_avoid_flag!=1) //过障碍物处理
{
if(turn_avoid_flag==1) //状态1，电机固定偏差驶离赛道
{
Round_PWM_L = PID_Calculate(&pid_motor_left,Left_motor.get_encoder_speed,AVOID_SPEED_QUIT_L);
Round_PWM_R = PID_Calculate(&pid_motor_right,Right_motor.get_encoder_speed,AVOID_SPEED_QUIT_R);
Motor_Left_Command(Round_PWM_L);
Motor_Right_Command(Round_PWM_R);
}
else if(turn_avoid_flag==2) //状态2，驶回赛道
{
Round_PWM_L = PID_Calculate(&pid_motor_left,Left_motor.get_encoder_speed,AVOID_SPEED_BACK_L);
Round_PWM_R = PID_Calculate(&pid_motor_right,Right_motor.get_encoder_speed,AVOID_SPEED_BACK_R);
Motor_Left_Command(Round_PWM_L);
Motor_Right_Command(Round_PWM_R);
}
}
5、坡道判断和处理
坡道也是和障碍物差不多，坡道也会摆在一个比较长的直道上，为了不和障碍物处理冲突，在比赛前应该将车放在一个先过障碍物，再过坡道的方向上发车，这样比较好处理，处理方式：如果车在直道上（中间电感或者差比和偏差判断），测距值小于某一阈值，或者说左右两边电感同时达到一个很大的值（因为在进入坡道时，因为电磁前瞻，电感和电磁线的距离会比其他元素下要小），那么可以认为这是一个坡道，坡道上，为了防止速度太小上不去坡道，我们需要适当提速并累计编码值，当编码值大于某一阈值时，清空标志位，这样就完成了坡道的通过。下面是代码：
/** 坡道处理 ***/
//一次坡道标志为1，距离小于阈值，或者两边电感很大，并且在直道上
if((One_avoid_flag==1 && dl1b_distance_mm<=SLOPE_DISTANCE ||(Left_Adc==100&&Right_Adc==100)) && \
(Middle_Adc>-5&&Middle_Adc<5))
Slope_Flag = 1; //坡道标志位
if(Slope_Flag==1)
{
Slope_Encoder+=(temp_left_pluse+temp_right_pluse)/2; //累计编码值
if(Slope_Encoder>STROUGH_SLOPE_RNCODER) Slope_Flag = 0; //编码值大于阈值，清空标志
}
/** 坡道处理 ***/
提速控制，给电机一个固定目标值，代码如下：
else if(Straight_Angle_flag==0 && Slope_Flag==1) //坡道处理
{
motor_control_L->set_encoder_speed = SLOPE_SPEED; //坡道提速
motor_control_R->set_encoder_speed = SLOPE_SPEED;
}
五、总结和效果展示
对于上面使用到的代码，我将所有的代码放在了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届电磁组，我参加了安徽赛区的比赛，现场差点没完赛，侥幸混了一个省二，没有拍到现场完赛视频，但是确实可以跑全元素，这里给出一个调试的视频（没有障碍和坡道）：
电磁视频
扫一扫
4437
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
