單片機PID循跡小車代碼

ID:423284 · 發表于 2019-1-2 18:36

就是比較簡單的循跡小車，可以循黑線的那種，因為在學PID，所以給它加了個PD控制，不過效果不太好這是實現PID控制的主要代碼

#include"PID.h"
#include"Trace.h"
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};
/*================================================================
將flag的值與RK=5相減，當右邊的紅外對管檢測到黑線時
，即flag為7 8 9 10 11 12時小車發生了左偏，增加左輪轉速，減小右輪
將flag的值與RK=5相減，當左邊的紅外對管檢測到黑線時
，即flag為5 4 3 2 1 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);
}

復制代碼

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下載地址：

循跡小車PID算法.rar (42.84 KB, 下載次數: 133)