[求助] 不知算不算BUG...

zgbkdlm 2013-9-25 23:18 楼主

今年电赛做的倒立摆,这是其中的PID部分
这个D微分就是没反应.Ki从0.001-10000都试了,丝毫不起作用.估计是代码的问题,,比赛期间熬的没状态,看了半天也没看出来哪有问题...现在看也看得头晕...
最后就只能PI去参加比赛了,,,,用了纯PI控制电机,,咳咳,,,我就不说啥了...没办法..

long Output=0;
float Setpoint=297;
float errSum=0, lastErr=0,lastQEI_Input=0;
float dErr;
float timeChange=0.001;
float kp=78, ki=0.011;
float kd=2;
GrStringDraw(&sContext, "Progame 3 Selected, Runnning now...", 35, 0, 100, true);
GrFlush(&sContext);
TimerMatchSet(TIMER1_BASE, TIMER_A,4000);
delay_ms(2000);
GrStringDraw(&sContext, "Position Ready, Waiting for Commands...", 39, 0, 120, true);
GrFlush(&sContext);
IntDisable(INT_GPIOF);
GPIOPinIntClear(GPIO_PORTD_AHB_BASE,GPIO_PIN_2); //Define zero point.
IntEnable(INT_GPIOD);
QEI_Input=0;
delay_ms(20);
while(abs((long)QEI_Input)<260);
GrStringDraw(&sContext, "Commands Accepted, Running PID now...", 36, 0, 120, true);
GrFlush(&sContext);
//TimerEnable(TIMER0_BASE, TIMER_A); //Initial Time counter
while(1)
{
float error = Setpoint - QEI_Input;
/*Compute PID Output*/
float dErr;
if (error<0)
{
error=-error;
if (error<=1) {error=0;} //dead band
if (error>=40) errSum=errSum; else
errSum = errSum + ((error+lastErr) /2* timeChange); //梯形积分;
GPIOPinWrite(GPIO_PORTF_AHB_BASE,GPIO_PIN_1|GPIO_PIN_3,8);
}
else
{
if (error<=1) {error=0;} //dead band
if (error>=40) errSum=errSum; else
errSum = errSum + ((error+lastErr) /2* timeChange); //梯形积分;
GPIOPinWrite(GPIO_PORTF_AHB_BASE,GPIO_PIN_1|GPIO_PIN_3,2);
}
dErr = (error-lastErr) * 1000.000;
Output= (long)(kp * error+ ki * errSum + kd * dErr);
//if (Output>=4000) Output=2100;
TimerMatchSet(TIMER1_BASE, TIMER_A,Output);
/*Remember some variables for next time*/
lastErr = error;
}