循迹小车项目
前言
这篇很久之前写的文章，有挺多问题的。还有这么多人喜欢，两年后，也就是2022年我决定重新修改文章并补充一些内容，并提供C++的参考代码，不会C++的可以看之前的C代码，不过写的有点烂就是了，而且还有点语法错误，实在是不太容易读懂。
Arduino程序简单，很多函数都封装好了，IDE设计也足够合理，非常适合新手入门Arduino，下面就用一个Arduino实现循迹小车。内容包括两路模拟量循迹小车，数字量循迹小车，PID控制，电机控制。循迹小车是新手的一个比较简单的项目，把文章的内容拿下，那简单的控制项目就拿下啦！
如果想要用其他芯片，比如STM32,C51等等，都可以参考，学习思路就可以啦！
一、选用的硬件
其实Arduino的很多程序都是可以通用的，选择一款合适的控制主板就行。之后根据相关方案，连接好相关传感器和驱动就OK。
-
我选用的开发板
arduino uno
-
循迹模块
-
5路红外数字量循迹
-
灰度循迹两个
-
-
L298N一个
- TT马达2个
二、相关硬件放置
黑线是图上虚线，一般传感器的相对位置这样放比较简单，也比较合适。图中的圆蓝色形是传感器位置，其他的有四个圆的就是车子啦！图画的比较抽象，但也勉强能看懂（つ﹏⊂）
三、额外知识补充
循迹模块使用
使用仅仅是了解它的简单原理以及Arduino如何简单的初始化和使用
5路循迹模块
原理：根据不太的材质吸收光的强弱不同，根据反射的光的强度，以此输出数字量。比如黑色，吸光强，输出数字量1，白色吸光弱，输出数字量0。
简单使用：
//数字量引脚初始化
void SensorPinInit()
{
pinMode(2, INPUT );
pinMode(3, INPUT );
pinMode(4, INPUT );
pinMode(7, INPUT );
pinMode(8, INPUT );
}
/*读取某一引脚的值*/
int SensorPinRead(uint8_t pin)
{
return digitalRead(pin);
}
灰度传感器模块
原理：根据白色光源（发光二极管）照射到不同颜色的物品，光敏电阻感受到的光强不一样，输出不同的模拟量。
简单使用：
//模拟量引脚初始化，ADC
void SensorPinInit()
{
;
}
/*读取某一引脚的值*/
int SensorPinRead(uint8_t pin)
{
return analogRead(pin);
}
PID控制器
PID控制器比较复杂一点点，想要深入的理解，自己找相关文章啦。PID控制器是可以自己设定控制强度的，这就是调参，调参比较麻烦，也请自己去了解啦。文章现在内容太多了，不能承载更多知识，也不利于学习。
简单理解：PID控制器，作用是控制器。能过够根据不同的输入，给出相应的控制值。比如说，我们要控制电机。传感器获取到的一个偏离程度P，P为1，输入到PID控制器，PID控制器输出的值是98，这个98就是需要输出到电机的值。这样电机就能根据PID控制器给出不同的值，做出不同的反应。我们也能够使用PID控制器去控制循迹，根据传感器获取到的值不同，从而传入PID控制器的值也不同，最终电机输出的值跟随变化。达到效果，稳定循迹。
C语言版本：
/*******************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;
}
C++使用例子
class pidctr /*PID控制器对象*/
{
public:
pidctr()
{
position.Derivative=0.1;//Kd，加快反应速度，更容易超调，曲线更稳
position.Proportion=1.0;//Kp,KP,比例系数,30有点太大
position.Integral=0.11;//Ki，消除静差，直线更稳，
position.Error2=0;//第二次误差
position.Error1=0;
position.iError=0;
position.Error_sum=0;//第一次误差
MatOut = 65535;
Setpoint =0;
}
~pidctr()
{
}
void PidInit(int setpoint,int maxout)
{
MatOut=maxout;
Setpoint=setpoint;
}
/*设置PID控制器的值*/
void SetPid(float p,float i,float d)
{
position.Derivative=d;
position.Proportion=p;
position.Integral=i;
}
float PID_Postion (int CurrentPoint)
{
float iIncpid=0;
position.iError=Setpoint-CurrentPoint; // 计算当前误差
position.Error_sum+=position.iError;
iIncpid=position.Proportion * position.iError // P
+position.Integral * position.Error_sum // I
+position.Derivative * (position.iError-position.Error1); // D
position.Error1=position.iError; // 存储误差，用于下次计算
/*限幅处理*/
if(abs(iIncpid)>MatOut)
{
iIncpid=iIncpid>0?MatOut:-MatOut;
}
return iIncpid; // 返回计算值
}
private:
/*******************PID定义*****************************/
typedef struct
{
float Proportion; // 比例常数 Proportional Const
float Integral; // 积分常数 Integral Const
float Derivative; // 微分常数 Derivative Const
int Error1; // Error[n-1]
int Error2; // Error[n-2]
int iError; // Error[n]
int Error_sum;
}PID;
PID position;//位置式PID
int MatOut; /*设定输出最大值*/
int Setpoint;/*PID控制器设定值*/
};
/*使用例子*/
void Simple()
{
int result=1;
/*控制器部分*/
pidctr mctr;
mctr.PidInit(0,100);/*初始化*/
result=mctr.PID_Postion(result);
}
L298N模块简单理解
L298N是一个驱动电机的模块，我们需要这个模块来控制电机。没有它，TT马达就不能够被控制。我们能够从arduino输入模拟量和数字量到这个驱动模块，通过它我们就能够间接控制，电机的转向，停止和速度了。具体怎么使用，自己搜索啦，也不复杂。
四、循迹方案
5路数字量循迹方案
最简单的循迹方案，就是检测精度低。具体怎么理解，看图就行啦，不难理解。图中蓝色为传感器，检测到黑线就会为黑，相关传感器就会输出对应值。文章没有把全部情况列举出来，只是列举了一些情况，但也能够理解就行。
5路数字量循迹方案
图解：循迹模块检测到黑线标黑
其他检测同理
5路数字量循迹检测代码
C语言版本是2年前写的，代码可读性不怎么好，但思路是没有变化的，建议看C++的！
C语言版本
//检测到黑线输出高，检测不到输出低
#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;
}
/*使用例子*/
void Simple()
{
int result;
read_sensro_init();
result=gate_averge();
}
C++版本
//作者： Silent Knight
//日期： 2022.7.4
//检测到黑线输出高，检测不到输出低
#define NOT_GETFLAGE 1
#define GETFLAGE 0
#define MAX_SENSOR 5 //最大传感器数
/*定义循迹的对象，目前仅仅支持5个传感器*/
class Track
{
public:
Track()
{
int i;
for(i=0;i<MAX_SENSOR;i++)
{
sensor[i] = NOT_GETFLAGE;//初始化传感器数据
use_pin[i] = 0;/*初始化使用到的引脚*/
}
number_sensor=0;
}
~Track()
{
;
}
/*选择使用引脚，请按照从左到右的顺序初始化*/
/*目前仅支持5个pin*/
void SensorPinInit(uint8_t pin, uint8_t mode)
{
use_pin[number_sensor++]=pin;/*记录每一个使用到的引脚*/
pinMode(pin,mode);
}
/*读取位置偏离程度*/
/*返回值左偏设置值为正，右偏设置值为负*/
int SensorPoint(int logic)//读取偏差值无滤波
{
int i,point=0;
for(i=0;i<number_sensor;i++)
{
sensor[i]=SensorPinRead(use_pin[i]);/*从左到右读取引脚数据*/
}
//赛道检测
//**********************************无偏差检测***************************************************//
//没有偏移情况,直线
if((sensor[0]==NOT_GETFLAGE)&&(sensor[1]==NOT_GETFLAGE)&&(sensor[2]==GETFLAGE)&&(sensor[3
]==NOT_GETFLAGE)&&(sensor[4])==NOT_GETFLAGE) point=0; //正好处于中间，没有偏离。
//特殊情况，不好判断有没有偏移，直接算没有偏移，先直走一段
if((sensor[0]==NOT_GETFLAGE)&&(sensor[1]==GETFLAGE)&&(sensor[2]==GETFLAGE)&&(sensor[3
]==GETFLAGE)&&(sensor[4])==NOT_GETFLAGE) point=0;
//**********************************左偏检测***************************************************//
if((sensor[0]==NOT_GETFLAGE)&&(sensor[1]==GETFLAGE)&&(sensor[2]==NOT_GETFLAGE)&&(sensor[3
]==NOT_GETFLAGE)&&(sensor[4])==NOT_GETFLAGE) point=1; //左偏移直线黑线，但方向是直线，算小左偏
if((sensor[0]==NOT_GETFLAGE)&&(sensor[1]==GETFLAGE)&&(sensor[2]==GETFLAGE)&&(sensor[3
]==NOT_GETFLAGE)&&(sensor[4])==NOT_GETFLAGE) point=2;//中左偏
if((sensor[0]==GETFLAGE)&&(sensor[1]==GETFLAGE)&&(sensor[2]==NOT_GETFLAGE)&&(sensor[3
]==NOT_GETFLAGE)&&(sensor[4])==NOT_GETFLAGE) point=3; //大左偏
if((sensor[0]==GETFLAGE)&&(sensor[1]==NOT_GETFLAGE)&&(sensor[2]==NOT_GETFLAGE)&&(sensor[3
]==NOT_GETFLAGE)&&(sensor[4])==NOT_GETFLAGE) point=4; //大大左偏
//**********************************右偏检测***************************************************//
if((sensor[0]==NOT_GETFLAGE)&&(sensor[1]==NOT_GETFLAGE)&&(sensor[2]==NOT_GETFLAGE)&&(sensor[3
]==GETFLAGE)&&(sensor[4])==NOT_GETFLAGE) point=-1; //右偏移直线黑线，但方向是直线，算小右偏
if((sensor[0]==NOT_GETFLAGE)&&(sensor[1]==NOT_GETFLAGE)&&(sensor[2]==GETFLAGE)&&(sensor[3
]==GETFLAGE)&&(sensor[4])==NOT_GETFLAGE) point=-2;//中右偏
if((sensor[0]==NOT_GETFLAGE)&&(sensor[1]==NOT_GETFLAGE)&&(sensor[2]==NOT_GETFLAGE)&&(sensor[3
]==GETFLAGE)&&(sensor[4])==GETFLAGE) point=-3; //大右偏
if((sensor[0]==NOT_GETFLAGE)&&(sensor[1]==NOT_GETFLAGE)&&(sensor[2]==NOT_GETFLAGE)&&(sensor[3
]==NOT_GETFLAGE)&&(sensor[4])==GETFLAGE) point=-4; //大大右偏
/*判断设定的逻辑：左偏设置值为正，右偏设置值为负*/
point=logic==1?point:-point;
return point;/*返回设定的偏离程度*/
}
/*使用均值滤波，传入滤波值*/
int SensorPointfilter(int times)
{
int i,sum;
for(i=0,sum=0;i<times;i++)
{
sum+=SensorPoint();
}
return sum/i;
}
private:
/*读取某一引脚的值*/
int SensorPinRead(uint8_t pin)
{
return digitalRead(pin);
}
uint8_t sensor[MAX_SENSOR]; //5个传感器数值的数组
uint8_t number_sensor;
uint8_t use_pin[MAX_SENSOR];
};
/*使用例子*/
void Simple()
{
int result;
Track me;
/*引脚初始化*/
me.SensorPinInit(2, INPUT);
me.SensorPinInit(3, INPUT);
me.SensorPinInit(4, INPUT);
me.SensorPinInit(7, INPUT);
me.SensorPinInit(8, INPUT);
result=me.SensorPointfilter(10);
}
2路模拟量循迹方案
检测原理：灰度传感器是模拟传感器，有一只发光二极管和一只光敏电阻，安装在同一面上。灰度传感器利用不同颜色的检测面对光的反射程度不同，光敏电阻对不同检测面返回的光其阻值也不同的原理进行颜色深浅检测。在有效的检测距离内，发光二极管发出白光，照射在检测面上，检测面反射部分光线，光敏电阻检测此光线的强度并将其转换为机器人可以识别的信号。
基本思路：
通过检测两个光敏传感器的差值，就可以知道车子的偏离黑线的程度，检测方案类似5路传感器。例如，直线检测，两个传感器都没有检测到黑线的值，那么两个传感器获取到的模拟量应该是类似的，也就是差值很小就判断为直线。反之，判断为左偏离黑线或者右偏离黑线。
C语言版本是2年前写的，代码可读性不怎么好，但思路是没有变化的，建议看C++的！
C语言版本
/*************************************************/
//函数名： read_sensor_values
//作者： anonymous
//日期： 2020.11.12
//功能： 检测灰度值
//输入参数：void
//返回值： 返回偏差值
/*************************************************/
/*合理偏差范围值*/
#define STRAIGHT_LINE_RANGE 40
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(abs(adval)<=STRAIGHT_LINE_RANGE) adval=0;
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;
}
C++版本
/*定义循迹的对象，目前仅仅支持2个传感器*/
class Track
{
public:
Track()
{
int i;
for(i=0;i<MAX_SENSOR;i++)
{
sensor[i] = 0;//初始化传感器数据
use_pin[i] = 0;/*初始化使用到的引脚*/
}
number_sensor=0;
}
~Track()
{
;
}
/*选择使用引脚，请按照从左到右的顺序初始化*/
/*目前仅支持5个pin*/
void SensorPinInit(uint8_t pin)
{
use_pin[number_sensor++]=pin;/*记录每一个使用到的引脚*/
}
/*使用均值滤波，传入滤波值*/
int SensorPointfilter(int times)
{
int i,sum;
for(i=0,sum=0;i<times;i++)
{
sum+=SensorPoint(1);
}
return sum/i;
}
private:
/*读取某一引脚的值*/
int SensorPinRead(uint8_t pin)
{
return analogRead(pin);
}
/*读取位置偏离程度*/
/*左偏设置值为正，右偏设置值为负*/
int SensorPoint(int logic)//读取偏差值无滤波
{
int i,point=0;
for(i=0;i<number_sensor;i++)
{
sensor[i]=SensorPinRead(use_pin[i]);/*从左到右读取引脚数据*/
}
//偏离程度计算
point= sensor[1]-sensor[0];/*最简单算法/
//point=sensor[1]-sensor[0]/sensor[1]+sensor[0];/*基本差比和算法*/
if(abs(point)<=NOBIAS) point=0;
/*判断设定的逻辑：左偏设置值为正，右偏设置值为负*/
point=logic==1?point:-point;
return point;/*返回设定的偏离程度*/
}
int sensor[MAX_SENSOR]; //5个传感器数值的数组
uint8_t number_sensor;
uint8_t use_pin[MAX_SENSOR];
};
/*使用例子*/
void Simple()
{
int result;
Track me;
/*引脚初始化*/
me.SensorPinInit(A0);
me.SensorPinInit(A1);
result=me.SensorPointfilter(10);
/*控制器部分*/
pidctr mctr;
mctr.PidInit(0,100);
result=mctr.PID_Postion(result);
}
五、电机驱动L298N控制参考代码
L298N真值表
这里C语言写的其实不是很好，可以简单看看C++我是怎么写代码的，虽然也不是很好，但也应该会有点帮助！
/*******************电机定义*****************************/
#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
/*************************************************/
void Motor_init()
{
pinMode(10, OUTPUT );
pinMode(6, OUTPUT );
digitalWrite(10, LOW);//10是左轮
digitalWrite(6, LOW);//6是右轮
}
/*************************************************/
//函数名：写入电机速度
//作者： anonymous
//日期： 2020.11.10
//功能： 写入电机速度，motor_speed>0，表示使电机正转，并写入速度（0
//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);
digitalWrite(6, LOW);
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);
digitalWrite(9, LOW);
analogWrite(6,temp_speed);
analogWrite(10,temp_speed);
return -Motor_All;
}
}
}
C++版本：
/*电机控制引脚*/
#define MOTOR1_IN1 1
#define MOTOR1_IN2 2
#define MOTOR2_IN1 3
#define MOTOR2_IN2 4
#define MOTOR1_SP 5
#define MOTOR2_SP 6
/*电机控制方式*/
#define MOTOR_FORWARD 1
#define MOTOR_BACK 2
#define MOTOR_STOP 3
/*电机编号*/
#define MOTOR1 1
#define MOTOR2 2
class ttmotor
{
public:
ttmotor()
{
int i;
for(i=0;i<4;i++) motorctrpin[i]=0;
motornumber[0]=0;
motornumber[1]=0;
};
~ttmotor(){};
/*参数1传入设置控制电机的引脚，从左到右以此是通道1的IN1,IN2，通道2的IN1,IN2*/
/*参数2传入设置控制电机的速度，通道1的控制脚，通道2的控制脚*/
void TtMotorSetCtr(uint8_t pin[],uint8_t motor[])
{
int i;
for(i=0;i<4;i++)
{
motorctrpin[i]=pin[i];
pinMode(pin[i],OUTPUT);
}
motornumber[0]=motor[0];
motornumber[1]=motor[1];
/*初始化电机方式,默认为正向*/
TtMotorModeSet(MOTOR1,MOTOR_FORWARD);
TtMotorModeSet(MOTOR2,MOTOR_FORWARD);
}
/*电机速度控制*/
void TtMotorModeSpeedSet(int motornum,int speed)
{
int setspeed;
if(speed>0)
{
setspeed=speed>1023?1023:speed;
TtMotorModeSet(motornum,MOTOR_FORWARD);
if(motornum==1) analogWrite(motornumber[0],setspeed);
else analogWrite(motornumber[1],setspeed);
}
else
{
setspeed=speed<-1023?-1023:speed;
TtMotorModeSet(motornum,MOTOR_BACK);
if(motornum==1) analogWrite(motornumber[0],setspeed);
else analogWrite(motornumber[1],setspeed);
}
}
private:
/*电机模式设置,motornum1是L298N的通道1控制电机*/
void TtMotorModeSet(int mode,int motornum)
{
switch (mode)
{
case MOTOR_FORWARD:
{
if(motornum==MOTOR1)
{
digitalWrite(motorctrpin[0],LOW);
digitalWrite(motorctrpin[1],HIGHT);
}
else
{
digitalWrite(motorctrpin[2],LOW);
digitalWrite(motorctrpin[3],HIGHT);
}
break;
}
case MOTOR_BACK:
{
if(motornum==MOTOR1)
{
digitalWrite(motorctrpin[0],HIGHT);
digitalWrite(motorctrpin[1],LOW);
}
else
{
digitalWrite(motorctrpin[2],HIGHT);
digitalWrite(motorctrpin[3],LOW);
}
break;
}
case MOTOR_STOP:
{
if(motornum==MOTOR1)
{
digitalWrite(motorctrpin[0],HIGHT);
digitalWrite(motorctrpin[1],HIGHT);
}
else
{
digitalWrite(motorctrpin[2],HIGHT);
digitalWrite(motorctrpin[3],HIGHT);
}
break;
}
default :break;
}
}
uint8_t motorctrpin[4];/*电机控制方向引脚*/
uint8_t motornumber[2];/*电机控制速度引脚*/
};
/*简单使用*/
void Simple()
{
ttmotor mymotor;
uint8_t ctrpin[4],speedpin[2];
ctrpin[0]=MOTOR1_IN1;
ctrpin[1]=MOTOR1_IN2;
ctrpin[2]=MOTOR2_IN1;
ctrpin[3]=MOTOR2_IN2;
speedpin[0]=MOTOR1_SP;
speedpin[1]=MOTOR2_SP;
mymotor.TtMotorSetCtr(ctrpin,speedpin);
mymotor.TtMotorModeSpeedSet(MOTOR1,100);
}
六、控制示例
控制示例，这里仅使用最少的硬件，通过控制速度，使用差速的方法在控制方向的同时控制前进。
控制示例
C语言不完整代码
//C语言版本，不提供完整代码，自己上面初始化都有，我就懒得提供啦！仅提供伪代码
void setup()
{
/*这里初始化各个使用到的东西*/
/*PID控制器初始化*/
/*L298N控制电机模块初始化*/
/*传感器初始化*/
}
void loop()
{
CtlCar();
delay(100);
}
/*************************************************/
//函数名： CtlCar
//作者： anonymous
//日期： 2020.11.12
//功能： 差速方向控制函数
//输入参数：void
//返回值： void
/*************************************************/
void CtlCar()
{
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);
}
C++完整代码
C++版本：
ttmotor mymotor;/*创建电机对象*/
pidctr myctl;/*创建PID对象*/
Track mytrack;/*创建传感器循迹对象*/
void setup()
{
/*这里初始化各个使用到的东西*/
/*PID控制器初始化*/
myctl.PidInit(0,1023);
/*L298N控制电机模块初始化*/
uint8_t ctrpin[4],speedpin[2];
ctrpin[0]=MOTOR1_IN1;
ctrpin[1]=MOTOR1_IN2;
ctrpin[2]=MOTOR2_IN1;
ctrpin[3]=MOTOR2_IN2;
speedpin[0]=MOTOR1_SP;
speedpin[1]=MOTOR2_SP;
mymotor.TtMotorSetCtr(ctrpin,speedpin);
mymotor.TtMotorModeSpeedSet(MOTOR1,100);
/*传感器初始化*/
mytrack.SensorPinInit(1);
mytrack.SensorPinInit(2);
mytrack.SensorPinInit(3);
mytrack.SensorPinInit(4);
mytrack.SensorPinInit(5);
}
void loop()
{
CtlCar(100);
delay(100);
}
/*参数设定巡线速度*/
/*控制循迹*/
void CtlCar(int runspeed)
{
int SenSorResult,PidOut;
SenSorResult=mytrack.SensorPointfilter(10);
PidOut=myctl.PID_Postion(SenSorResult);
mymotor.TtMotorModeSpeedSet(MOTOR1,PidOut+runspeed);/*差速控制*/
mymotor.TtMotorModeSpeedSet(MOTOR2,PidOut-runspeed);/*差速控制*/
}
//作者： Silent Knight
//日期： 2022.7.4
//检测到黑线输出高，检测不到输出低
#define NOBIAS 40 /*无偏差范围*/
#define MAX_SENSOR 2 //最大传感器数
/*定义循迹的对象，目前仅仅支持2个传感器*/
class Track
{
public:
Track()
{
int i;
for(i=0;i<MAX_SENSOR;i++)
{
sensor[i] = 0;//初始化传感器数据
use_pin[i] = 0;/*初始化使用到的引脚*/
}
number_sensor=0;
}
~Track()
{
;
}
/*选择使用引脚，请按照从左到右的顺序初始化*/
/*目前仅支持5个pin*/
void SensorPinInit(uint8_t pin)
{
use_pin[number_sensor++]=pin;/*记录每一个使用到的引脚*/
}
/*使用均值滤波，传入滤波值*/
int SensorPointfilter(int times)
{
int i,sum;
for(i=0,sum=0;i<times;i++)
{
sum+=SensorPoint(1);
}
return sum/i;
}
private:
/*读取某一引脚的值*/
int SensorPinRead(uint8_t pin)
{
return analogRead(pin);
}
/*读取位置偏离程度*/
/*左偏设置值为正，右偏设置值为负*/
int SensorPoint(int logic)//读取偏差值无滤波
{
int i,point=0;
for(i=0;i<number_sensor;i++)
{
sensor[i]=SensorPinRead(use_pin[i]);/*从左到右读取引脚数据*/
}
//偏离程度计算
point= sensor[1]-sensor[0];/*最简单算法/
//point=sensor[1]-sensor[0]/sensor[1]+sensor[0];/*基本差比和算法*/
if(abs(point)<=NOBIAS) point=0;
/*判断设定的逻辑：左偏设置值为正，右偏设置值为负*/
point=logic==1?point:-point;
return point;/*返回设定的偏离程度*/
}
int sensor[MAX_SENSOR]; //5个传感器数值的数组
uint8_t number_sensor;
uint8_t use_pin[MAX_SENSOR];
};
class pidctr
{
public:
pidctr()
{
position.Derivative=1.0;//Kd，加快反应速度，更容易超调，曲线更稳
position.Proportion=0.1;//Kp,KP,比例系数,30有点太大
position.Integral=0.11;//Ki，消除静差，直线更稳，
position.Error2=0;//第二次误差
position.Error1=0;
position.iError=0;
position.Error_sum=0;//第一次误差
MatOut = 65535;
Setpoint =0;
}
~pidctr()
{
}
void PidInit(int setpoint,int maxout)
{
MatOut=maxout;
Setpoint=setpoint;
}
/*设置PID控制器的值*/
void SetPid(float p,float i,float d)
{
position.Derivative=d;
position.Proportion=p;
position.Integral=i;
}
float PID_Postion (int CurrentPoint)
{
float iIncpid=0;
position.iError=Setpoint-CurrentPoint; // 计算当前误差
position.Error_sum+=position.iError;
iIncpid=position.Proportion * position.iError // P
+position.Integral * position.Error_sum // I
+position.Derivative * (position.iError-position.Error1); // D
position.Error1=position.iError; // 存储误差，用于下次计算
/*限幅处理*/
if(abs(iIncpid)>MatOut)
{
iIncpid=iIncpid>0?MatOut:-MatOut;
}
return iIncpid; // 返回计算值
}
private:
/*******************PID定义*****************************/
typedef struct
{
float Proportion; // 比例常数 Proportional Const
float Integral; // 积分常数 Integral Const
float Derivative; // 微分常数 Derivative Const
int Error1; // Error[n-1]
int Error2; // Error[n-2]
int iError; // Error[n]
int Error_sum;
}PID;
PID position;//位置式PID
int MatOut; /*设定输出最大值*/
int Setpoint;/*PID控制器设定值*/
};
/*电机控制引脚*/
#define MOTOR1_IN1 1
#define MOTOR1_IN2 2
#define MOTOR2_IN1 3
#define MOTOR2_IN2 4
#define MOTOR1_SP 5
#define MOTOR2_SP 6
/*电机控制方式*/
#define MOTOR_FORWARD 1
#define MOTOR_BACK 2
#define MOTOR_STOP 3
/*电机编号*/
#define MOTOR1 1
#define MOTOR2 2
class ttmotor
{
public:
ttmotor()
{
int i;
for(i=0;i<4;i++) motorctrpin[i]=0;
motornumber[0]=0;
motornumber[1]=0;
};
~ttmotor(){};
/*参数1传入设置控制电机的引脚，从左到右以此是通道1的IN1,IN2，通道2的IN1,IN2*/
/*参数2传入设置控制电机的速度，通道1的控制脚，通道2的控制脚*/
void TtMotorSetCtr(uint8_t pin[],uint8_t motor[])
{
int i;
for(i=0;i<4;i++)
{
motorctrpin[i]=pin[i];
pinMode(pin[i],OUTPUT);
}
motornumber[0]=motor[0];
motornumber[1]=motor[1];
/*初始化电机方式,默认为正向*/
TtMotorModeSet(MOTOR1,MOTOR_FORWARD);
TtMotorModeSet(MOTOR2,MOTOR_FORWARD);
}
/*电机速度控制*/
void TtMotorModeSpeedSet(int motornum,int speed)
{
int setspeed;
if(speed>0)
{
setspeed=speed>1023?1023:speed;
TtMotorModeSet(motornum,MOTOR_FORWARD);
if(motornum==1) analogWrite(motornumber[0],setspeed);
else analogWrite(motornumber[1],setspeed);
}
else
{
setspeed=speed<-1023?-1023:speed;
TtMotorModeSet(motornum,MOTOR_BACK);
if(motornum==1) analogWrite(motornumber[0],setspeed);
else analogWrite(motornumber[1],setspeed);
}
}
private:
/*电机模式设置,motornum1是L298N的通道1控制电机*/
void TtMotorModeSet(int mode,int motornum)
{
switch (mode)
{
case MOTOR_FORWARD:
{
if(motornum==MOTOR1)
{
digitalWrite(motorctrpin[0],LOW);
digitalWrite(motorctrpin[1],HIGHT);
}
else
{
digitalWrite(motorctrpin[2],LOW);
digitalWrite(motorctrpin[3],HIGHT);
}
break;
}
case MOTOR_BACK:
{
if(motornum==MOTOR1)
{
digitalWrite(motorctrpin[0],HIGHT);
digitalWrite(motorctrpin[1],LOW);
}
else
{
digitalWrite(motorctrpin[2],HIGHT);
digitalWrite(motorctrpin[3],LOW);
}
break;
}
case MOTOR_STOP:
{
if(motornum==MOTOR1)
{
digitalWrite(motorctrpin[0],HIGHT);
digitalWrite(motorctrpin[1],HIGHT);
}
else
{
digitalWrite(motorctrpin[2],HIGHT);
digitalWrite(motorctrpin[3],HIGHT);
}
break;
}
default :break;
}
}
uint8_t motorctrpin[4];/*电机控制方向引脚*/
uint8_t motornumber[2];/*电机控制速度引脚*/
};
中断函数处理数据
由于PID控制算法受控制周期影响，控制周期需要稳定，使用delay不是一个好的选择。故需要使用定时器处理函数。不同周期，PID控制器返回的结果是不一样的，这一点需要注意。这里我们使用。
MsTimer2库函数。
MsTimer2::set(20,time_interval); //中断库
MsTimer2::start( );
中断函数处理程序
void time_interval()//定时器中断函数
{
CtlCar(100);//循迹函数
}
总结
程序控制图解:需要控制3个对象，传感器,PID控制器，电机
看看图片，再看看代码，一致的对吧。
/*控制循迹*/
void CtlCar(int runspeed)
{
int SenSorResult,PidOut;
SenSorResult=mytrack.SensorPointfilter(10);
PidOut=myctl.PID_Postion(SenSorResult);
mymotor.TtMotorModeSpeedSet(MOTOR1,PidOut+runspeed);/*差速控制*/
mymotor.TtMotorModeSpeedSet(MOTOR2,PidOut-runspeed);/*差速控制*/
}