电脑鼠 | 增量式PID

//以下为电脑鼠程序中电脑鼠直行的增量式PID控制

//error=e(k); d_error=e(k)-e(k-1); dd_error=e(k)-2e(k-1)+e(k-2);

// __GmSPID.sFeedBack: 编码器反馈的速度值；

//__GmSPID.sRef: 直行速度的期望值；

//deadband: 误差死区，允许的误差范围；

//error_IMAX: ???

void __SPIDContr(void) 
{ 
    int16  error,d_error,dd_error;
    static uint8   K_I=1;
    error = __GmSPID.sRef - __GmSPID.sFeedBack;
    d_error = error - __GmSPID.sPreError; 
    dd_error = d_error - __GmSPID.sPreDerror;
    if(error> Deadband)
      error -= Deadband;
    else if(error < -Deadband)
      error += Deadband;
    else
      error = 0;
    if((error > error_IMAX)||(error < -error_IMAX))
      K_I=0;
    else
      K_I=1;
    
    __GmSPID.sPreError = error; 
    __GmSPID.sPreDerror = d_error;
    
    __GmSPID.iPreU += (int16)(  __GmSPID.fKp * d_error + K_I*__GmSPID.fKi * error  + __GmSPID.fKd*dd_error); 
}