NXC: PID motor controller (approach encoder/sensor target)

Discussion specific to NXT-G, NXC, NBC, RobotC, Lejos, and more.

NXC: PID motor controller (approach encoder/sensor target)

Postby HaWe » 02 Jun 2013, 11:49

the latest version: a NXC PID motor controller (approach to encoder or sensor target).

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,d


Adjustments for OUT_B und OUT_C are different, for experimenting.
Code: Select all
SetPIDparam(OUT_B, 0.4, 0.5, 12);    // p,i,d
SetPIDparam(OUT_C, 0.4, 0.4, 10);    // p,i,d


Use of the PID control:
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
///////////////////////////////////////////////////////////////////////////////////////



The following code is featuring a debug mode to display the motor control as a graph on the screen (for tuning and fine-tuning).
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 output



p,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.php?f=25&t=7521#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

  }

}
Last edited by HaWe on 10 Apr 2014, 21:34, edited 7 times in total.
User avatar
HaWe
 
Posts: 2500
Joined: 04 Nov 2014, 19:00

Re: NXC: PID motor controller (approach encoder/sensor target)

Postby afanofosc » 04 Jun 2013, 14:49

Thank you for sharing this code! I appreciate your contributions to the MIINDSTORMS community and for your willingness to use NXC and BricxCC in spite of their imperfections. :-)

John Hansen
Multi-platform LEGO MINDSTORMS programming
http://bricxcc.sourceforge.net/
User avatar
afanofosc
Site Admin
 
Posts: 1256
Joined: 26 Sep 2010, 19:36
Location: Nashville, TN

Re: NXC: PID motor controller (approach encoder/sensor target)

Postby HaWe » 04 Jun 2013, 15:04

well, I do my very best -
and for me and my skills BCC/NXC is still the best choice 8-)
User avatar
HaWe
 
Posts: 2500
Joined: 04 Nov 2014, 19:00

Re: NXC: PID motor controller (approach encoder/sensor targe

Postby HaWe » 30 Sep 2016, 19:35

some screenshots about good and bad PID-tuning:

bad tuning:
PID_bad.png
PID_bad.png (21.88 KiB) Viewed 4995 times


good tuning:
PID_good.png
PID_good.png (1.2 KiB) Viewed 4995 times



latest update:
porting the NXT PID control to C/C++ code for a Raspberry Pi to control NXT and EV3 encoder motors (servo motors):
http://www.mindstormsforum.de/viewtopic ... 089#p69089
User avatar
HaWe
 
Posts: 2500
Joined: 04 Nov 2014, 19:00


Return to Mindstorms Software

Who is online

Users browsing this forum: No registered users and 2 guests

cron