Generic PID Controller class

[kirkpthompson]

Here is a PID controller class implementation that supports output ramping, deadband, and high/low output limiting:

Code: Select all

package net.mosen.nxt;

/**
 *  Proportional <Kp>, Integral <Ki>, Derivitive <Kd> controller implementation.
 * 
 *  Kp depends on the present error, Ki on the accumulation of past errors, and Kd is a prediction of future errors, based on
 *  current rate of change.
 * <p> <b>Proportional gain, Kp:</b>
 * Larger values typically mean faster response since the larger the error, the larger the proportional term compensation.
 * An excessively large proportional gain will lead to process instability and oscillation.<br>
 * <p><b>Integral gain, Ki:</b>
 * Larger values imply steady state errors are eliminated more quickly. The trade-off is larger overshoot: any negative error integrated during transient response must be integrated away by positive error before reaching steady state.
 * <p><b>Derivative gain, Kd:</b>
 * Larger values decrease overshoot, but slow down transient response and may lead to instability due to signal noise amplification in the differentiation of the error.
 *  <p><b>Definitions:</b>
 *  <ul>
 *  <li>MV - Manipulated Variable. What the PID contoller calculates to be used as the input to the process (i.e. motor speed).
 *  <LI>PV - Process Variable. The measurement of the process value.
 *  <LI>SP - Setpoint. The desired value that the PID contoller works to.
 *  <li>e - Error. The difference between PV and MV
 *  </ul>
 *
 *  The proportional term Kp (sometimes called gain) makes a change to the output that is proportional to the
 *  current error value. The proportional response is adjusted by multiplying the error by Kp. A high proportional gain
 *  results in a large change in the output for a given change in the error.
 *  <P>
 *  The integral term Ki (sometimes called reset) accelerates the movement of the process towards the setpoint and eliminates the residual
 *  steady-state error that
 *  occurs with a proportional only controller. However, since the integral term is responding to accumulated errors from the past,
 *  it can cause the present value to overshoot the setpoint value.
 * <p>
 * The derivative term Kd (sometimes called rate) slows the rate of change of the controller output and this effect is most noticeable
 * close to the controller setpoint. Hence, derivative control is used to reduce the magnitude of the overshoot produced by the
 * integral component (Ki) and improve the combined controller-process stability.
 *  <p>
 *  It is important to tune the PID controller with an implementation of a consistent delay between calls to <code>doPID()</code>
 *  because the MV calc in a PID controller is time-dependent by definition. The implementation calculates the time delta between
 *  calls to <code>{@link #doPID}</code>.
 * 
 *  @auther Kirk Thompson, 2/5/2011 lejos[AT]mosen[dot]net
 */
public class PIDController {
    /**
     * Proportional term ID
     * @see #setPIDParam
     * @see #getPIDParam
     */
    public static final int PID_KP = 0;
    /**
     * Integral term ID
     *
     * @see #setPIDParam
     * @see #getPIDParam
     */
    public static final int PID_KI = 1;
    /**
     * Derivitive term ID
     * @see #setPIDParam
     * @see #getPIDParam
     */
    public static final int PID_KD = 2;
    /**
     * The Ramping Exponential value ID. Used for output ramping which determines ramp shape. 1.0=linear, Set to 0 to disable
     * output ramping. Larger values create steeper ramping curves.
     * steeper ramps.
     * @see #setPIDParam
     * @see #getPIDParam
     */
    public static final int PID_RAMP_POWER = 3;
    /**
     * The Ramping Threshold value ID. Used for output ramping. When the PID Manipulated Variable (MV) is within this range (-+), output ramping is
     * applied to MV before it is returned from <code>{@link #doPID}</code>.
     * @see #setPIDParam
     * @see #getPIDParam
     */
    public static final int PID_RAMP_THRESHOLD = 4;
    /**
     * The deadband value ID. Used for output clipping. if MV within +- this range relative to zero, MV of zero is returned.
     * Set to zero to effectively disable. This is useful to avoid hunting arount the SP when there is a lot
     * of slop in whatever the controller is controlling i.e. gear & link lash.
     * @see #setPIDParam
     * @see #getPIDParam
     */
    public static final int PID_DEADBAND = 5;
   
    /**
     *     The high limit cutoff value ID. Use for  high limit cutoff for Manipulated Variable (MV). Set to a large value to
     *     effectively disable. This is applied to MV before any ramping. Default is 900.
     *  at instantiation.
     * @see #setPIDParam
     * @see #getPIDParam
     */
    public static final int PID_LIMITHIGH = 6;
   
    /**
     * The low limit cutoff value ID. Use for  low limit cutoff for Manipulated Variable (MV). Set to a large negative value to
     *     effectively disable. This is applied to MV before any ramping. Default is -900.
     * @see #setPIDParam
     * @see #getPIDParam
     */
    public static final int PID_LIMITLOW = 7;
   
    /**
     * The Setpoint value ID. This is the value the PID controller works toward.
     * @see #setPIDParam
     * @see #getPIDParam
     */
    public static final int PID_SETPOINT = 8;

    //private BTLogger dataLogger = null;     // the datalogger instance that may be registered
    //private boolean hasDataLogger = false;  // If a datalogger has been registered, this is set to true
    // Our default Constants for the PID controller
    private float Kp=1.8f;          // proportional value determines the reaction to the current error
    private float Ki=0.00001f;      // integral value determines the reaction based on the sum of recent errors
    private float Kd=150.0f;        // derivative value determines the reaction based on the rate at which the error has been changing
    private int highLimit = 900;            // assuming control of motor speed and thereby max would be 900 deg/sec
    private int lowLimit = -highLimit;
    private int previous_error = 0;
    private int deadband = 0;
     
    private long logBeginTime;              // baseline for logging absolute time
    private int dt = 0;                     // cycle time, ms
    private long cycleTime=0;               // used to calc the time between each call (dt) to doPID()
    private int setpoint;                   // The setpoint to strive for
    private int error;                      // proportional term
    private int integral = 0;               // integral term
    private float derivative;               // derivitive term
   
    private float power = 0;
    private int rampThresold = 0;
    private double rampExtent = 1;
   
    /**
     * construct a PID controller instance using passed setpoint (SP)
     * @param setpoint The goal of the MV
     */
    public PIDController(int setpoint) {
        logBeginTime = System.currentTimeMillis();
        this.setpoint = setpoint;
    }
   
    /**
     * Set PID controller parameters.
     * @param paramID What parameter to set. See the constant definitions for this class.
     * @param value The value to set it to
     * @see #getPIDParam
     */
    public void setPIDParam(int paramID, float value) {
        switch (paramID) {
            case PIDController.PID_KP:
                this.Kp = value;
                break;
            case PIDController.PID_KI:
                this.Ki = value;
                break;
            case PIDController.PID_KD:
                this.Kd = value;
                break;
            case PIDController.PID_RAMP_POWER:
                this.power = value;
                rampExtent = Math.pow(this.rampThresold, this.power);
                break;
            case PIDController.PID_RAMP_THRESHOLD:
                this.rampThresold = (int)value;
                if (this.rampThresold==0) break;
                rampExtent = Math.pow(this.rampThresold, this.power);
                break;
            case PIDController.PID_DEADBAND:
                this.deadband = (int)value;
                break;
            case PIDController.PID_LIMITHIGH:
                this.highLimit = (int)value;
                break;
            case PIDController.PID_LIMITLOW:
                this.lowLimit = (int)value;
                break;
            case PIDController.PID_SETPOINT:
                this.setpoint = (int)value;
                cycleTime = 0;
                integral = 0;
                break;
            default:
        }
    }

    /** Get PID controller parameters.
     * @param paramID What parameter to get. See the constant definitions for this class.
     * @return The requested parameter value
     *  @see #setPIDParam
     */
    public float getPIDParam(int paramID) {
        float retval =0.0f;
        switch (paramID) {
            case PIDController.PID_KP:
                retval=this.Kp;
                break;
            case PIDController.PID_KI:
                retval=this.Ki;
                break;
            case PIDController.PID_KD:
                retval=this.Kd;
                break;
            case PIDController.PID_RAMP_POWER:
                retval=this.power;
                break;
            case PIDController.PID_RAMP_THRESHOLD:
                retval=this.rampThresold;
                break;
            case PIDController.PID_DEADBAND:
                retval = this.deadband;
                break;
            case PIDController.PID_LIMITHIGH:
                retval = this.highLimit;
                break;
            case PIDController.PID_LIMITLOW:
                retval = this.lowLimit;
                break;
            case PIDController.PID_SETPOINT:
                retval = this.setpoint;
                break;
            default:
        }
        return retval;
    }
   
    /**
     * Do the PID calc for a single iteration. Your implementation must provide the delay between calls to this method .
     * @param processVariable The PV value from the process (sensor reading, etc.).
     * @return The output Manipulated Variable (MV)
     */
    public int doPID(int processVariable){
        int outputMV;
//        LCD.drawString("PV: " + processVariable + "  " , 0, 5);
//        if (true) return 0;
        if (cycleTime==0) {
            cycleTime = System.currentTimeMillis();
            return 0;
        }
        error = setpoint - processVariable;
        error = Math.abs(error)<=deadband?0:error;
        integral += error * dt; // error to time product
        derivative = ((float)(error - previous_error))/dt;
        outputMV = (int)(Kp*error + Ki*integral + Kd*derivative);
       
        if (outputMV>highLimit) outputMV=highLimit;
        if (outputMV<lowLimit) outputMV=lowLimit;
        previous_error = error;
       
        //LCD.drawString("dt=" + dt + " ", 9, 5); // TODO remove after debugging/testing

        dt = (int)(System.currentTimeMillis() - cycleTime);
       
        cycleTime = System.currentTimeMillis();
        return rampOut(outputMV);
    }
   
    private int rampOut(int ov){
        if (power==0 || rampThresold==0) return ov;
        if (Math.abs(ov)>rampThresold) return ov;
        int workingOV;
        workingOV=(int)(Math.pow(Math.abs(ov), power) / rampExtent * rampThresold);
        return (ov<0)?-1*workingOV:workingOV;
    }
}


Comments welcome. Enjoy!
-K

[bbagnall]

Hi Kirk,

Have you used this PID class in a practical project? Some code samples would be nice. I'd be curious how this could be used in the typical Segway application.

From past attempts at PID it seemed like a lot of trial and error to home in on the right values for the three constants. A mini-tutorial (in the API docs) explaining how one would arrive at each value would be useful.

[gloomyandy]

Hi Kirk,
nice code. You may want to consider adding some sort of control of integral windup over and above limiting the total output...

Andy

[kirkpthompson]

Brian: Yes, I developed it for testing the Gyro vectoring class I have been working on forever. I built a test platform that allows the brick to control brick rotation via one motor. I use the PID controller to keep the brick always pointing in the same direction. I also have used it with a compass sensor (which, with a magnet, really is funny to watch how the PID controller "reacts".)

test Platform from LDD:

[gloomyandy]

Hi Kirk,
That looks pretty cool, do you have any video of the tests you have been running?

Andy

[kirkpthompson]

Andy, Thanks. So, a limiter on the Integral accumulator would be relatively simple to implement. Can you help me understand what exactly this would do? I guess I haven't experienced integral windup issues with my testbed (or I didn't recognize them if they did happen).

Did a little research and I see there are multiple ways to address windup. I'll start experimenting. I was thinking of using a modular approach so you could "plug_in"/register your own implementation of a integral modulator but need to understand what exactly the behavoirs will be. Hmmmmm.. Thanks for the challenge!

[kirkpthompson]

re: code examples
I have built an interface that you can use to implement your main PID control functions (using PIDController) as an anonymous inner class that you pass to a PIDTunerController class instance that provides a on-brick user interface to tune the PIDController parameters. It's pretty lightweight but will still take clock cycles away so tuning a very sensitive process may be tricky (or not possible).

Code: Select all


package net.mosen.nxt;

/** Interface to allow process control implementation in <tt>{@link PIDTuner}</tt>. Used by the <tt>PIDTuner</tt> class
 * to take over/continue the
 * control of your process during term adjustment so you can see the effects of your changes
 * dynamically.
 */
public interface PIDTunerController {
    /** Implement the process variable retrieval code here. This where you read a sensor (PV) and return the result.
     * @return  The Process Variable (sensor value)
     */
    public int getPV();

    /** Implement the PID controller ref value retrieval code here.
     * @return The PID controller's reference
     */
    public PIDController getPIDController();
   
    /**
     * Implement your PID controller-supplied process control implementation here. Ensure timing dependencies are included.
     * <p>Example:<pre>
     *       public void setMV() {
     *       &nbsp;&nbsp;int MV = getMV();
     *       &nbsp;&nbsp;positioner.setSpeed(MV);
     *       &nbsp;&nbsp;if (MV<0) positioner.backward(); else if (MV>0) positioner.forward(); else positioner.flt();
     *       &nbsp;&nbsp;doWait(50);
     *       }
     *       </pre>
     */
    public void setMV();

    /** Implement your process actuator/positioner halt (turn off, float motor, pause, etc.) code here.
     */
    public void halt();
}


The PIDTuner class allows you to use the keys to select which parameter you want to adjust and change it using a increment/decrement (left/right keys) scheme. The inc/dec increment value can be cycled from 10.0 to 0.0001 with the enter key. The PIDTunerController instance you pass is used to run the process during the [dynamic/interactive] tuning of each parameter.

Code: Select all


package net.mosen.nxt;

import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Motor;

public class PIDTuner {
    private PIDController pID;
    private PIDTunerController PIDControl;
    private int incrementIndex = 0;
   
    public PIDTuner(PIDTunerController PIDControl) {
        this.pID= PIDControl.getPIDController();
        this.PIDControl = PIDControl;
    }
   
    public void keyTune(){
        int pIDParam=0;
        int paramcache;
       
        LCD.clearDisplay();
        if (pID==null) {
            LCD.drawString( "Null pID val" , 0, 0, true);
            doWait(3000);
            return;
        }
       
        // pause the motor
        PIDControl.halt();
        debounce();
        while (!Button.ESCAPE.isPressed()){           
            LCD.drawString("ENT-Edit,ESC-Exit" , 0, 0, true);
            if (Button.ENTER.isPressed()){
                while (!Button.ESCAPE.isPressed()){
                    paramcache = pIDParam;
                    // get which param needs adjustment
                    pIDParam=selParam(pIDParam);
                    if (pIDParam!=-1) {
                        adjustParam(pIDParam);
                    } else {
                        pIDParam = paramcache;
                        LCD.clearDisplay();
                        break;
                    }
                    LCD.clearDisplay();
                }
                debounce();
            }
            pIDParam = 0;
            outputVars();
            debounce();
        }
        LCD.clearDisplay();
        debounce();
    }
   
    private void outputVars() {
        LCD.drawString("P=" + pID.getPIDParam(pID.PID_KP) + "    ", 0, 2);
        LCD.drawString("I=" + Float.toString(pID.getPIDParam(pID.PID_KI)) + "    ", 0, 3);
        LCD.drawString("D=" + pID.getPIDParam(pID.PID_KD) + "    ", 0, 4);
        LCD.drawString("pow=" + pID.getPIDParam(pID.PID_RAMP_POWER) + "    ", 0, 5);
        LCD.drawString("t=" + pID.getPIDParam((int)pID.PID_RAMP_THRESHOLD) + "    ", 0, 6);
        LCD.drawString("Deadband=" + (int)pID.getPIDParam(pID.PID_DEADBAND) + "    ", 0, 7);
    }
   
    private void adjustParam(int pIDParam){
        float term;
        float increment = getIncrement(false);;
       
        // get keytone to restore later
        //int keyTone = Button.getKeyClickTone(Button.ID_ENTER);       
       
        LCD.clearDisplay();
        // disable ENTER key tone
        //Button.setKeyClickTone(Button.ID_ENTER, 0);
        // display the selected param name
        LCD.drawString(getParamDisp(pIDParam) , 0, 0, true);
        LCD.drawString("x" + increment + "     " , 8, 1);
        outputVars();
        term = pID.getPIDParam(pIDParam);
        while (!Button.ESCAPE.isPressed()) {
            if (Button.LEFT.isPressed()){
                term = term - increment;
                if (term<=0.0f) term=0.0f;
                pID.setPIDParam(pIDParam, term);
                outputVars();
                debounce();
            }
            if (Button.RIGHT.isPressed()){
                term = term + increment;
                pID.setPIDParam(pIDParam, term);
                outputVars();
                debounce();
            }
            if (Button.ENTER.isPressed()){
                increment=getIncrement(true);
                LCD.drawString("x" + increment + "     " , 8, 1);
                debounce();
            }
            // do the process control
           PIDControl.setMV();
        }
        LCD.clearDisplay();
        //Button.setKeyClickTone(Button.ID_ENTER, keyTone);
        PIDControl.halt();
        debounce();
    }
   
    private float getIncrement(boolean doNext){
        if (doNext) incrementIndex++;
        if (incrementIndex>5) incrementIndex=0;
        return (float)(10/Math.pow(10,incrementIndex));
       
    }
   
    private int selParam(int startwith){
        int param = startwith;
        final int MAXPARAM = 5;
       
        LCD.clearDisplay();
        LCD.drawString( "Select < > Param" , 0, 0, true);
        debounce();
        while (!Button.ENTER.isPressed()) {
            if (Button.LEFT.isPressed()) param--;
            if (Button.RIGHT.isPressed()) param++;
            if (param<0) param = MAXPARAM;
            if (param>MAXPARAM) param = 0;
            LCD.drawString(getParamDisp(param), 0, 1);
            debounce();
            outputVars();
            if (Button.ESCAPE.isPressed()) {
                param =-1;
                break;
            }
        }
        debounce();
        return param;
    }
   
    private String getParamDisp(int param){
        String strParam = "unknown";
        switch (param){
        case PIDController.PID_KP:
            strParam = "Proportional    ";
            break;
        case PIDController.PID_KI:
            strParam = "Integral        ";
            break;
        case PIDController.PID_KD:
            strParam = "Derivitive      ";
            break;
        case PIDController.PID_RAMP_POWER:
            strParam = "Ramp Exponential";
            break;
        case PIDController.PID_RAMP_THRESHOLD:
            strParam = "Ramp Threshold  ";
            break;
        case PIDController.PID_DEADBAND:
            strParam = "Deadband        ";
            break;
        }
        return strParam;
    }
   
    private void debounce(){
        doWait(333);
    }
     
    private void doWait(long milliseconds) {
         try {
             Thread.sleep(milliseconds);
         } catch (InterruptedException e) {
             //Thread.currentThread().interrupt();
              LCD.drawString("wait interrupted", 0, 7);
              Button.waitForPress();
         }
    }
}


Here is a method snippet from my testing class that uses this stuff in a PID controller for a compass sensor. The enter key is pressed to initiate the tuner:

Code: Select all

    private void testCompass(){
        //int heading;
        //int MV;
        final Motor positioner = Motor.C;
        final PIDController pID;
        final CompassSensor mySensor;
        mySensor = new CompassSensor(SensorPort.S1);
       
        LCD.drawString("running test" , 0, 1);
        doWait(500);
       
        initMotor(positioner);
       
        // do the PID thang
        pID = new PIDController((int)mySensor.getDegrees());
        pID.setPIDParam(PIDController.PID_DEADBAND, 1); // degrees
        pID.setPIDParam(PIDController.PID_KP, 11.0f);
        pID.setPIDParam(PIDController.PID_KI, 0.003f);
        pID.setPIDParam(PIDController.PID_KD, 30.0f);
        pID.setPIDParam(PIDController.PID_RAMP_POWER, 3.0f);
        pID.setPIDParam(PIDController.PID_RAMP_THRESHOLD, 120);
       
        // anonymous inner class to pass implementation to the PID tuner
        PIDTunerController myPID = new PIDTunerController(){
            public int getPV() {
                int heading = (int)(mySensor.getDegrees());
                //LCD.drawString("hdg: " + heading + "  " , 0, 5);
                return heading;
            }
            public PIDController getPIDController(){
                return pID;
            }
            public void setMV() {
//                int MV = getMV();
                int MV = getPIDController().doPID(getPV());
                positioner.setSpeed(MV);
                if (MV<0) positioner.backward(); else if (MV>0) positioner.forward(); else positioner.flt();
                doWait(50);
            }
            public void halt() {
                positioner.flt();
            }
        };
        // create the PID tuner
        PIDTuner pIDTuner = new PIDTuner(myPID);
       
        while (!Button.ESCAPE.isPressed()) {
            LCD.drawString("hdg: " + myPID.getPV() + "  " , 0, 5);
            myPID.setMV();
            if (Button.ENTER.isPressed()){
                pIDTuner.keyTune();
                // wait for debounce
                doWait(125);
            }
        }
       
        LCD.drawString( "done " , 0, 5);
        killThread = true;
        doWait(250);
    }

That's a lot for one post so I'm stopping now...
-K

[kirkpthompson]

Andy: No video but I can try to create one (<complement>not as professional as yours though!</compliment>). Let me see what I can do....

**ADDENDUM **
Youtube: http://www.youtube.com/watch?v=VO3CoKFmmOc

[kirkpthompson]

Here is a graph showing some sample PID_RAMP_POWER values with a PID_RAMP_THRESHOLD of 200:

[Excel]

[gloomyandy]

Love the video! Had me feeling a little dizzy by the end! A great demo of how the gyro can be used... With your walk around test how much do you think it was out be the time you had finished? Do you do anything to fix the gyro drift, or voltage/temperature drift?

Very cool...

Andy

[kirkpthompson]

The walk-around test ended with about 12 degrees off. I don't have the math down but the gyro integration doesn't account for any tilt from the horizontal plane (I'll call that x-y) so when the gyro is moved in the z plane, the angle on the x-y is not 90 deg if the gyro integrates to 90 deg. so on and so forth. The gyro reads "about 1 deg./sec" according to the documentation (key word being "about") so my integration constant of 1 deg./1000 msecs may be slightly off (and/or clock accuracy of the NXT) which will affect error accumulation and the final heading. I also have to remember this is a toy (albeit a fun one!) and I can't expect industrial/military/scientific accuracy. I guess if I had a way to calibrate it (or read the true x degs/sec it produces) that would help nail my integration constant. Lastly, I don't use any advanced math (fast Fourier transforms?) to filter noise, disturbances, perturbations etc. so error will accumulate there as well.

re: Gyro drift: My GyroSensor class uses an algorithm that constantly samples the gyro reading and calculates the bias reading when stationary. This bias is applied to the readHeading() method. It knows when it is stationary based on the degree of disturbance that the [very sensitive] gyroscope picks up. If the gyroscope was "in-flight" and at a constant velocity with no perturbations, the bias calculator would think it's stationary and probably calculate a wrong bias/offset value. My robots don't fly so that isn't a problem for me.
My experience shows that the gyro readings change rather quickly once a program starts (ie. power applied) so a theory is the resistance changes as things heat up, or, maybe that that is the operational characteristics of the the particular gyro chip/circuit.

Andy, can you help me understand what exactly the integral windup limiter would/could/should do for the PIDController? Do you have a test case where this can be explored?

Does anyone have any suggestions to increase the accuracy of the Gyro integration?

Thanx!
-K

[gloomyandy]

Hi Kirk,
OK I'll have a stab at it. Integral windup is basically what happens when the controller is pushed into a state where it has reached the maximum output power but the set point has not been reached. In this situation the integral term will continue to grow even though it can no longer have any impact on the controller output. The problem is that when the controller finally manages to get to the set point the integral term may have grown very large and this will cause an overshoot that can take a relatively long time to correct (as the integral value now needs to unwind). Two classic examples are:
1. A large change in the set point. This will take time for the motor or whatever to move to that point meanwhile the integral term builds up, and eventually will cause an overshoot.
2. The motor is stalled for a short period of time, at full power, again the integral term will continue to grow and will cause an overshoot.

There are lots of docs on the web describing this problem. It is not always an issue for all situations or controllers, but if this class is intended to be a generic PID calss then I think it is worth including some sort of limit...

Hope the above makes some sort of sense...

Andy

[kirkpthompson]

Hi Andy. That is a good explanation. I am going to use it in the documentation for the integral limiter that I will include in the class. I see there are multiple methodologies to limit integral windup and am exploring which I will implement. These seem to cover most bases:

Disabling the integral function until the PV has entered the controllable region
Preventing the integral term from accumulating above or below pre-determined bounds

I would like to have the setting related to publically accessible fields so I was thinking a ratio to MV or something. Do you foresee a need to have integral and derivative terms publically accessible?

-K

[kirkpthompson]

Latest class source is at http://www.mosen.net/lego/src/PIDController.java

Comments encouraged.

-K

[temp12]

Latest class source is at http://www.mosen.net/lego/src/PIDController.java

Comments encouraged.

-K

Hallo Kirk,
i am trying to use your class in the line follow scenario. But I didn't have success. Have you an example for the line following?

Thanks
temp