setup: plug 3 motors to the NXT.
To my observations the controller runs quite smooth (OUT_A), with no apparent overshooting.
Code: Select all
SetPIDparam(OUT_A, 0.5, 0.3, 9); // p,i,dCode: Select all
SetPIDparam(OUT_B, 0.4, 0.5, 12); // p,i,d
SetPIDparam(OUT_C, 0.4, 0.4, 10); // p,i,d Code: Select all
///////////////////////////////////////////////////////////////////////////////////////
//void RotatePIDtoTarget (char port, long Target, float RotatSpeed); // absolute target
//void RotatePIDdegrees (char port, long Target, float RotatSpeed); // relative target
// // new features:
//void RotatePIDcontinue (char port, long Target, float RotatSpeed); // continue mode
//void StopPIDcontrol (char port); // stop PID
///////////////////////////////////////////////////////////////////////////////////////
via #define you may adjust which graphs of which motors should be shown on the screen:
Code: Select all
#define debug_PID_A // debug mode featuring graphic output
//#define debug_PID_B // debug mode featuring graphic output
//#define debug_PID_C // debug mode featuring graphic outputp,i,d juggling and adjusting:
By the current settings the PID controller for OUT_A avoids overshooting.
If you want more power, however, also in the vicinity of the target, you will have to risk overshooting (more or less);
here you may either increase the "pid.I" parameter (maybe e.g., to 0.5 or 0.6) - or - set the "PID.damp" parameter higher (previously 0.75), e.g. to 0.85:
void SetPIDparamEx(char port, float P, float I, float D, float prec, int rtime, float damp)
If overshooting is extended to multiple oscillatings, you may try to decrease pid.I - or - increase pid.D,
but if the PID control causes quick jerking then only decreasing pid.D will be useful.
It is possible to change the sensor input readings and sensor targets in order to use the PID motor controller with different sensor readings and targets, e.g. USS sensor or light sensor values instead of encoder values, or, e.g. RCX style DC motors combined with RCX rotation sensors (especially also if attached to multiplexers).
To-do-list [resolved]: set continuous regulation mode (follow-up-mode) if target has been approached (no termination condition in the PID regulation loop).
Source code (beta version) see link below
(edit: code is working correctly also with negative targets, but the graphs are working just for positive ones => => to-do-list. )
If anyone wants to test: share and enjoy! Comments and bug reports are welcome!
to read the most updated source code of the controller you may wish to refer to this link (progress to the end of the thread!):
http://www.mindstormsforum.de/viewtopic ... 521#p61921
Code: Select all
// PID controller: exactly approaching sensor targets by motor control
// ver 1.02
// (c) HaWe 2013
// protected under the friendly Creative Commons Attribution-NonCommercial-ShareAlike 3.0 Unported License
// http://creativecommons.org/licenses/by-nc-sa/3.0/
///////////////////////////////////////////////////////////////////////////////////////
//void RotatePIDtoTarget (char port, long Target, float RotatSpeed); // absolute target
//void RotatePIDdegrees (char port, long Target, float RotatSpeed); // relative target
// // new features:
//void RotatePIDcontinue (char port, long Target, float RotatSpeed); // continue mode
//void StopPIDcontrol (char port); // stop PID
///////////////////////////////////////////////////////////////////////////////////////
// NXC API: runstate constants
/*
#define OUT_RUNSTATE_IDLE 0x00
#define OUT_RUNSTATE_RAMPUP 0x10
#define OUT_RUNSTATE_RUNNING 0x20
#define OUT_RUNSTATE_RAMPDOWN 0x40
#define OUT_RUNSTATE_HOLD 0x80
*/
#define OUT_RUNSTATE_ACTIVE 0x01
#define debug_PID_A // debug mode featuring graphic output
//#define debug_PID_B // debug mode featuring graphic output
//#define debug_PID_C // debug mode featuring graphic output
//forward:
void DisplayMask();
#define printf1( _x, _y, _format1, _value1) { \
string sval1 = FormatVal(_format1, _value1); \
TextOut(_x, _y, sval1); \
}
inline long round(float f) { return (f>0?(f+0.5):(f-0.5)); }
//==============================================================================
struct PIDstruct {
// custom target values
long target; // set target
int tarpwm; // motor target speed
// custom regulation parameters
float P; // P: basic propotional to error
float I; // I: integral: avoid perish
float D; // D: derivative: avoid oscillating
float precis; // error precision to target
int regtime; // PID loop time
float damp; // damp the integral memory
char cont; // target: continue or hit once
// internal control variables
char runstate; // monitors runstate
int outp; // PID control output value
int maxout; // max output (max motor pwr)
long read; // current sensor reading
float err; // current error
float integr; // integral of errors
float cspeed; // current speed
} ;
PIDstruct PID_A, PID_B, PID_C;
//==============================================================================
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;
//............................................................................
// init variables for graph output
#ifdef debug_PID_A
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.cspeed= (PID_A.read-readold)*100/dtime; // rotat speed [degrees/100ms]
aspeed = abs(PID_A.cspeed) ;
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.cspeed*0.3);
oldtx=timex; oldy=pwm0+PID_A.cspeed*0.3;
printf1( 0,48,"%5.0f" , PID_A.cspeed);
//..........................................................................
}
#endif
//**************************************************************************
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(63, 48,"%-6d" , cmax);
printf1(63, 40,"%-6d" , cmin);
printf1(63, 32,"%6.1f" , PWMpwr);
//..........................................................................
#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(63, 48,"%-6d" , cmax);
printf1(63, 40,"%-6d" , cmin);
printf1(63, 32,"%6.1f" , PWMpwr);
#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
}
//==============================================================================
task task_PID_B() {
float aspeed, damp, PWMpwr, readold, errorold, tprop;
long readstart, cmax, cmin; // for monitoring
long starttime, runtime, clock, dtime; // timer
char regloop;
PID_B.runstate = 0x10; // reg state: RAMPUP
PID_B.read = (MotorRotationCount(OUT_B)); // get current encoder reading
PID_B.err = PID_B.target - PID_B.read; // error to target
readstart = PID_B.read;
regloop = 1;
//............................................................................
// init variables for graph output
#ifdef debug_PID_B
ClearScreen();
DisplayMask();
int timex, oldtx, oldy=15, pwm0=15; // values for graphic screen
float scrXratio, scrYratio;
scrXratio=abs(PID_B.err)/8;
if(PID_B.target<50) scrXratio=abs(PID_B.err)/4;
scrYratio=abs(PID_B.err)/40;
#endif
//............................................................................
damp=0; // damp the integral memory
starttime= CurrentTick();
// appoach target
_Bstart:
PID_B.runstate = 0x10; // run state: RUNNING
do {
dtime = CurrentTick() - clock;
clock = CurrentTick();
runtime = clock - starttime;
tprop = dtime/20.0;
if ((PID_B.err==errorold)&& (abs(PID_B.err)>PID_B.precis)) damp=1; // stalling
else
damp=PID_B.damp;
PID_B.integr = (damp * PID_B.integr) + PID_B.err;
if((PID_B.integr) > 3*PID_B.maxout) PID_B.integr = 3*PID_B.maxout; // cut away
else
if((PID_B.integr) <-3*PID_B.maxout) PID_B.integr =-3*PID_B.maxout;
PWMpwr= (PID_B.P*PID_B.err) + (PID_B.I*PID_B.integr)*tprop + (PID_B.D*(PID_B.err-errorold))/tprop;
if(PWMpwr > PID_B.maxout) PWMpwr= PID_B.maxout; // forward maxout
else
if(PWMpwr < -PID_B.maxout) PWMpwr= -PID_B.maxout; // reverse maxout
PID_B.cspeed= (PID_B.read-readold)*100/dtime; // rotat speed [degrees/100ms]
aspeed = abs(PID_B.cspeed) ;
if (aspeed > PID_B.tarpwm) {
PWMpwr = sign(PWMpwr)*PID_B.tarpwm;
}
PID_B.outp = round(PWMpwr);
#ifdef debug_PID_B
{
//..........................................................................
// for graph output
timex= runtime/scrXratio;
PointOut(timex,(PID_B.read-readstart)/scrYratio);
LineOut(oldtx, oldy, timex, pwm0+PID_B.cspeed*0.3);
oldtx=timex; oldy=pwm0+PID_B.cspeed*0.3;
printf1( 0,48,"%5.0f" , PID_B.cspeed);
//..........................................................................
}
#endif
//**************************************************************************
OnFwd(OUT_B, (PID_B.outp)); // action !
Wait(PID_B.regtime); // wait regulation time
//**************************************************************************
readold = PID_B.read; // save old sensor
errorold = PID_B.err; // save old error
PID_B.read = (MotorRotationCount(OUT_B)); // get new encoder value
PID_B.err = PID_B.target-PID_B.read; // new error to target
if (PID_B.read>cmax) cmax=PID_B.read; // monitor overshooting
else
if (PID_B.read<cmin) cmin=PID_B.read; // monitor overshooting
if ((PID_B.cont)&& (abs(PID_B.err)<=PID_B.precis)) PID_B.runstate = 0x60;
else PID_B.runstate = 0x20;
#ifdef debug_PID_B
//..........................................................................
printf1(63, 48,"%-6d" , cmax);
printf1(63, 40,"%-6d" , cmin);
printf1(63, 32,"%6.1f" , PWMpwr);
//..........................................................................
#endif
if (PID_B.cont) continue;
if (abs(PID_B.err)<=PID_B.precis) { regloop +=1 ; PID_B.runstate = 0x40; }
} while ((abs(PID_B.err)>=PID_B.precis) && (regloop<=5)); // target reached
Off(OUT_B); // finished - brake motor
PID_B.runstate = 0x40; // run state: RAMPDOWN
PID_B.outp=0;
Wait(50);
PID_B.read = MotorRotationCount(OUT_B);
regloop=1;
if (PID_B.read>cmax) cmax=PID_B.read; // detect overshooting
if (PID_B.read<cmin) cmin=PID_B.read; // detect overshooting
PID_B.err = PID_B.target-PID_B.read;
#ifdef debug_PID_B
printf1(63, 48,"%-6d" , cmax);
printf1(63, 40,"%-6d" , cmin);
printf1(63, 32,"%6.1f" , PWMpwr);
#endif
if ((abs(PID_B.err)>PID_B.precis)) {goto _Bstart;}
#ifdef debug_PID_B
//............................................................................
PointOut(timex,PID_B.read/scrYratio);
LineOut(oldtx, oldy, timex, pwm0);
LineOut(timex+2,PID_B.target/scrYratio, timex+10, PID_B.target/scrYratio);
LineOut(timex+2, pwm0, timex+10, pwm0);
//............................................................................
#endif
PID_B.runstate=0;
Wait(1); //runstate = IDLE
}
//==============================================================================
task task_PID_C() {
float aspeed, damp, PWMpwr, readold, errorold, tprop;
long readstart, cmax, cmin; // for monitoring
long starttime, runtime, clock, dtime; // timer
char regloop;
PID_C.runstate = 0x10; // reg state: RAMPUP
PID_C.read = (MotorRotationCount(OUT_C)); // get current encoder reading
PID_C.err = PID_C.target - PID_C.read; // error to target
readstart = PID_C.read;
regloop = 1;
//............................................................................
// init variables for graph output
#ifdef debug_PID_C
ClearScreen();
DisplayMask();
int timex, oldtx, oldy=15, pwm0=15; // values for graphic screen
float scrXratio, scrYratio;
scrXratio=abs(PID_C.err)/8;
if(PID_C.target<50) scrXratio=abs(PID_C.err)/4;
scrYratio=abs(PID_C.err)/40;
#endif
//............................................................................
damp=0; // damp the integral memory
starttime= CurrentTick();
// appoach target
_Cstart:
PID_C.runstate = 0x10; // run state: RUNNING
do {
dtime = CurrentTick() - clock;
clock = CurrentTick();
runtime = clock - starttime;
tprop = dtime/20.0;
if ((PID_C.err==errorold)&& (abs(PID_C.err)>PID_C.precis)) damp=1; // stalling
else
damp=PID_C.damp;
PID_C.integr = (damp * PID_C.integr) + PID_C.err;
if((PID_C.integr) > 3*PID_C.maxout) PID_C.integr = 3*PID_C.maxout; // cut away
else
if((PID_C.integr) <-3*PID_C.maxout) PID_C.integr =-3*PID_C.maxout;
PWMpwr= (PID_C.P*PID_C.err) + (PID_C.I*PID_C.integr)*tprop + (PID_C.D*(PID_C.err-errorold))/tprop;
if(PWMpwr > PID_C.maxout) PWMpwr= PID_C.maxout; // forward maxout
else
if(PWMpwr < -PID_C.maxout) PWMpwr= -PID_C.maxout; // reverse maxout
PID_C.cspeed= (PID_C.read-readold)*100/dtime; // rotat speed [degrees/100ms]
aspeed = abs(PID_C.cspeed) ;
if (aspeed > PID_C.tarpwm) {
PWMpwr = sign(PWMpwr)*PID_C.tarpwm;
}
PID_C.outp = round(PWMpwr);
#ifdef debug_PID_C
{
//..........................................................................
// for graph output
timex= runtime/scrXratio;
PointOut(timex,(PID_C.read-readstart)/scrYratio);
LineOut(oldtx, oldy, timex, pwm0+PID_C.cspeed*0.3);
oldtx=timex; oldy=pwm0+PID_C.cspeed*0.3;
printf1( 0,48,"%5.0f" , PID_C.cspeed);
//..........................................................................
}
#endif
//**************************************************************************
OnFwd(OUT_C, (PID_C.outp)); // action !
Wait(PID_C.regtime); // wait regulation time
//**************************************************************************
readold = PID_C.read; // save old sensor
errorold = PID_C.err; // save old error
PID_C.read = (MotorRotationCount(OUT_C)); // get new encoder value
PID_C.err = PID_C.target-PID_C.read; // new error to target
if (PID_C.read>cmax) cmax=PID_C.read; // monitor overshooting
else
if (PID_C.read<cmin) cmin=PID_C.read; // monitor overshooting
if ((PID_C.cont)&& (abs(PID_C.err)<=PID_C.precis)) PID_C.runstate = 0x60;
else PID_C.runstate = 0x20;
#ifdef debug_PID_C
//..........................................................................
printf1(63, 48,"%-6d" , cmax);
printf1(63, 40,"%-6d" , cmin);
printf1(63, 32,"%6.1f" , PWMpwr);
//..........................................................................
#endif
if (PID_C.cont) continue;
if (abs(PID_C.err)<=PID_C.precis) { regloop +=1 ; PID_C.runstate = 0x40; }
} while ((abs(PID_C.err)>=PID_C.precis) && (regloop<=5)); // target reached
Off(OUT_C); // finished - brake motor
PID_C.runstate = 0x40; // run state: RAMPDOWN
PID_C.outp=0;
Wait(50);
PID_C.read = MotorRotationCount(OUT_C);
regloop=1;
if (PID_C.read>cmax) cmax=PID_C.read; // detect overshooting
if (PID_C.read<cmin) cmin=PID_C.read; // detect overshooting
PID_C.err = PID_C.target-PID_C.read;
#ifdef debug_PID_C
printf1(63, 48,"%-6d" , cmax);
printf1(63, 40,"%-6d" , cmin);
printf1(63, 32,"%6.1f" , PWMpwr);
#endif
if ((abs(PID_C.err)>PID_C.precis)) {goto _Cstart;}
#ifdef debug_PID_C
//............................................................................
PointOut(timex,PID_C.read/scrYratio);
LineOut(oldtx, oldy, timex, pwm0);
LineOut(timex+2,PID_C.target/scrYratio, timex+10, PID_C.target/scrYratio);
LineOut(timex+2, pwm0, timex+10, pwm0);
//............................................................................
#endif
PID_C.runstate=0;
Wait(1); //runstate = IDLE
}
//==============================================================================
safecall void RotatePID(char port, long Target, float RotatSpeed, char cont) {
if (port==OUT_A) {
if ((PID_A.runstate!=0)|| (PID_A.cont)) { // stop PID task if already running
stop task_PID_A;
PlayTone(TONE_C7,100);
Off(OUT_A);
Wait(50);
}
PID_A.runstate=1; // set runstate: PID active
// custom init PID_A
PID_A.target =Target; // assign target
PID_A.tarpwm =RotatSpeed; // assign rotation speed
PID_A.cont=cont; // cont vs. hit once
// Reset PID control defaults
PID_A.outp =0; // PID control output value
PID_A.maxout =100; // absolute max possible output (max pwr)
PID_A.read =0; // current reading
PID_A.err =0; // current error
PID_A.integr =0; // integral of errors
PID_A.cspeed =0; // current speed
start task_PID_A;
}
else
if (port==OUT_B) {
if ((PID_B.runstate!=0)|| (PID_B.cont)) { // stop PID task if already running
stop task_PID_B;
PlayTone(TONE_C7,100);
Off(OUT_B);
Wait(50);
}
PID_B.runstate=1; // set runstate: PID active
// custom init PID_B
PID_B.target =Target; // assign target
PID_B.tarpwm =RotatSpeed; // assign rotation speed
PID_B.cont=cont; // cont vs. hit once
// Reset PID control defaults
PID_B.outp =0; // PID control output value
PID_B.maxout =100; // absolute max possible output (max pwr)
PID_B.read =0; // current reading
PID_B.err =0; // current error
PID_B.integr =0; // integral of errors
PID_B.cspeed =0; // current speed
Wait(1);
start task_PID_B;
}
else
if (port==OUT_C) {
if ((PID_C.runstate!=0)|| (PID_C.cont)) { // stop PID task if already running
stop task_PID_C;
PlayTone(TONE_C7,100);
Off(OUT_C);
Wait(50);
}
PID_C.runstate=1; // set runstate: PID active
// custom init PID_C
PID_C.target =Target; // assign target
PID_C.tarpwm =RotatSpeed; // assign rotation speed
PID_A.cont=cont; // cont vs. hit once
// Reset PID control defaults
PID_C.outp =0; // PID control output value
PID_C.maxout =100; // absolute max possible output (max pwr)
PID_C.read =0; // current reading
PID_C.err =0; // current error
PID_C.integr =0; // integral of errors
PID_C.cspeed =0; // current speed
Wait(1);
start task_PID_C;
}
}
//==============================================================================
inline void RotatePIDtoTarget(char port, long Target, float RotatSpeed) {
RotatePID(port, Target, RotatSpeed, false);
Wait(1);
}
inline void RotatePIDcont(char port, long Target, float RotatSpeed) {
RotatePID(port, Target, RotatSpeed, true);
Wait(1);
}
inline void RotatePIDdegrees(char port, long Target, float RotatSpeed) {
RotatePID(port, Target+MotorRotationCount(port), RotatSpeed, false);
Wait(1);
}
safecall void StopPIDcontrol (char port) {
if (port==OUT_A) {
if (PID_A.runstate!=0) { // stop PID task if already running
stop task_PID_A;
PlayTone(TONE_C7,100);
Off(OUT_A);
Wait(50);
PID_A.cont=false;
PID_A.runstate=0;
Wait(1);
return;
}
}
else
if (port==OUT_B) {
if (PID_B.runstate!=0) { // stop PID task if already running
stop task_PID_B;
PlayTone(TONE_C7,100);
Off(OUT_B);
Wait(50);
PID_B.cont=false;
PID_B.runstate=0;
Wait(1);
return;
}
}
else
if (port==OUT_C) {
if (PID_C.runstate!=0) { // stop PID task if already running
stop task_PID_C;
PlayTone(TONE_C7,100);
Off(OUT_C);
Wait(50);
PID_C.cont=false;
PID_C.runstate=0;
Wait(1);
return;
}
}
}
//==============================================================================
void SetPIDparamEx(char port,float P,float I,float D,float prec,int rtime, float damp) { // custom PID parameters
if (port==OUT_A) {
PID_A.P =P; // P: propotional to error
PID_A.I =I; // I: avoid perish
PID_A.D =D; // D: derivative: avoid oscillating
PID_A.precis =prec; // error precision to target
PID_A.regtime =rtime; // PID regulation time
PID_A.damp =damp; // damp error integral
} else
if (port==OUT_B) {
PID_B.P =P; // P: propotional to error
PID_B.I =I; // I: avoid perish
PID_B.D =D; // D: derivative: avoid oscillating
PID_B.precis =prec; // error precision to target
PID_B.regtime=rtime; // PID regulation time
PID_B.damp =damp; // damp error integral
} else
if (port==OUT_C) {
PID_C.P =P; // P: propotional to error
PID_C.I =I; // I: integral: avoid perish
PID_C.D =D; // D: derivative: avoid oscillating
PID_C.precis =prec; // error precision to target
PID_C.regtime=rtime; // PID regulation time
PID_C.damp =damp; // damp error integral
}
}
//==============================================================================
void SetPIDparam(char port,float P,float I,float D) { // custom PID parameters
if (port==OUT_A) {
PID_A.P =P; // P: propotional to error
PID_A.I =I; // I: integral: avoid perish
PID_A.D =D; // D: derivative: avoid oscillating
} else
if (port==OUT_B) {
PID_B.P =P; // P: propotional to error
PID_B.I =I; // I: integral: avoid perish
PID_B.D =D; // D: derivative: avoid oscillating
} else
if (port==OUT_C) {
PID_C.P =P; // P: propotional to error
PID_C.I =I; // I: integral: avoid perish
PID_C.D =D; // D: derivative: avoid oscillating
}
}
//==============================================================================
void PIDinit() {
for (char port=0; port<3; ++port) {
SetPIDparamEx(port,0.40, 0.40, 10, 1, 5, 0.75); // p,i,d, precis, reg_time, damp
Wait(1);
}
}
//==============================================================================
//..........................................................................
// display, monitor, and debug
void DisplayMask() {
printf1(36, 48,"max","" ); // max value
printf1(36, 40,"min",""); // min value
printf1(36, 32,"out" ,""); // last pwm out
}
task Displayenc(){
while(1) {
printf1( 0,56,"run %-2d", PID_A.runstate); // run state
printf1(49,56,"%-2d", PID_B.runstate);
printf1(77,56,"%-2d", PID_C.runstate);
printf1(21,24,"%-6d", MotorRotationCount(OUT_A)); // sensor read
printf1(49,24,"%-6d", MotorRotationCount(OUT_B));
printf1(77,24,"%-6d", MotorRotationCount(OUT_C));
printf1(21, 0,"%-6d", PID_A.target); // sensor destination
printf1(49, 0,"%-6d", PID_B.target);
printf1(77, 0,"%-6d", PID_C.target);
Wait(50);
}
}
//..........................................................................
task main() {
long Target; // custom values
int RotatSpeed;
PIDinit();
#ifdef ( debug_PID_A || debug_PID_B || debug_PID_C )
start Displayenc;
#endif
while(true) {
Target = 200; // set new motor rotation target
RotatSpeed = 80; // set max rotation speed
SetPIDparam(OUT_A, 0.4, 0.8, 12 ); // p,i,d
RotatePIDdegrees(OUT_A, Target, RotatSpeed);
SetPIDparam(OUT_B, 0.6, 0.5, 10); // p,i,d
RotatePIDdegrees(OUT_B, Target, RotatSpeed);
SetPIDparam(OUT_C, 1.0, 2.0, 30); // p,i,d
RotatePIDdegrees(OUT_C, Target, RotatSpeed);
while ((PID_A.runstate!=0)||(PID_B.runstate!=0)||(PID_C.runstate!=0));
PlayTone(TONE_C4,100); Wait(100);
printf1(0,48,"%s","OK");
while (BTNCENTER!= getchar()); // wait for BtnCtr to cont
}
}
