前言
Arduino程序简单，很多函数都封装好了，IDE设计也足够合理，非常适合新手入门Arduino，下面就用一个Arduino实现循迹小车。内容包括两路模拟量循迹小车，数字量循迹小车，PID控制。
一、选用的硬件
- 选用的开发板
选用探索者套件的控制主板以及扩展版
- 循迹模块
- 5路红外数字量循迹
- 灰度循迹
二、程序
电机控制代码
由于扩展版上已经搭载了相关的电机驱动电路，直接控制相关管脚就可以控制电机方向以及电机速度。主要是写入电机速度，可以比较方便地控制电机速度和电机方向
/*******************电机定义*****************************/
#define Motor_Left_Gruop 0X07 //左轮组
#define Motor_Right_Gruop 0X38 //右轮组
#define Motor_All (Motor_Right_Gruop|Motor_Left_Gruop) //全部电机
/*******************电机定义*****************************/
/*************************************************/
//函数名： 电机初始化
//作者： anonymous
//日期： 2020.11.12
//功能： 初始化电机为正转
//输入参数：void
//返回值： void
//其他：接线说明//+M接pin10，-M接pin9以此类推，+M接6，-M接5，如果这样接线不能保证小车往前，请将线反接，使得小车电机转向都往前，以免影响使用
/*************************************************/
void Motor_init()//输入的是电机号和电机模式
{
pinMode(10, OUTPUT );
pinMode(6, OUTPUT );
//写D5，D9PWM
digitalWrite(10, LOW);//10是左轮
digitalWrite(6, LOW);//6是右轮
}
/*************************************************/
//函数名：写入电机速度
//作者： anonymous
//日期： 2020.11.10
//功能： 写入电机速度，motor_speed>0，表示使电机正转，并写入速度（0<motor_speed<255）反之表示使电机反转，并写入速度
int motor控制写入电机号，调用开头的宏定义即可，例如write_motor_speed（Motor_Left_Gruop ，-100）。
//输入参数：int motor,int motor_speed
//返回值：int ，返回写入的电机组，便于调试,速度为正，返回正电机组数值，反之，返回负电机组数值
//其他：请配合电机初始化使用，效果更佳。
/*************************************************/
int write_motor_speed(int motor,int motor_speed)
{
//左轮电机：A3,D9,D10
//右轮电机：D11,D5,D6
int temp_speed=0;
if((motor==Motor_Left_Gruop))//左轮组
{
if(motor_speed>0)//判断正转,控制速度
{
temp_speed=motor_speed;
pinMode(10, OUTPUT );
//写D5，D9PWM
digitalWrite(10, LOW);//10是左轮
analogWrite(9,temp_speed);
return Motor_Left_Gruop;
}
else
{
temp_speed=-motor_speed;
pinMode(9, OUTPUT );
//写D5，D9PWM
digitalWrite(9, LOW);//10是左轮
analogWrite(10,temp_speed);
return -Motor_Left_Gruop;
}
}
if((motor==Motor_Right_Gruop))//右轮组
{
if(motor_speed>0)//判断正转,控制速度
{
temp_speed=motor_speed;
pinMode(6, OUTPUT );
//写D5，D9PWM
digitalWrite(6, LOW);//6是右轮
analogWrite(5,temp_speed);
return Motor_Right_Gruop;
}
else
{
temp_speed=-motor_speed;
pinMode(5, OUTPUT );
digitalWrite(5, LOW);//5是右轮
analogWrite(6,temp_speed);
return -Motor_Right_Gruop;
}
}
if((motor==Motor_All))
{
if(motor_speed>0)//判断正转,控制速度
{
temp_speed=motor_speed;
pinMode(10, OUTPUT );
pinMode(6, OUTPUT );
//写D5，D9PWM
digitalWrite(10, LOW);//10是左轮
digitalWrite(6, LOW);//6是右轮
analogWrite(5,temp_speed);
analogWrite(9,temp_speed);
return Motor_All;
}
else
{
temp_speed=-motor_speed;
pinMode(5, OUTPUT );
pinMode(9, OUTPUT );
digitalWrite(5, LOW);//5是右轮
digitalWrite(9, LOW);//9是左轮
analogWrite(6,temp_speed);
analogWrite(10,temp_speed);
return -Motor_All;
}
}
}
循迹方案
5路数字量循迹方案
程序，能够走直线和曲线，小伙伴可以根据不同场地元素进行相关元素地判定。数字量由于只有5路，两个红外反射小灯之间有一定地间距，这样就会造成一定的循迹盲区，检测不到小车比较微小的变化，因此小车直线稳定性不如模拟量循迹好，但也有一定的好处就是程序容易理解，调试简单。
5路数字量循迹原理
图解：循迹模块检测到黑线标黑
其他检测同理5路数字量循迹检测
//检测到黑线输出高，检测不到输出低
#define NOT_GETFLAGE 1
#define GETFLAGE 0
int sensor[5] = {NOT_GETFLAGE, NOT_GETFLAGE, NOT_GETFLAGE, NOT_GETFLAGE, NOT_GETFLAGE}; //5个传感器数值的数组
/*************************************************/
//函数名：read_sensro_init
//作者： anonymous
//日期： 2020.11.12
//功能： 数字量循迹初始化,初始化引脚
//输入参数：void
//返回值： void
/*************************************************/
void read_sensro_init()
{
pinMode(2, INPUT );
pinMode(3, INPUT );
pinMode(4, INPUT );
pinMode(7, INPUT );
pinMode(8, INPUT );
}
/*************************************************/
//函数名： read_sensor_values
//作者： anonymous
//日期： 2020.11.12
//功能： 读取偏差值
//输入参数：void
//返回值： 返回偏差值
/*************************************************/
int read_sensor_values()//读取偏差值
{
//从左到右的检测模块，依次标为0，1，2，3，4
//左偏设置值为正，右偏设置值为负
sensor[0] = digitalRead(2);
sensor[1] = digitalRead(3);
sensor[2] = digitalRead(4);
sensor[3] = digitalRead(7);
sensor[4] = digitalRead(8);
//赛道检测
//**********************************无偏差检测***************************************************//
//没有偏移情况,直线
if((sensor[0]==NOT_GETFLAGE)&&(sensor[1]==NOT_GETFLAGE)&&(sensor[2]==GETFLAGE)&&(sensor[3
]==NOT_GETFLAGE)&&(sensor[4])==NOT_GETFLAGE) Error0=0; //正好处于中间，没有偏离。
//特殊情况，不好判断有没有偏移，直接算没有偏移，先直走一段
if((sensor[0]==NOT_GETFLAGE)&&(sensor[1]==GETFLAGE)&&(sensor[2]==GETFLAGE)&&(sensor[3
]==GETFLAGE)&&(sensor[4])==NOT_GETFLAGE) Error0=0;
//**********************************左偏检测***************************************************//
if((sensor[0]==NOT_GETFLAGE)&&(sensor[1]==GETFLAGE)&&(sensor[2]==NOT_GETFLAGE)&&(sensor[3
]==NOT_GETFLAGE)&&(sensor[4])==NOT_GETFLAGE) Error0=1; //左偏移直线黑线，但方向是直线，算小左偏
if((sensor[0]==NOT_GETFLAGE)&&(sensor[1]==GETFLAGE)&&(sensor[2]==GETFLAGE)&&(sensor[3
]==NOT_GETFLAGE)&&(sensor[4])==NOT_GETFLAGE) Error0=2;//中左偏
if((sensor[0]==GETFLAGE)&&(sensor[1]==GETFLAGE)&&(sensor[2]==NOT_GETFLAGE)&&(sensor[3
]==NOT_GETFLAGE)&&(sensor[4])==NOT_GETFLAGE) Error0=3; //大左偏
if((sensor[0]==GETFLAGE)&&(sensor[1]==NOT_GETFLAGE)&&(sensor[2]==NOT_GETFLAGE)&&(sensor[3
]==NOT_GETFLAGE)&&(sensor[4])==NOT_GETFLAGE) Error0=4; //大大左偏
//**********************************右偏检测***************************************************//
if((sensor[0]==NOT_GETFLAGE)&&(sensor[1]==NOT_GETFLAGE)&&(sensor[2]==NOT_GETFLAGE)&&(sensor[3
]==GETFLAGE)&&(sensor[4])==NOT_GETFLAGE) Error0=-1; //右偏移直线黑线，但方向是直线，算小右偏
if((sensor[0]==NOT_GETFLAGE)&&(sensor[1]==NOT_GETFLAGE)&&(sensor[2]==GETFLAGE)&&(sensor[3
]==GETFLAGE)&&(sensor[4])==NOT_GETFLAGE) Error0=-2;//中右偏
if((sensor[0]==NOT_GETFLAGE)&&(sensor[1]==NOT_GETFLAGE)&&(sensor[2]==NOT_GETFLAGE)&&(sensor[3
]==GETFLAGE)&&(sensor[4])==GETFLAGE) Error0=-3; //大右偏
if((sensor[0]==NOT_GETFLAGE)&&(sensor[1]==NOT_GETFLAGE)&&(sensor[2]==NOT_GETFLAGE)&&(sensor[3
]==NOT_GETFLAGE)&&(sensor[4])==GETFLAGE) Error0=-4; //大大右偏
return Error0;
}
/*************************************************/
//函数名： gate_averge
//作者： anonymous
//日期： 2020.11.12
//功能： 均值平均滤波法,多次读取偏差值，减少误差
//输入参数：void
//返回值： 返回偏差值
/*************************************************/
int gate_averge()
{
//均值滤波
float temp=0,erroor[5],error_sum=0;
int i;
for(i=0;i<5;i++){
erroor[i] =read_sensor_values();//得到偏差值
}//得到偏差值
//累加偏差求平均值
for(i=0;i<5;i++) error_sum=error_sum+erroor[i];
error_sum=error_sum/i;
return error_sum;
Serial.println(error_sum,DEC);
}
2路模拟量循迹检测：一
检测原理：灰度传感器是模拟传感器，有一只发光二极管和一只光敏电阻，安装在同一面上。灰度传感器利用不同颜色的检测面对光的反射程度不同，光敏电阻对不同检测面返回的光其阻值也不同的原理进行颜色深浅检测。在有效的检测距离内，发光二极管发出白光，照射在检测面上，检测面反射部分光线，光敏电阻检测此光线的强度并将其转换为机器人可以识别的信号。
利用Arduino的ADC将灰度传感器检测的电压映射为0~1023。由于在黑线和底面的颜色不一样，检测到黑线边缘的值也不一样，因此可以稍微检测到比较微小的偏差。具体偏差值设定还是要看场地来测量，因此这种方法受场地影响比较大，受光照影响也比较大，因此适用性不强。
2路灰度循迹检测
/*************************************************/
//函数名： read_sensor_values
//作者： anonymous
//日期： 2020.11.12
//功能： 检测灰度值，手动设定偏差值
//输入参数：void
//返回值： 返回偏差值
/*************************************************/
int read_sensor_values()
{
int val[2]={0.0};
int i = 0;
//均值滤波
for (i = 0; i < 4; i++) {
val[0] += analogRead(A0);
val[1]+= analogRead(A1);
}
val[0]=val[0]/4;
val[1]=val[1]/4;
if (flag == 1) {//前进flag设为1
if (val[0]< 800 && val[1] < 600)//在黑线
error = 0;
else if (val[0] < 800 && val[1] > 600)//偏右
error = 3;
else if (val[1]> 800&& val[1] < 600)//偏左
error = 3;
else
error = 0;
}
return error ;
}
2路模拟量循迹方案：二
检测原理和偏差检测和2路模拟量循迹方案一类似，在此就不赘述了。方案二和方案一的区别是，这里是通过偏差值，要是偏差值符合在设定黑线偏差值，则设置为无偏差，要是在黑线左或者右，偏差值会改变。**通过判断偏差值大小和正负就可以知道小车微小的偏差。**由于这里不需要测定场地的反馈的模拟值，能够适应不同场地，受光照影响比较小，因此比方案一要好用一点。要是场地比较脏，检测到的值误差会很大，摆放位置对读取的值影响也很大，一点点移动就会有很大的变化。
/*************************************************/
//函数名： read_sensor_values
//作者： anonymous
//日期： 2020.11.12
//功能： 检测灰度值
//输入参数：void
//返回值： 返回偏差值
/*************************************************/
//检测到不同元素输出值不一样。
#define STRAIGHT_LINE_RANGE 40
#define YAKELI_LINE_RANGE 50
#define BLACK_LINE_RANGE 200
int sensor[2] = {0,0}; //2个传感器数值的数组
int read_sensor_values()//读取偏差值
{
int adval=0;//临时两边AD偏差值
//从左到右的检测模块，依次标为0，1
sensor[0] = analogRead(A0);
sensor[1] = analogRead(A4);
adval= sensor[1]-sensor[0];
//依旧左偏取值为正，右偏取值为负
//赛道检测 ,黑色线值比较小，其他线的值比较大
//**********************************无偏差检测***************************************************//
if(adval<=STRAIGHT_LINE_RANGE) adval=0;
if((sensor[0]<=BLACK_LINE_RANGE+50&&sensor[0]>BLACK_LINE_RANGE+50)&&(sensor[1]<=BLACK_LINE_RANGE+50&&sensor[1]>BLACK_LINE_RANGE+50)) decide0=LINE;
return adval;
}
/*************************************************/
//函数名： gate_averge
//作者： anonymous
//日期： 2020.11.12
//功能： 中值滤波+排序滤波
//输入参数：void
//返回值： 返回偏差值
/*************************************************/
//中值滤波+排序滤波是必要的,由于我买的硬件读取的值不够稳定，会有时忽然偏差很大或者偏差很小。
double gate_averge()
{
//均值滤波
int ad_averge=0,ad_value_temp[9],ad_sum=0;
int i,j,t;
for(i=0;i<9;i++){
ad_value_temp[i] =read_sensor_values();//得到偏差值
}//得到偏差值
//冒泡排序
for(i=0;i<9-1;++i)//n个数,总共需要进行n-1次
{ //n-1个数排完,第一个数一定已经归位
//每次会将最大(升序)或最小(降序)放到最后面
for(j=0;j<9-i-1;++j)
{
if(ad_value_temp[j]>ad_value_temp[j+1])//每次冒泡,进行交换
{
t=ad_value_temp[j];
ad_value_temp[j]=ad_value_temp[j+1];
ad_value_temp[j+1]=t;
}
}
}
for(i=1;i<8;i++) ad_sum=ad_sum+ad_value_temp[i];
ad_averge=ad_sum/(i-1);
return ad_averge;
}
处理获取值
基本思路是通过两个轮子的差速进行方向调控，通过获取到的偏差直接对电机进行输出。这里我用到了PID算法来对偏差的值进行处理，从而使小车稳定在黑线。
PID算法
/*******************PID定义*****************************/
typedef struct
{
volatile float Proportion; // 比例常数 Proportional Const
volatile float Integral; // 积分常数 Integral Const
volatile float Derivative; // 微分常数 Derivative Const
volatile int Error1; // Error[n-1]
volatile int Error2; // Error[n-2]
volatile int iError; // Error[n]
volatile int Error_sum;
} PID;
PID pid_increase;//增量式,位置式初始化
PID* sptr_increase=&pid_increase;//PID初始化
#define SET_POINT 0
/*******************PID定义*****************************/
/*************************************************/
//函数名： PID_Postion
//作者： anonymous
//日期： 2020.11.12
//功能： 位置式PID控制器
//输入参数：void
//返回值： 返回计算值
/*************************************************/
float PID_Postion (float SetPoint,float CurrentPoint, PID* sptr)//速度PID
{
float iIncpid=0;
sptr->iError=SetPoint-CurrentPoint; // 计算当前误差
sptr->Error_sum+=sptr->iError;
iIncpid=sptr->Proportion * sptr->iError // P
+sptr->Integral * sptr->Error_sum // I
+sptr->Derivative * (sptr->iError-sptr->Error1); // D
sptr->Error1=sptr->iError; // 存储误差，用于下次计算
iIncpid=PID_OutputLimit(iIncpid);//在其他地方进行了限幅处理，此处就不用了
return(iIncpid); // 返回计算值
}
/*************************************************/
//函数名： PID_Init
//作者： anonymous
//日期： 2020.11.12
//功能： PID初始化
//输入参数：void
//返回值： void
/*************************************************/
void PID_Init(PID *sptr)
{
sptr->Derivative=0;//Kd，加快反应速度，更容易超调，曲线更稳
sptr->Proportion=0;//Kp,KP,比例系数,30有点太大
sptr->Integral=0;//Ki，消除静差，直线更稳，
sptr->Error2=0;//第二次误差
sptr->Error1=0;
sptr->iError=0;
sptr->Error_sum=0;//第一次误差
}
/*************************************************/
//函数名： PID_OutputLimit
//作者： anonymous
//日期： 2020.11.12
//功能： pid输出限幅
//输入参数：void
//返回值： output
/*************************************************/
float PID_OutputLimit(double output)
{
if ((int)output < -500)
{
output = -500;
}
else if ((int)output > 500)
{
output = 500;
}
return output;
}
控制速度算法
/*************************************************/
//函数名： direction_control
//作者： anonymous
//日期： 2020.11.12
//功能： 差速方向控制函数
//输入参数：void
//返回值： void
/*************************************************/
void direction_control()
{
float right_motor_speed=0;
float left_motor_speed=0;
int SPEED_AVERAGER=100;巡线速度，速度过快不好控制
double PID_Dvalue,PID_Error;
PID_Error=gate_averge();
PID_Error=(PID_Error-0)/(1023-0);//归一化的公式如下：(x-Min)/（Max-Min）
PID_Error=PID_Error*500;//分辨率减少,以免过于灵敏
PID_Dvalue=PID_Postion(SET_POINT,PID_Error,sptr_increase);//PID返回值有正，有负
left_motor_speed = SPEED_AVERAGER - PID_Dvalue;//设定速度+PID返回偏差值
right_motor_speed = SPEED_AVERAGER + PID_Dvalue;
//PWM限幅处理
if(left_motor_speed<-SPEED_MAX) left_motor_speed = -SPEED_MAX;
if(left_motor_speed > SPEED_MAX) left_motor_speed = SPEED_MAX;
//右轮组限幅处理
if(right_motor_speed<-SPEED_MAX) right_motor_speed =- SPEED_MAX;
if(right_motor_speed > SPEED_MAX) right_motor_speed = SPEED_MAX;
//左轮组速度设置
write_motor_speed(Motor_Left_Gruop,left_motor_speed);
//右轮组速度设置
write_motor_speed(Motor_Right_Gruop,right_motor_speed);
}
中断函数处理数据
由于PID控制算法受周期影响，故需要使用定时器处理函数。不同周期，PID控制器返回的结果是不一样的，这一点需要注意。这里我们使用。
MsTimer2库函数。
MsTimer2::set(20,time_interval); //中断库
MsTimer2::start( );
中断函数处理程序
void time_interval()//定时器中断函数
{
direction_control();//循迹函数
}
总结
程序控制图解
评论（0）
您还未登录，请登录后发表或查看评论