循迹小车项目
前言
這篇很久之前寫的文章，有挺多問題的。還有這么多人喜歡，兩年后，也就是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 Constvolatile float Integral; // 積分常數 Integral Constvolatile float Derivative; // 微分常數 Derivative Constvolatile 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); // Dsptr->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); // Dposition.Error1=position.iError; // 存儲誤差，用于下次計算 /*限幅處理*/if(abs(iIncpid)>MatOut) {iIncpid=iIncpid>0?MatOut:-MatOut;} return iIncpid; // 返回計算值}private:/*******************PID定義*****************************/typedef struct{float Proportion; // 比例常數 Proportional Constfloat Integral; // 積分常數 Integral Constfloat Derivative; // 微分常數 Derivative Constint Error1; // Error[n-1]int Error2; // Error[n-2]int iError; // Error[n]int Error_sum;}PID;PID position;//位置式PIDint 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語言版本
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路模擬量循跡方案
檢測原理：灰度傳感器是模擬傳感器，有一只發光二極管和一只光敏電阻，安裝在同一面上?；叶葌鞲衅骼貌煌伾臋z測面對光的反射程度不同，光敏電阻對不同檢測面返回的光其阻值也不同的原理進行顏色深淺檢測。在有效的檢測距離內，發光二極管發出白光，照射在檢測面上，檢測面反射部分光線，光敏電阻檢測此光線的強度并將其轉換為機器人可以識別的信號。
基本思路：
通過檢測兩個光敏傳感器的差值，就可以知道車子的偏離黑線的程度，檢測方案類似5路傳感器。例如，直線檢測，兩個傳感器都沒有檢測到黑線的值，那么兩個傳感器獲取到的模擬量應該是類似的，也就是差值很小就判斷為直線。反之，判斷為左偏離黑線或者右偏離黑線。
C語言版本是2年前寫的，代碼可讀性不怎么好，但思路是沒有變化的，建議看C++的！
C語言版本
/*************************************************/ //函數名： read_sensor_values //作者： anonymous //日期： 2020.11.12 //功能： 檢測灰度值 //輸入參數：void //返回值： 返回偏差值 /*************************************************/ /*合理偏差范圍值*/ #define STRAIGHT_LINE_RANGE 40int sensor[2] = {0,0}; //2個傳感器數值的數組 int read_sensor_values()//讀取偏差值 {int adval=0;//臨時兩邊AD偏差值//從左到右的檢測模塊，依次標為0，1sensor[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<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,D6int temp_speed=0;if((motor==Motor_Left_Gruop))//左輪組{if(motor_speed>0)//判斷正轉,控制速度{temp_speed=motor_speed;pinMode(10, OUTPUT );//寫D5，D9PWMdigitalWrite(10, LOW);//10是左輪analogWrite(9,temp_speed);return Motor_Left_Gruop;}else {temp_speed=-motor_speed;pinMode(9, OUTPUT );//寫D5，D9PWMdigitalWrite(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，D9PWMdigitalWrite(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，D9PWMdigitalWrite(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); // Dposition.Error1=position.iError; // 存儲誤差，用于下次計算 /*限幅處理*/if(abs(iIncpid)>MatOut) {iIncpid=iIncpid>0?MatOut:-MatOut;} return iIncpid; // 返回計算值}private:/*******************PID定義*****************************/typedef struct{float Proportion; // 比例常數 Proportional Constfloat Integral; // 積分常數 Integral Constfloat Derivative; // 微分常數 Derivative Constint Error1; // Error[n-1]int Error2; // Error[n-2]int iError; // Error[n]int Error_sum;}PID;PID position;//位置式PIDint 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);/*差速控制*/ }總結
- 上一篇： Eclipse 9.x 10.0 之破
- 下一篇： 【对抗攻击代码实战】对抗样本的生成——F