控制環(huán)節(jié)與控制量

整個(gè)系統(tǒng)的控制器就是偏差處理函數(shù)，方向環(huán)多為離散的PD控制器（PD控制器會(huì)在后面PID的文章中做介紹）

這里就理解為一個(gè)公式吧，而控制量就是偏差處理函數(shù)計(jì)算出的結(jié)果，還是以電磁為例；下面是電磁偏差處理代碼片。

/*******************************************************************************
* 函數(shù)名    ：Direction_out
* 描述      ：方向環(huán)
* 參數(shù)      ：void
* 返回      :void
* 編寫(xiě)者    ：  小向是個(gè)Der
* 編寫(xiě)日期  ：  2021-12-19
*******************************************************************************/
void Direction_Out(void)
{
   My_Direction.KP=10.0; 
   My_Direction.KD=2.5;
   //轉(zhuǎn)向PD控制//電感差比和算出的偏差做位置式PD計(jì)算
   My_Direction.SumError=My_Direction.KP*My_Direction.NowError+
   My_Direction.KD*(My_Direction.NowError-My_Direction.PrevError);
   My_Direction.PrevError=My_Direction.NowError;
  //中心偏差濾波、加權(quán)滑動(dòng)平均濾波
   My_Direction.Pre1_Error[3]= My_Direction.Pre1_Error[2];
   My_Direction.Pre1_Error[2]= My_Direction.Pre1_Error[1];
   My_Direction.Pre1_Error[1]= My_Direction.Pre1_Error[0];
   My_Direction.Pre1_Error[0]=My_Direction.SumError;
   My_Direction.Direct_Parameter=My_Direction.Pre1_Error[0]*0.8+My_Direction.Pre1_Error[1]*0.1+
     My_Direction.Pre1_Error[2]*0.06+My_Direction.Pre1_Error[3]*0.04 ;
   //此處Price_PWM就是控制量
   Price_PWM=(int16)(My_Direction.Direct_Parameter);
}

執(zhí)行機(jī)構(gòu)

執(zhí)行機(jī)構(gòu)在此系統(tǒng)中主要是電機(jī)驅(qū)動(dòng)和舵機(jī)，其中電機(jī)驅(qū)動(dòng)已經(jīng)在硬件篇介紹過(guò)，也有大佬們的文章作為參考

原理就是利用控制器輸出的控制量這里就是上述代碼中的Price_PWM，分別加減到兩個(gè)電機(jī)的PWM占空比上，實(shí)現(xiàn)內(nèi)外輪速度差，進(jìn)而使小車(chē)轉(zhuǎn)向差速代碼片如下，這里對(duì)于電機(jī)驅(qū)動(dòng)不在做過(guò)多贅述，重點(diǎn)介紹一下舵機(jī)。

//差速處理
  dutyL=200+Price_PWM/5+2*error;
  dutyR=200-Price_PWM/5-2*error;
  //電機(jī)動(dòng)作 單極控制
  pwm_duty(PWMA_CH2P_P62, 0);
  pwm_duty(PWMA_CH1P_P60, dutyL*12);
  pwm_duty(PWMA_CH4P_P66, 0);
  pwm_duty(PWMA_CH3P_P64, dutyR*12);