【文章內(nèi)容簡介】 ed 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=ATD0DR4L。 AD_6=ATD0DR5L。 //////////////////////////////// /////////斜坡檢測////////////// /////////////////////////////// //if(AD_190 amp。amp。 AD_490 || AD_150 amp。amp。 AD_450) //{ // PWMDTY23=1565。 //Sudu(num_last2)。 //} //else // { if(AD_13 || AD_43) { x1=AD_282。 //中間左 x2=AD_382。 //中間右 if(AD_1AD_4) x1=x1。 if(AD_4AD_1) x2=x2。 y=Steer_PID(x1,x2)。 //舵機PID if(y650) y=650。 if(y650) y=650。 numm=1570y。 PWMDTY23=numm。 if(AD_140 || AD_440) //彎道 { if(flag==1 amp。amp。 FeedBackFB) { if(AD_1AD_4) { Led_left=0。 PWMDTY23=numm。 shacheV(SH)。 Sudu(V1)。 } else { Led_right=0。 PWMDTY23=numm。 shacheV(SH1)。 Sudu(V2)。 } } else {