標(biāo)題: PID算法演示 [打印本頁]
作者: Marauder    時(shí)間: 2018-9-9 23:02
標(biāo)題: PID算法演示
** ===================================================================
** SpeedPID
   輸入：speedCount采集車速，AmSpeed 目標(biāo)車速  ；  
   輸出 ：SpeedPWMOUT  計(jì)算車速 ；
** ===================================================================
*/
int16 SpeedControl(int16 speedCount,int16 AmSpeed,uint8 speedKP,uint8 speedKI,uint8 speedKD)
{
  
  
  
  *******************************************************************************************
    //speedCount      檢測值
    //AmSpeed         目標(biāo)值
    //speedKP          P
    //speedKI          I
    //speedKD           D
   
   
   static float Speed1_Err,SumErrSpeed;  //靜態(tài)變量存儲中間變量
   float Speed2_Err,Speed_EC;
   float Speed_P_Value,Speed_D_Value ;
  
   static int16  SpeedPWMOUT;
  
   Speed2_Err = Speed1_Err ;                //將上一次的偏差保存(上一次的偏差值為0)
  
   Speed1_Err = AmSpeed - speedCount  ;      //  計(jì)算新的偏差值（目標(biāo)值-傳感器檢測值）
        
        
  
   Speed_EC = Speed1_Err - Speed2_Err;      //  計(jì)算新的偏差變化值 （上次的偏差值-上上次的偏差值）
        
        
Speed_P_Value =  Speed1_Err * speedKP/10.0 ;   //  增量式PID控制計(jì)算P調(diào)節(jié)量
  
   SumErrSpeed  +=  Speed1_Err * speedKI ;    //增量式PID控制計(jì)算I調(diào)節(jié)量
   Speed_D_Value =  Speed_EC   *  speedKD/100.0 ;     //  增量式PID控制計(jì)算D調(diào)節(jié)量
        

SpeedPWMOUT += (int16)(Speed_P_Value + SumErrSpeed + Speed_D_Value);
        
        
   
        
        
//88888888888888888888888888888888   限幅        7777777777777777777777777777777
   if(SpeedPWMOUT < SPEED_PWM_MIN )
   {
     SpeedPWMOUT = SPEED_PWM_MIN ;
   }        
   else if(SpeedPWMOUT > SPEED_PWM_MAX)
   {
     SpeedPWMOUT = SPEED_PWM_MAX ;
         
   }   
   if(SpeedPWMOUT<=0)SpeedPWMOUT=0;
        
//888888888888888888888888888888888     限幅       77777777777777676666777777777
         
    return  SpeedPWMOUT ;  
}






歡迎光臨 (http://www.torrancerestoration.com/bbs/) Powered by Discuz! X3.1