|
|
就是比較簡(jiǎn)單的循跡小車(chē)，可以循黑線的那種，因?yàn)樵趯W(xué)PID，所以給它加了個(gè)PD控制，不過(guò)效果不太好這是實(shí)現(xiàn)PID控制的主要代碼
- #include"PID.h"
- #include"Trace.h"
- extern int flag;
- int RK=6; //固定基準(zhǔn)，不能變得
- int KP=4; //比例調(diào)節(jié)參數(shù)
- int out; //最后輸出結(jié)果
- //int out0=40; //初始偏移分量
- int Td=7; //微分時(shí)間常數(shù)
- int Ti=1; //積分時(shí)間常數(shù)
- int T =1; //采樣周期
- int Sk; //積分，微分
- int Dk;
- int Ek[]={0,0,0,0,0};
- /*================================================================
- 將flag的值與RK=5相減，當(dāng)右邊的紅外對(duì)管檢測(cè)到黑線時(shí)
- ，即flag為7 8 9 10 11 12時(shí)小車(chē)發(fā)生了左偏，增加左輪轉(zhuǎn)速，減小右輪
- 將flag的值與RK=5相減，當(dāng)左邊的紅外對(duì)管檢測(cè)到黑線時(shí)
- ，即flag為5 4 3 2 1 0 時(shí)小車(chē)發(fā)生了右偏，增加右輪轉(zhuǎn)速，減小左輪
- //================================================================*/
- void PID (int flag)
- {
- Ek[4]=Ek[3];
- Ek[3]=Ek[2];
- Ek[2]=Ek[1];
- Ek[1]=Ek[0];
- Ek[0]=RK-flag;
- //Sk = (T/Ti)*(Ek[0]+Ek[1]+Ek[2]+Ek[3]+Ek[4]);
- Dk = (Td/T)*(Ek[0]-Ek[1]);
- out = KP*Ek[0]+Sk+Dk;
- turn(25+out,25-out);
-
-
-
- }
-
extern int flag;
int RK=6;
int KP=4;
int out;
//int out0=40;
int Td=7;
int Ti=1;
int T =1;
int Sk;
int Dk;
int Ek[]={0,0,0,0,0};
void PID (int flag)
{
Ek[4]=Ek[3];
Ek[3]=Ek[2];
Ek[2]=Ek[1];
Ek[1]=Ek[0];
Ek[0]=RK-flag;
//Sk = (T/Ti)*(Ek[0]+Ek[1]+Ek[2]+Ek[3]+Ek[4]);
Dk = (Td/T)*(Ek[0]-Ek[1]);
out = KP*Ek[0]+Sk+Dk;
turn(25+out,25-out);
}
void xunji (void)
{
if(count ==200)//采樣周期
{
if(input==0x77)//0111 0 111
flag=6;
else if(input==0x7b)//0111 0 01
flag=7;
else if(input==0x7b)//0111 1 011
flag=8;
else if(input==0x75)//0111 1 001
flag=9;
else if(input==0x7d)//0111 1 101
flag=10;
else if(input==0x7c)//0111 1 100
flag=11;
else if(input==0x7e)//0111 1 110
flag=12;
else if(input==0x6d)//0110 0 111
flag=5;
else if(input==0x6f)//0110 1 111
flag=4;
else if(input==0x4f)//0100 1 111
flag=3;
else if(input==0x5f)//0101 1 111
flag=2;
else if(input==0x1f)//0001 1 111
flag=1;
else if(input==0x10)//0011 1 111
flag=0;
count=0;
}
PID (flag);
}
全部資料51hei下載地址：
循跡小車(chē)PID算法.rar
(42.84 KB, 下載次數(shù): 133)
2019-1-2 18:35 上傳
點(diǎn)擊文件名下載附件
|
評(píng)分
-
查看全部評(píng)分
|
