NXC: custom PID control for NXT?

[HaWe]

I'm looking for custom PID functions written in NXC which could be included into own robot projects, because the Lego firmware PID control doesn't work accurately enough and reacts sometimes unexplainably unpredictable.

I found some code in the www ( http://libnxter.sourceforge.net/_p_i_d_8nxc.html ) but I'm not clear about how to use it:

Code: Select all

/*
PID.nxc
Go to the documentation of this file 
http://libnxter.sourceforge.net/_p_i_d_8nxc.html
*/
00001 /* -*- Mode: C; indent-tabs-mode: t; c-basic-offset: 4; tab-width: 4 -*- */
00002 /*
00003     PID.nxc
00004     Copyright (C) 2008 Naba Kumar   <[email protected]>
00005 
00006     This program is free software; you can redistribute it and/or modify
00007     it under the terms of the GNU General Public License as published by
00008     the Free Software Foundation; either version 2 of the License, or
00009     (at your option) any later version.
00010 
00011     This program is distributed in the hope that it will be useful,
00012     but WITHOUT ANY WARRANTY; without even the implied warranty of
00013     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014     GNU General Public License for more details.
00015 
00016     You should have received a copy of the GNU General Public License
00017     along with this program; if not, write to the Free Software
00018     Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00019 */
00020 
00021 #ifndef _PID_CONTROLLER_H_
00022 #define _PID_CONTROLLER_H_
00023 
00032 struct PIDControl
00033 {
00034     long PIDScale;                   
00035     long PValue;                     
00036     long IValue;                     
00037     long DValue;                     
00038     long setPoint;                   
00039     int steadyStateCountThreshold;   
00043     /* Output parameters */
00044     long outputGain;                  
00045     long absMaxOutput;                
00046     long absIntegralLimit;            
00048     /* State */                       
00049     long integral;                    
00050     long lastError;                   
00051     long steadyStateCount;
00052 };
00053 
00071 void PIDControlInit(PIDControl &pidControl, long PIDScale,
00072                     long PValue, long IValue, long DValue,
00073                     int steadyStateCountThreshold,
00074                     long outputGain, long absMaxOutput)
00075 {
00076     pidControl.PIDScale = PIDScale;
00077     pidControl.PValue = PValue;
00078     pidControl.IValue = IValue;
00079     pidControl.DValue = DValue;
00080     pidControl.steadyStateCountThreshold = steadyStateCountThreshold;
00081     pidControl.outputGain = outputGain;
00082     pidControl.absMaxOutput = Abs(absMaxOutput);
00083     pidControl.absIntegralLimit = pidControl.absMaxOutput;
00084     pidControl.setPoint = 0;
00085     pidControl.integral = 0;
00086     pidControl.lastError = 0;
00087     pidControl.steadyStateCount = 0;
00088 }
00089 
00094 void PIDControlSetIntegralLimit(PIDControl &pidControl, long absIntegralLimit)
00095 {
00096     pidControl.absIntegralLimit = Abs(absIntegralLimit);
00097 }
00098 
00103 void PIDControlSetPoint(PIDControl &pidControl, long setPoint)
00104 {
00105     pidControl.setPoint = setPoint;
00106     pidControl.integral = 0;
00107     pidControl.steadyStateCount = 0;
00108 }
00109 
00120 bool PIDControlCheckEnd(PIDControl &pidControl)
00121 {
00122     if (pidControl.steadyStateCount > pidControl.steadyStateCountThreshold)
00123         return true;
00124     else
00125         return false;
00126 }
00127 
00131 long PIDControlGetLastError(PIDControl &pidControl)
00132 {
00133     return pidControl.lastError;
00134 }
00135 
00142 long PIDControlStep(PIDControl &pidControl, long currentPoint)
00143 {
00144     long P, I, D, error, output;
00145     
00146     if (PIDControlCheckEnd(pidControl))
00147         return 0;
00148 
00149     error = pidControl.setPoint - currentPoint;
00150 
00151     /* If error hasn't changed for last steadyStateCountThreshold samples, end PID control */
00152     if (pidControl.steadyStateCountThreshold > 0 && (error - pidControl.lastError) == 0)
00153         pidControl.steadyStateCount++;
00154 
00155     P = ((pidControl.PValue * error) / pidControl.PIDScale);
00156     I = pidControl.integral + ((pidControl.IValue * error) / pidControl.PIDScale);
00157     D = ((pidControl.DValue) * (error - pidControl.lastError)) / pidControl.PIDScale;
00158 
00159     /* Limit integral output */
00160     if (I > (pidControl.absIntegralLimit/pidControl.outputGain))
00161        I = pidControl.absIntegralLimit;
00162     if (I < -(pidControl.absIntegralLimit/pidControl.outputGain))
00163        I = -pidControl.absIntegralLimit;
00164 
00165     output = pidControl.outputGain * (P + I + D);
00166     if (output > pidControl.absMaxOutput) output = pidControl.absMaxOutput;
00167     if (output < -pidControl.absMaxOutput) output = -pidControl.absMaxOutput;
00168 
00169     pidControl.integral = I;
00170     pidControl.lastError = error;
00171     
00172     return output;
00173 }
00174 
00175 #endif
00176 

// Generated on Sat Jun 6 12:50:27 2009 for libnxter by  doxygen 1.5.6

Any other 100% NXC-compatible code is appreciated!

ps
does anybody know how to erase the line numbers?

[mattallen37]

Have you tried absolute position regulation? It seems to be good to within several degrees with a load. I did write a PID motor position controller, but only because I wanted to be able to use feedback besides encoders, and output other than the motor ports.

I can't test this now, but I think it works fine:

Code: Select all

float error;
float integral = 0;
float lastError;
float derivative;

int PID_Motor_Position(long Target_Position, long Current_Position, float K_P=3, float K_I=0.1, float K_D=3){ //3,3 for NXT motors, 20,5 for RCX

  error = Target_Position - Current_Position;
  integral = integral*(0.75) + error;
  derivative = error - lastError;
  lastError = error;
  return Clip(K_P * error + K_I * integral + K_D * derivative, -100, 100);
}

As you can see, I didn't really use integral, so it's basically just a PD controller.

Here is an example program as I left it 6 months ago:

Code: Select all

#define Clip(in,min,max) (in<min?min:(in>max?max:in))

float error;
float integral = 0;
float lastError;
float derivative;

int PID_Motor_Position(long Target_Position, long Current_Position, float K_P=3, float K_I=0.1, float K_D=3){ //3,3 for NXT motors, 20,5 for RCX

  error = Target_Position - Current_Position;
  integral = integral*(0.75) + error;
  derivative = error - lastError;
  lastError = error;
  return Clip(K_P * error + K_I * integral + K_D * derivative, -100, 100);
}

long DesiredPosition;
int speed;
task main ()
{
  SetSensor(S1, SENSOR_ROTATION);
  SetSensor(S2, SENSOR_ROTATION);
  while (true){
    ClearScreen();

    DesiredPosition = SENSOR_2;//MotorTachoCount (OUT_B)/22;
    
    speed = PID_Motor_Position(DesiredPosition, SENSOR_1, 20, 0, 5)*(-1);

    OnFwdEx(OUT_A, speed, 0);   //Don't reset the TachoCount

    NumOut(0, LCD_LINE1, DesiredPosition);
    NumOut(0, LCD_LINE2, SENSOR_1);
    NumOut(0, LCD_LINE3, error);
    NumOut(0, LCD_LINE4, integral);
    NumOut(0, LCD_LINE5, derivative);

    NumOut(0, LCD_LINE7, speed);
    
    Wait(50);
  }
}

[HaWe]

yes, I tried absolute pos reg, but it doesn't work either (see different thread https://sourceforge.net/apps/phpbb/mind ... =30#p11515).

How does your code work for 3 different motors with 3 different targets ?
(edited)

[mattallen37]

I guess you could do something like this:

Code: Select all

#define Clip(in,min,max) (in<min?min:(in>max?max:in))

int PID_Motor_Position_A(long Target_Position, long Current_Position, float K_P=3, float K_I=0.1, float K_D=3){ //3,3 for NXT motors, 20,5 for RCX
  float error;
  static float integral = 0;
  static float lastError;
  float derivative;

  error = Target_Position - Current_Position;
  integral = integral*(0.75) + error;
  derivative = error - lastError;
  lastError = error;
  return Clip(K_P * error + K_I * integral + K_D * derivative, -100, 100);
}

int PID_Motor_Position_B(long Target_Position, long Current_Position, float K_P=3, float K_I=0.1, float K_D=3){ //3,3 for NXT motors, 20,5 for RCX
  float error;
  static float integral = 0;
  static float lastError;
  float derivative;

  error = Target_Position - Current_Position;
  integral = integral*(0.75) + error;
  derivative = error - lastError;
  lastError = error;
  return Clip(K_P * error + K_I * integral + K_D * derivative, -100, 100);
}

int PID_Motor_Position_C(long Target_Position, long Current_Position, float K_P=3, float K_I=0.1, float K_D=3){ //3,3 for NXT motors, 20,5 for RCX
  float error;
  static float integral = 0;
  static float lastError;
  float derivative;

  error = Target_Position - Current_Position;
  integral = integral*(0.75) + error;
  derivative = error - lastError;
  lastError = error;
  return Clip(K_P * error + K_I * integral + K_D * derivative, -100, 100);
}

long A_Target_Position, B_Target_Position, C_Target_Position;

task main(){
  while(true){
    OnFwdEx(OUT_A, PID_Motor_Position_A(A_Target_Position, MotorTachoCount (OUT_A)), 0);
    OnFwdEx(OUT_B, PID_Motor_Position_B(B_Target_Position, MotorTachoCount (OUT_B)), 0);
    OnFwdEx(OUT_C, PID_Motor_Position_C(C_Target_Position, MotorTachoCount (OUT_C)), 0);
  }
}

If you need to reverse it, you could either do -A_Target_Position instead of A_Target_Position, or you could invert the TachoCount, and use OnRevEx instead of OnFwdEx.

I am not testing this as I go, so it's all still just theory.

Since all of what you are doing is specifically with NXT motors and encoders, you could do something more like this instead:

Code: Select all

#define Clip(in,min,max) (in<min?min:(in>max?max:in))

float integral[3];
float lastError[3];
float error;
float derivative;
  
void PID_Motor_Position(byte port, long Target_Position, float K_P=3, float K_I=0.1, float K_D=3){
  byte ArrayPointer;
  if(port == OUT_A)ArrayPointer = 0;
  else if(port == OUT_B)ArrayPointer = 1;
  else if(port == OUT_C)ArrayPointer = 2;

  error = Target_Position - MotorTachoCount(port);
  integral[ArrayPointer] = integral[ArrayPointer] *(0.75) + error;
  derivative = error - lastError[ArrayPointer];
  lastError[ArrayPointer] = error;
  OnFwdEx(port, Clip(K_P * error + K_I * integral[ArrayPointer] + K_D * derivative, -100, 100), 0);
}

long A_Target_Position, B_Target_Position, C_Target_Position;

task main(){
  while(true){
    PID_Motor_Position(OUT_A, A_Target_Position);
    PID_Motor_Position(OUT_B, B_Target_Position);
    PID_Motor_Position(OUT_C, C_Target_Position);
    Wait(10);
  }
}

[HaWe]

motor tacho count does not work with my assembly (see different thread: https://sourceforge.net/apps/phpbb/mind ... 762#p11762)
- do you have any suspect that it shouldn't work with motor rotation count as well?

- and - all positionings of all motors must run simultaneously (multithreaded).

do you have any knowledge of the accuracy (it may oscillate at the end, but the error must remain < 5 degrees) ?

so all what I actually need is a substitute for RotateMotorPID in this task:

Code: Select all

long A1target;
char A1rdy;
//------------------------------------------------------------------------------
task  RotateToTargetA1() {
//------------------------------------------------------------------------------
  char pwr=-100;
  A1rdy=0; Wait(1);

  RotateMotorPID(A1, pwr,  MEnc(A1)-A1target, 32, 64, 96);

  Off(A1);   Wait(50);
  A1rdy= true;
}

MEnc := MotorRotationCount,
A1 := OUT_B

[mattallen37]

What do you mean it doesn't work? I doesn't really matter what encoder counter you use, just do not let it get reset during operation. In fact, you could even use the HT rotation sensor for all I care... that's why I built my own PID control loop (to allow for any input, and any output).

If it needs to be able to control all three motors, and not rely on timing of the PID calls from other tasks to the other motors, then look at the first example of my last post.

Additionally, why can't you use a mutex to protect the single function? The function call should take a very short amount of time, so sharing it with other tasks shouldn't slow you down that much.

As far as the accuracy of position, that is controlled by the P Konstant. The oscillation is somewhat controlled by the D Konstant. You would need to tune them to your likeness.

[HaWe]

did you read what I wrote what happened when I was using "tacho" instead of "rotation"? I suspect / don't trust this "tacho" thing (probably too many side effects), I always use "rotation"...

[mattallen37]

Well, I don't care what counter in the IO map you read the encoder values from, and neither does the PID control loop, just as long as you don't reset the value or anything like that (which is why I used OnFwdEx, so that I could choose to not reset the encoder).

[HaWe]

maybe that's the point:
I do need to reset the counters during runtime (maybe several times intermediately) and I do need to recalibrate the targets during runtime manually.

[mattallen37]

Well then, use one that you can reset, I don't care. Just don't let it get reset when it doesn't need to be (like in the middle of a run). Either that, or use an offset instead.