控制環節與控制量

整個系統的控制器就是偏差處理函數，方向環多為離散的PD控制器（PD控制器會在后面PID的文章中做介紹）

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

/*******************************************************************************
* 函數名    ：Direction_out
* 描述      ：方向環
* 參數      ：void
* 返回      :void
* 編寫者    ：  小向是個Der
* 編寫日期  ：  2021-12-19
*******************************************************************************/
void Direction_Out(void)
{
   My_Direction.KP=10.0; 
   My_Direction.KD=2.5;
   //轉向PD控制//電感差比和算出的偏差做位置式PD計算
   My_Direction.SumError=My_Direction.KP*My_Direction.NowError+
   My_Direction.KD*(My_Direction.NowError-My_Direction.PrevError);
   My_Direction.PrevError=My_Direction.NowError;
  //中心偏差濾波、加權滑動平均濾波
   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);
}

執行機構

執行機構在此系統中主要是電機驅動和舵機，其中電機驅動已經在硬件篇介紹過，也有大佬們的文章作為參考

原理就是利用控制器輸出的控制量這里就是上述代碼中的Price_PWM，分別加減到兩個電機的PWM占空比上，實現內外輪速度差，進而使小車轉向差速代碼片如下，這里對于電機驅動不在做過多贅述，重點介紹一下舵機。

//差速處理
  dutyL=200+Price_PWM/5+2*error;
  dutyR=200-Price_PWM/5-2*error;
  //電機動作 單極控制
  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);