第13章-循跡功能 循跡小車 原理分析 STM32智能小車 紅外對(duì)管使用 PID循跡算法分析

講解一下我們小車?yán)锩娴难E部分，包括紅外基礎(chǔ)使用，無(wú)PID循跡和有PID循跡。

第13章-循跡功能

13.1-非PID循跡功能完成

先紅外對(duì)管調(diào)試

我們這里學(xué)習(xí)一下，如何實(shí)現(xiàn)循跡功能

如何才能讓小車沿著黑線運(yùn)動(dòng)、要讓小車感知到黑線的位置，使用這種傳感器就可以反饋黑線是否存在

根據(jù)傳感器特性，我們檢測(cè)紅外對(duì)管DO引腳的電壓就可以知道，下面有沒有黑線

DO 高電平->有黑線 小燈滅

DO低電平->沒有黑線 小燈亮

這是好多地方對(duì)這個(gè)產(chǎn)品的說(shuō)明

然后我們組合上面的紅外對(duì)管，安裝到小車上，就可以知道小車是否偏離了黑線，

下面我們通過單片機(jī)讀取紅外對(duì)管DO口的電壓，就知道黑線在小車下面的位置了
STM32初始化

先看原理圖需要初始化那些引腳

把OUT_1-PA5、OUT_2-PA7、OUT_3-PB0、OUT_4-PB1初始化為輸入模式

重新生成

然后我們?cè)趃pio.h 添加讀取GPIO的宏，使得程序更簡(jiǎn)潔

#define READ_HW_OUT_1   HAL_GPIO_ReadPin(HW_OUT_1_GPIO_Port,HW_OUT_1_Pin) //讀取紅外對(duì)管連接的GPIO電平
#define READ_HW_OUT_2   HAL_GPIO_ReadPin(HW_OUT_2_GPIO_Port,HW_OUT_2_Pin)
#define READ_HW_OUT_3   HAL_GPIO_ReadPin(HW_OUT_3_GPIO_Port,HW_OUT_3_Pin)
#define READ_HW_OUT_4   HAL_GPIO_ReadPin(HW_OUT_4_GPIO_Port,HW_OUT_4_Pin)

根據(jù)紅外對(duì)管狀態(tài)控制電機(jī)速度

注意：整個(gè)主函數(shù)不要加入延時(shí)，這樣實(shí)時(shí)性更高，可以根據(jù)紅外對(duì)管狀態(tài)做出及時(shí)控制

	if(READ_HW_OUT_1 == 0&&READ_HW_OUT_2 == 0&&READ_HW_OUT_3 == 0&&READ_HW_OUT_4 == 0 )
	{
		printf("應(yīng)該前進(jìn)rn");
		motorPidSetSpeed(1,1);//前運(yùn)動(dòng)
	}
	if(READ_HW_OUT_1 == 0&&READ_HW_OUT_2 == 1&&READ_HW_OUT_3 == 0&&READ_HW_OUT_4 == 0 )
	{
		printf("應(yīng)該右轉(zhuǎn)rn");
		motorPidSetSpeed(0.5,2);//右邊運(yùn)動(dòng)
	}
	if(READ_HW_OUT_1 == 1&&READ_HW_OUT_2 == 0&&READ_HW_OUT_3 == 0&&READ_HW_OUT_4 == 0 )
	{
		printf("快速右轉(zhuǎn)rn");
		motorPidSetSpeed(0.5,2.5);//快速右轉(zhuǎn)
	}
	if(READ_HW_OUT_1 == 0&&READ_HW_OUT_2 == 0&&READ_HW_OUT_3 == 1&&READ_HW_OUT_4 == 0 )
	{
		printf("應(yīng)該左轉(zhuǎn)rn");
		motorPidSetSpeed(2,0.5);//左邊運(yùn)動(dòng)
	}
	if(READ_HW_OUT_1 == 0&&READ_HW_OUT_2 == 0&&READ_HW_OUT_3 == 0&&READ_HW_OUT_4 == 1 )
	{
		printf("快速左轉(zhuǎn)rn");
		motorPidSetSpeed(2.5,0.5);//快速左轉(zhuǎn)
	}

然后測(cè)試

1. 測(cè)試紅外對(duì)管靈敏度，放在有黑線的地上或者紙上，然后把小車黑線比如放到最右邊 及第一個(gè)紅外對(duì)管，觀察紅外對(duì)管小燈變化情況和串口輸出情況，如果小燈沒有滅，就調(diào)節(jié)紅外對(duì)管靈敏度和室內(nèi)燈光，直到每個(gè)紅外對(duì)管都可以感應(yīng)到小燈。
2. 然后在黑線上讓小車循跡
   

然后循跡功能完成

然后放到地上

13.2-加入循跡PID

前面的代碼我們對(duì)循跡是判斷的幾個(gè)狀態(tài)，然后PID控制電機(jī)不同速度，但是我們可以使用紅外對(duì)管狀態(tài)作為PID控制的輸入然后再控制電機(jī)。

PID的輸入是紅外對(duì)管狀態(tài)，我們?cè)O(shè)計(jì) PID輸入是紅外對(duì)管的狀態(tài)、然后輸出一個(gè)速度值，然后左右電機(jī)去加或者減這個(gè)值，就可以完成根據(jù)紅外對(duì)管輸入對(duì)電機(jī)的差速控制

主函數(shù)添加的

extern tPid pidHW_Tracking;//紅外循跡的PID
uint8_t g_ucaHW_Read[4] = {0};//保存紅外對(duì)管電平的數(shù)組
int8_t g_cThisState = 0;//這次狀態(tài)
int8_t g_cLastState = 0; //上次狀態(tài)
float g_fHW_PID_Out;//紅外對(duì)管PID計(jì)算輸出速度
float g_fHW_PID_Out1;//電機(jī)1的最后循跡PID控制速度
float g_fHW_PID_Out2;//電機(jī)2的最后循跡PID控制速度

然后實(shí)現(xiàn)PID循跡控制、注意為了更加快，要減少?zèng)]有必要的程序和優(yōu)化判斷、將沒有必要的輸出都注釋掉

	g_ucaHW_Read[0] = READ_HW_OUT_1;//讀取紅外對(duì)管狀態(tài)、這樣相比于寫在if里面更高效
	g_ucaHW_Read[1] = READ_HW_OUT_2;
	g_ucaHW_Read[2] = READ_HW_OUT_3;
	g_ucaHW_Read[3] = READ_HW_OUT_4;

	if(g_ucaHW_Read[0] == 0&&g_ucaHW_Read[1] == 0&&g_ucaHW_Read[2] == 0&&g_ucaHW_Read[3] == 0 )
	{
//		printf("應(yīng)該前進(jìn)rn");//注釋掉更加高效，減少無(wú)必要程序執(zhí)行
		g_cThisState = 0;//前進(jìn)
	}
	else if(g_ucaHW_Read[0] == 0&&g_ucaHW_Read[1] == 1&&g_ucaHW_Read[2] == 0&&g_ucaHW_Read[3] == 0 )//使用else if更加合理高效
	{
//		printf("應(yīng)該右轉(zhuǎn)rn");
		g_cThisState = -1;//應(yīng)該右轉(zhuǎn)
	}
	else if(g_ucaHW_Read[0] == 1&&g_ucaHW_Read[1] == 0&&g_ucaHW_Read[2] == 0&&g_ucaHW_Read[3] == 0 )
	{
//		printf("快速右轉(zhuǎn)rn");
		g_cThisState = -2;//快速右轉(zhuǎn)
	}
	else if(g_ucaHW_Read[0] == 1&&g_ucaHW_Read[1] == 1&&g_ucaHW_Read[2] == 0&&g_ucaHW_Read[3] == 0)
	{
//		printf("快速右轉(zhuǎn)rn");
		g_cThisState = -3;//快速右轉(zhuǎn)
	}
	else if(g_ucaHW_Read[0] == 0&&g_ucaHW_Read[1] == 0&&g_ucaHW_Read[2] == 1&&g_ucaHW_Read[3] == 0 )
	{
//		printf("應(yīng)該左轉(zhuǎn)rn");
		g_cThisState = 1;//應(yīng)該左轉(zhuǎn)	
	}
	else if(g_ucaHW_Read[0] == 0&&g_ucaHW_Read[1] == 0&&g_ucaHW_Read[2] == 0&&g_ucaHW_Read[3] == 1 )
	{
//		printf("快速左轉(zhuǎn)rn");
		g_cThisState = 2;//快速左轉(zhuǎn)
	}
	else if(g_ucaHW_Read[0] == 0&&g_ucaHW_Read[1] == 0&&g_ucaHW_Read[2] == 1&&g_ucaHW_Read[3] == 1)
	{
//	    printf("快速左轉(zhuǎn)rn");
		g_cThisState = 3;//快速左轉(zhuǎn)
	}
	g_fHW_PID_Out = PID_realize(&pidHW_Tracking,g_cThisState);//PID計(jì)算輸出目標(biāo)速度 這個(gè)速度，會(huì)和基礎(chǔ)速度加減

	g_fHW_PID_Out1 = 3 + g_fHW_PID_Out;//電機(jī)1速度=基礎(chǔ)速度+循跡PID輸出速度
	g_fHW_PID_Out2 = 3 - g_fHW_PID_Out;//電機(jī)1速度=基礎(chǔ)速度-循跡PID輸出速度
	if(g_fHW_PID_Out1 >5) g_fHW_PID_Out1 =5;//進(jìn)行限幅 限幅速度在0-5之間
	if(g_fHW_PID_Out1 <0) g_fHW_PID_Out1 =0;
	if(g_fHW_PID_Out2 >5) g_fHW_PID_Out2 =5;
	if(g_fHW_PID_Out2 <0) g_fHW_PID_Out2 =0;
	if(g_cThisState != g_cLastState)//如何這次狀態(tài)不等于上次狀態(tài)、就進(jìn)行改變目標(biāo)速度和控制電機(jī)、在定時(shí)器中依舊定時(shí)控制電機(jī)
	{
		motorPidSetSpeed(g_fHW_PID_Out1,g_fHW_PID_Out2);//通過計(jì)算的速度控制電機(jī)
	}
	
	g_cLastState = g_cThisState;//保存上次紅外對(duì)管狀態(tài)

在pid.中

tPid pidHW_Tracking;//紅外循跡的PID

	pidHW_Tracking.actual_val=0.0;
	pidHW_Tracking.target_val=0.00;//紅外循跡PID 的目標(biāo)值為0
	pidHW_Tracking.err=0.0;
	pidHW_Tracking.err_last=0.0;
	pidHW_Tracking.err_sum=0.0;
	pidHW_Tracking.Kp=-1.50;
	pidHW_Tracking.Ki=0;
	pidHW_Tracking.Kd=0.80;

然后就可以跑一下試試了。

可以改進(jìn)的地方

2. 紅外對(duì)管影響差速轉(zhuǎn)向，也影響基礎(chǔ)直行的速度 ，會(huì)有更好控制效果，所以可以加入每種紅外對(duì)管狀態(tài)下對(duì)基礎(chǔ)速度的影響。
3. 紅外對(duì)管的數(shù)量越多，效果會(huì)越好。

第15章我們會(huì)講解手機(jī)遙控的功能

聯(lián)系:Q,1930299709