Code:
task task_PID_A() {
float aspeed, damp, PWMpwr, readold, errorold, tprop;
long readstart, cmax, cmin; // for monitoring
long starttime, runtime, clock, dtime; // timer
char regloop;
PID_A.runstate = 0x10; // reg state: RAMPUP
PID_A.read = (MotorRotationCount(OUT_A)); // get current encoder reading
PID_A.err = PID_A.target - PID_A.read; // error to target
readstart = PID_A.read;
regloop = 1;
#ifdef debug_PID_A
//............................................................................
// init variables for graph output
ClearScreen();
DisplayMask();
int timex, oldtx, oldy=15, pwm0=15; // values for graphic screen
float scrXratio, scrYratio;
scrXratio=abs(PID_A.err)/8;
if(PID_A.target<50) scrXratio=abs(PID_A.err)/4;
scrYratio=abs(PID_A.err)/40;
//............................................................................
#endif
damp=0; // damp the integral memory
starttime= CurrentTick();
// appoach target
_Astart:
PID_A.runstate = 0x10; // run state: RUNNING
do {
dtime = CurrentTick() - clock;
clock = CurrentTick();
runtime = clock - starttime;
tprop = dtime/20.0;
if ((PID_A.err==errorold)&& (abs(PID_A.err)>PID_A.precis)) damp=1; // stalling
else
damp=PID_A.damp;
PID_A.integr = (damp * PID_A.integr) + PID_A.err;
if((PID_A.integr) > 3*PID_A.maxout) PID_A.integr = 3*PID_A.maxout; // cut away
else
if((PID_A.integr) <-3*PID_A.maxout) PID_A.integr =-3*PID_A.maxout;
PWMpwr= (PID_A.P*PID_A.err) + (PID_A.I*PID_A.integr)*tprop + (PID_A.D*(PID_A.err-errorold))/tprop;
if(PWMpwr > PID_A.maxout) PWMpwr= PID_A.maxout; // forward maxout
else
if(PWMpwr < -PID_A.maxout) PWMpwr= -PID_A.maxout; // reverse maxout
PID_A.speed= (PID_A.read-readold)*100/dtime; // rotat speed [degrees/100ms]
aspeed = abs(PID_A.speed) ;
if (aspeed > PID_A.tarpwm) {
PWMpwr = sign(PWMpwr)*PID_A.tarpwm;
}
PID_A.outp = round(PWMpwr);
#ifdef debug_PID_A
//..........................................................................
// for graph output
timex= runtime/scrXratio;
PointOut(timex,(PID_A.read-readstart)/scrYratio);
LineOut(oldtx, oldy, timex, pwm0+PID_A.speed*0.3);
oldtx=timex; oldy=pwm0+PID_A.speed*0.3;
//..........................................................................
#endif
//**************************************************************************
// PID regulation !
OnFwd(OUT_A, (PID_A.outp)); // action !
Wait(PID_A.regtime); // wait regulation time
//**************************************************************************
readold = PID_A.read; // save old sensor
errorold = PID_A.err; // save old error
PID_A.read = (MotorRotationCount(OUT_A)); // get new encoder value
PID_A.err = PID_A.target-PID_A.read; // new error to target
if (PID_A.read>cmax) cmax=PID_A.read; // monitor overshooting
else
if (PID_A.read<cmin) cmin=PID_A.read; // monitor overshooting
if ((PID_A.cont)&& (abs(PID_A.err)<=PID_A.precis)) PID_A.runstate = 0x60;
else PID_A.runstate = 0x20;
#ifdef debug_PID_A
//..........................................................................
printf1(68, 48,"%-5d" , cmax);
printf1(68, 40,"%-5d" , cmin);
printf1(68, 32,"%-5d" , PID_A.read);
//..........................................................................
#endif
if (PID_A.cont) continue;
if (abs(PID_A.err)<=PID_A.precis) { regloop +=1 ; PID_A.runstate = 0x40; }
} while ((abs(PID_A.err)>=PID_A.precis) && (regloop<=5)); // target reached
Off(OUT_A); // finished - brake motor
PID_A.runstate = 0x40; // run state: RAMPDOWN
PID_A.outp=0;
Wait(50);
PID_A.read = MotorRotationCount(OUT_A);
regloop=1;
if (PID_A.read>cmax) cmax=PID_A.read; // detect overshooting
if (PID_A.read<cmin) cmin=PID_A.read; // detect overshooting
PID_A.err = PID_A.target-PID_A.read;
#ifdef debug_PID_A
//............................................................................
printf1(68, 56,"%-5d" , PID_A.target);
printf1(68, 48,"%-5d" , cmax);
printf1(68, 40,"%-5d" , cmin);
printf1(68, 32,"%-5d" , PID_A.read);
printf1(56, 24,"P %5.2f" , PID_A.P);
printf1(56, 16,"I %5.2f" , PID_A.I);
printf1(56, 8,"D %5.2f" , PID_A.D);
//............................................................................
#endif
if ((abs(PID_A.err)>PID_A.precis)) {goto _Astart;}
#ifdef debug_PID_A
//............................................................................
PointOut(timex,PID_A.read/scrYratio);
LineOut(oldtx, oldy, timex, pwm0);
LineOut(timex+2,PID_A.target/scrYratio, timex+10, PID_A.target/scrYratio);
LineOut(timex+2, pwm0, timex+10, pwm0);
//............................................................................
#endif
PID_A.runstate=0;
Wait(1); //runstate = IDLE
}
//==============================================================================
Lesezeichen