【文章內(nèi)容簡介】 結(jié)果 unsigned int AD_5。 //AD 轉(zhuǎn)換結(jié)果 unsigned int AD_6。 //AD 轉(zhuǎn)換結(jié)果 signed int Speed,x1,x2,y,x11,x22。 //速度轉(zhuǎn)換值 unsigned int time,numm,m。 unsigned int numm_last0,numm_last1,numm_last2, num_last0,num_last1,num_last2。 //延時子程序 // void delay(unsigned int xms) { int a,b,c。 for(c=0。cxms。c++) { for(a=0。a=50。a++) { for(b=0。b=200。b++)。 } } } ///////////////////////////////////// //// 電機 PID 定義 ///////////////////////////////////// //int SetPoint=0。 //設(shè)定目標(biāo) Desired Value int FeedBack=0。 float KKp=。 //比例常數(shù) Proportional Const float KKi=0。 float KKd=。 //微分常數(shù) Derivative Const signed int EE0=0。 //當(dāng)前誤差 signed int EE1=0。 //Error[1] signed int EE2=0。 //Error[2] signed int EError0=0,EError1=0。 signed int iiIncpid=0。 int sp=0。 int pp。 /////////////////////////////// /// PID 定義 ////////////////////////////// float Kp=。 //比例常數(shù) Proportional Const float Ki=。 float Kd=。 //微分常數(shù) Derivative Const signed int E0=0 。 //當(dāng)前誤差 signed int E1=0。 //Error[1] signed int E2=0。 //Error[2] signed int Error0=0,Error1=0。 signed int iIncpid=0。 ///////////////////////////////////// ///////////////////////////////////// //// 電機 PID ///////////////////////////////////// //////////////////////////////////// unsigned int Motor_PID(int cch1,int cch2 ) { //cch2=cch2/3。 EE0=cch1cch2。 //增量計算 EError0=EE0EE1。 EError1=EE1EE2。 iiIncpid=(int)(KKi*EError0+KKp*EE0+KKd*(EError0EError1))。 EE1=EE0。 EE2=EE1。 //存儲誤差，用于下次計算 sp+=iiIncpid。 if(sp90) sp=90。 if(sp10) sp=10。 return sp。 } ////////////////////////// //////////速度 /////////// ///////////////////////// void Sudu(int SetPoint) { pp=Motor_PID(SetPoint,FeedBack)。 PWMDTY1=pp。 } /////////////////////////////// ///////舵機 PID /////////////// /////////////////////////////// int Steer_PID(signed int ch1 ,signed int ch2 ) { E0=ch1ch2。 //增量計算 Error0=E0E1。 Error1=E1E2。 iIncpid=(int)(Ki*Error0+Kp*E0+Kd*(Error0Error1))。 E1=E0。 E2=E1。 //存儲誤差，用于下次計算 return iIncpid。 } ////////////////////////////// //////////剎車 //////////////// //////////////////////////// void shacheV(int sh_v) { PWME_PWME0=1。 PWME_PWME1=0。 PWMDTY0=sh_v。 while(FeedBackFB)。 PWME_PWME0=0。 PWME_PWME1=1。 flag=0。 Led_right=1。 Led_left=1。 } /////////////////////////// /////////按鍵檢測 /////////// //////////////////////////// void KEY_SCAN(void) { if(KEY==0) { delay(10)。 if(KEY==0) { while(!KEY)。 z++。 if(z==1) { FB=85。 SH=45。 SH1=45。 V1=80。 V2=80。 V3=80。 V4=150。 V5=90。 V6=80。 Led_left=0。 Led_right=1。 delay(500)。 Led_left=1。 } if(z==2) { FB=90。 SH=45。 SH1=45。 V1=85。 V2=85。 V3=80。 V4=150。 V5=90。 V6=80。 Led_right=0。 Led_left=1。 delay(500)。 Led_right=1。 } if(z==3) { FB=100。 SH=45。 SH1=45。 V1=95。 V2=95。 V3=90。 V4= V5=100。 V6=100。 Led_right=0。 Led_left=0。 delay(500)。 Led_right=1。 Led_left=1。 } if(z==4) { z=0。 FB=100。 SH=45。 SH1=45。 V1=95。 V2=95。 V3=90。 V4=170。 V5=100。 V6=100。 Led_right=1。 Led_left=0。 delay(500)。 Led_left=1。 } } } } //////////////////////////////////// ///////////速度控制 ///////////////// //////////////////////////////////// void Speed_con(void) { while(!ATD0STAT0_SCF)。 //等待當(dāng)前隊列轉(zhuǎn)換完成 */ AD_1=ATD0DR0L。 AD_2=ATD0DR1L。 AD_3=ATD0DR2L。 AD_4=ATD0DR3L。 AD_5=