這個PID控制計算的function跑太慢，佔去60%的CPU時間，請問該如何化簡呢？
(正式版 keil uVision4)
void SetFeedback(void)
{
static int tempErr;
int u,Err;
ADCHL = ADCH; //載入揮發性變數
Err = ADCHL - 127; //求誤差
KI_Sum += Err;
if (KI_Sum>30000) KI_Sum= 30000;
else if(KI_Sum<-30000)KI_Sum=-30000;
if(125<=ADCHL || 129>=ADCHL)KI_Sum=0;//積分重置
KD_Diff = Err-tempErr;
tempErr=Err;
if (KD_Diff>30000) KD_Diff= 30000;
else if(KD_Diff<-30000)KD_Diff=-30000;
u = (Err*18) //P
+ (KI_Sum) //I
+ (KD_Diff*14) //D
;
if(u>255) u=255; //設定上限
else if(u<-255) u=-255; //設定下限
if(u>0) //正轉
{
CCAP0H = (~u)&0xFF;
CCAP1H = 0xFF;
}
else if(u<0) //反轉
{
CCAP0H = 0xFF;
CCAP1H = (0xFF+u)&0xFF; //去掉負號
}
else if(0==u)
CCAP0H=CCAP1H=0xFF;
printf("ADCHL=%d, u=%d\n",ADCHL,u);
}