【正文】 dir_kd=30。 } else { dir_kp=74。//彎道 dir_kd=10。// } } } /****************************/ void dir_caculater(char det) { uint Dir。 Dir=6020+dir_kp*dir_order+dir_kd*(dir_orderdir_order1)+det。 if(flagc==1) dir_order1=dir_order。 PWMDTY23=Dir。 } /****************************/ void speed_caculater(void) { if(zd==0) { if(((fm==1)amp。amp。(fm1==0)amp。amp。((fr==1)||(fl==1)))||(jzw==1)) //左右出線盲區(qū) SetPoint=*speedp。 else { if((fzd==1)amp。amp。(dir_order=4)amp。amp。(dir_order=4))//直道 SetPoint=*(speedp+3)。 else { if(((fdl==1)amp。amp。(dir_order0))||((fdr==1)amp。amp。(dir_order0)))//單邊圓弧 SetPoint=*(speedp+2)。 else SetPoint=*(speedp+1)。//其他 } } switch(fj) { case(0):SetPoint=SetPoint5。 case(1):SetPoint=SetPoint+1。 default:。 } if(ft==0) { if(fbang==1) bangbang()。 if(fpid==1) PIDF()。 } else { Control_Speed(0)。 } } } //// void bangbang(void) {uint jieguo。 uint shu。 if(pcount=SetPoint) { PTM_PTM7=1。 jieguo=300*(SetPointpcount)。 if(jieguo200) jieguo=200。 if(jieguo3180) jieguo=3180。 shu=jieguo。 Control_Speed(shu)。 } else { PTM_PTM7=0。 } } //PID參數(shù)初始化// void IncPIDInit(void) { Proportion=。 //比例常數(shù) Integral=。 //積分常數(shù) Derivative=。 //微分常數(shù) iIncpid=0。 PrevError=0。 LastError=0。 UKMAX=Proportion * (*(speedp+3))+Integral * (*(speedp+3)) + Derivative * (*(speedp+3))。 } //當前誤差 //增量式PID控制設計// void IncPIDCalc(void) { flagc=0。 iError=SetPointpcount。 //增量計算 if(iError=7) iIncpid=UKMAX。 else iIncpid+=Proportion * (iErrorLastError)+Integral * iError + Derivative * (iError2*LastError+PrevError)。 if(iIncpid0) iIncpid=0。 if(iIncpidUKMAX) iIncpid=UKMAX。 PrevError=LastError。 //存儲誤差，用于下次計算 LastError=iError。 } /*************PID控制***************/ void PIDF(void) {float zhan。 float shu。 int shu2。 if((flagc==1)amp。amp。(zd==0)amp。amp。(js==0)) { IncPIDCalc()。 PTM_PTM7=1。 zhan=iIncpid/UKMAX。 if(zhan1) zhan=1。 if(zhan0) zhan=0。 shu=3200*zhan。 shu2=(int)shu。