Generic PID Controller class

[kirkpthompson]

Hi Temp. No, I don't have an example of a line follower implementation using PIDController. Maybe you could provide some specifics on what you are trying to accomplish and we could go from there?

Also, FYI: This class will be included in the upcoming 0.9 release of LeJOS. The latest source can be viewed at http://lejos.svn.sourceforge.net/viewvc/lejos/trunk/classes/lejos/util/PIDController.java?view=log

Best,
-K

[temp12]

Hi Temp. No, I don't have an example of a line follower implementation using PIDController. Maybe you could provide some specifics on what you are trying to accomplish and we could go from there?

Hi Kirk,

I wanted to make a bot following the black line with the full speed on the map that is included in the Lego NXT 2.0 set. But I can't tune the variables properly.

Best regards
temp

[kirkpthompson]

Ok... Can you provide your PIDController implementation code?

-K

[temp12]

Ok... Can you provide your PIDController implementation code?
-K

I wanted to tune the variables over the bluetooth connection. Here is my code:

For NXT:

Code: Select all

import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;

import lejos.nxt.Button;
import lejos.nxt.ColorSensor;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.comm.Bluetooth;
import lejos.nxt.comm.NXTConnection;
import lejos.robotics.navigation.DifferentialPilot;

public class PIDLineFollow {
   public PIDController pidController;

   public float setpoint = 45;
   public float deadband = 1;
   public float kp = 3.5f;
   public float ki = 0.0f;
   public float kd = 0.0f;
   public float limithigh = 200.0f;
   public float limitlow = -200.0f;
   public float integrallimithigh = 0.0f;
   public float integrallimitlow = 0.0f;
   public long dt = 25;

   public boolean isThreadStopped = true;
   public boolean stopped = false;

   public ColorSensor sensor;
   public DifferentialPilot pilot;

   public PIDLineFollow() {
      sensor = new ColorSensor(SensorPort.S1);
      sensor.setFloodlight(true);
      pilot = new DifferentialPilot(5.6f, 12.6f, Motor.A, Motor.B, false);
        pilot.setTravelSpeed(30);
        pilot.setRotateSpeed(400);
   }

   public void setPIDParameters() {
      if (pidController != null) {
         pidController.setPIDParam(PIDController.PID_SETPOINT, setpoint);
         pidController.setPIDParam(PIDController.PID_DEADBAND, deadband);
         pidController.setPIDParam(PIDController.PID_KP, kp);
         pidController.setPIDParam(PIDController.PID_KI, ki);
         pidController.setPIDParam(PIDController.PID_KD, kd);
         pidController.setPIDParam(PIDController.PID_LIMITHIGH, limithigh);
         pidController.setPIDParam(PIDController.PID_LIMITLOW, limitlow);
         pidController.setPIDParam(PIDController.PID_I_LIMITHIGH, integrallimithigh);
         pidController.setPIDParam(PIDController.PID_I_LIMITLOW, integrallimitlow);
      }
   }

   public void start() {
      new Thread() {
         public void run() {
            isThreadStopped = false;

            pidController = new PIDController((int) setpoint);
            setPIDParameters();

            while (!stopped) {
               int lightvalue = sensor.getLightValue();
               int MV = pidController.doPID(lightvalue);
               
               LCD.drawString(lightvalue + " " + MV + "  ", 0, 1);      
               pilot.steer(MV);
               
               try {
                  Thread.sleep(dt);
               } catch (InterruptedException e) {

               }
            }            

            pilot.stop();
            
            isThreadStopped = true;
         }
      }.start();
   }

   public static void main(String argv[]) {
      // 0016530C218B
      LCD.drawString("waiting     ", 0, 0);
      NXTConnection connection = Bluetooth.waitForConnection();
      if (connection == null)
         return;

      PIDLineFollow p = new PIDLineFollow();
      LCD.drawString("connected    ", 0, 0);

      final DataInputStream input = connection.openDataInputStream();
      final DataOutputStream output = connection.openDataOutputStream();
   
      try {
         while (!Button.ESCAPE.isPressed()) {
            int command = input.readInt();

            LCD.drawString("command: " + command, 0, 1);

            if (PIDConstants.SHUTDOWN_COMMAND == command) {
               p.stopped = true;
               break;
            } else if (PIDConstants.STOP_COMMAND == command) {
               p.stopped = true;
            } else if (PIDConstants.START_COMMAND == command) {
               p.stopped = false;
               p.start();
            } else if (PIDConstants.WRITE_COMMAND == command) {
               int key = input.readInt();
               float value = input.readFloat();
               if (PIDController.PID_SETPOINT == key) {
                  p.setpoint = value;
               } else if (PIDController.PID_DEADBAND == key) {
                  p.deadband = value;
               } else if (PIDController.PID_KP == key) {
                  p.kp = value;
               } else if (PIDController.PID_KI == key) {
                  p.ki = value;
               } else if (PIDController.PID_KD == key) {
                  p.kd = value;
               } else if (PIDController.PID_LIMITHIGH == key) {
                  p.limithigh = value;
               } else if (PIDController.PID_LIMITLOW == key) {
                  p.limitlow = value;
               } else if (PIDController.PID_I_LIMITHIGH == key) {
                  p.integrallimithigh = value;
               } else if (PIDController.PID_I_LIMITLOW == key) {
                  p.integrallimitlow = value;
               } else if (PIDConstants.TRAVEL_SPEED == key) {
                  p.pilot.setTravelSpeed((int)value);
               } else if (PIDConstants.ROTATE_SPEED == key) {
                  p.pilot.setRotateSpeed((int)value);
               } else if (PIDConstants.INTERVAL == key) {
                  p.dt = (long) value;
               } else {
                  value = -1;
               }               
               output.writeFloat(value);
            } else if (PIDConstants.READ_COMMAND == command) {
               int key = input.readInt();
               LCD.drawString("key: " + key, 0, 2);
               float value = 0;
               if (PIDController.PID_SETPOINT == key) {
                  value = p.setpoint;
               } else if (PIDController.PID_DEADBAND == key) {
                  value = p.deadband;
               } else if (PIDController.PID_KP == key) {
                  value = p.kp;
               } else if (PIDController.PID_KI == key) {
                  value = p.ki;
               } else if (PIDController.PID_KD == key) {
                  value = p.kd;
               } else if (PIDController.PID_LIMITHIGH == key) {
                  value = p.limithigh;
               } else if (PIDController.PID_LIMITLOW == key) {
                  value = p.limitlow;
               } else if (PIDController.PID_I_LIMITHIGH == key) {
                  value = p.integrallimithigh;
               } else if (PIDController.PID_I_LIMITLOW == key) {
                  value = p.integrallimitlow;
               } else if (PIDController.PID_I == key) {
                  value = -999999;
                  if (p.pidController != null) {
                     value = p.pidController.getPIDParam(PIDController.PID_I);
                  }
               } else if (PIDConstants.RAW_OUTPUT_MV == key) {
                  value = -999999;
                  if (p.pidController != null) {
                     //value = p.pidController.rawOutputMV;
                  }
               } else if (PIDConstants.LIGHTVALUE == key) {
                  value = p.sensor.getLightValue();
               } else if (PIDConstants.TRAVEL_SPEED == key) {
                  value = p.pilot.getTravelSpeed();
               } else if (PIDConstants.ROTATE_SPEED == key) {
                  value = p.pilot.getRotateSpeed();
               } else if (PIDConstants.MAX_TRAVEL_SPEED == key) {
                  value = p.pilot.getMaxTravelSpeed();
               } else if (PIDConstants.MAX_ROTATE_SPEED == key) {
                  value = p.pilot.getMaxRotateSpeed();
               } else if (PIDConstants.INTERVAL == key) {
                  value = p.dt;
               } else {
                  value = -1;
               }
               output.writeFloat(value);
            } else {
               // unknown command
               output.writeInt(-1);
            }
            output.flush();
         }
         p.stopped = true;
         connection.close();
      } catch (IOException e) {
         e.printStackTrace();
      }

      p.stopped = true;
      while (!p.isThreadStopped) {
         try {
            Thread.sleep(100);
         } catch (InterruptedException e) {

         }
      }
   }
}


For the PC (you must configure also the bluetooth address of the used NXT):

Code: Select all

import java.io.BufferedReader;
import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;
import java.io.InputStreamReader;
import java.util.StringTokenizer;

import lejos.pc.comm.NXTCommException;
import lejos.pc.comm.NXTCommLogListener;
import lejos.pc.comm.NXTConnector;

public class PIDTuner {
   public static int getKey(String name) {
      if (name != null) {
         name = name.toLowerCase().trim();
      }
      
      if ("setpoint".equals(name)) {
         return PIDController.PID_SETPOINT;
      }
      if ("deadband".equals(name)) {
         return PIDController.PID_DEADBAND;
      }
      if ("kp".equals(name)) {
         return PIDController.PID_KP;
      }
      if ("ki".equals(name)) {
         return PIDController.PID_KI;
      }
      if ("kd".equals(name)) {
         return PIDController.PID_KD;
      }
      if ("limithigh".equals(name)) {
         return PIDController.PID_LIMITHIGH;
      }
      if ("limitlow".equals(name)) {
         return PIDController.PID_LIMITLOW;
      }
      if ("integrallimithigh".equals(name)) {
         return PIDController.PID_I_LIMITHIGH;
      }
      if ("integrallimitlow".equals(name)) {
         return PIDController.PID_I_LIMITLOW;
      }
      if ("integral".equals(name)) {
         return PIDController.PID_I;
      }
      if ("rawoutputmv".equals(name)) {
         return PIDConstants.RAW_OUTPUT_MV;
      }
      if ("lightvalue".equals(name)) {
         return PIDConstants.LIGHTVALUE;
      }
      if ("travelspeed".equals(name)) {
         return PIDConstants.TRAVEL_SPEED;
      }
      if ("rotatespeed".equals(name)) {
         return PIDConstants.ROTATE_SPEED;
      }
      if ("maxtravelspeed".equals(name)) {
         return PIDConstants.MAX_TRAVEL_SPEED;
      }
      if ("maxrotatespeed".equals(name)) {
         return PIDConstants.MAX_ROTATE_SPEED;
      }
      if ("interval".equals(name)) {
         return PIDConstants.INTERVAL;
      }

      return -1;
   }

   public static void main(String argv[]) throws NXTCommException {
      String bluetoothAddress = "0016530C218B";
      if (argv.length > 0) {
         bluetoothAddress = argv[0];
      }
      
      NXTConnector connector = new NXTConnector();
      connector.addLogListener(new NXTCommLogListener() {
         public void logEvent(String message) {
            System.out.println("BTSend Log.listener: " + message);
         }

         public void logEvent(Throwable throwable) {
            System.out.println("BTSend Log.listener - stack trace: ");
            throwable.printStackTrace();
         }
      });

      boolean connected = connector.connectTo("btspp://" + bluetoothAddress);
      if (!connected) {
         System.err.println("Failed to connect to any NXT");
         System.exit(1);
      }

      System.out.println("Connected!");

      DataOutputStream dos = connector.getDataOut();
      DataInputStream dis = connector.getDataIn();

      BufferedReader reader = new BufferedReader(new InputStreamReader(System.in));
      try {
         while (true) {            
            String line = reader.readLine().toLowerCase();

            if ("exit".equals(line)) {
               dos.writeInt(PIDConstants.SHUTDOWN_COMMAND);
               dos.flush();
               break;
            }
            if ("start".equals(line)) {
               dos.writeInt(PIDConstants.START_COMMAND);
               dos.flush();
            }
            if ("stop".equals(line)) {
               dos.writeInt(PIDConstants.STOP_COMMAND);
               dos.flush();
            }
            if (line.startsWith("read")) {
               int key = getKey(line.substring(4));
               
               if (key != -1) {
                  dos.writeInt(PIDConstants.READ_COMMAND);
                  dos.flush();
                  dos.writeInt(key);
                  dos.flush();
                  System.out.println("received: " + dis.readFloat());
               } else {
                  System.out.println("wrong key!");
               }               
            }
            if (line.startsWith("write")) {
               StringTokenizer tokenizer = new StringTokenizer(line.substring(5));
               
               if (tokenizer.countTokens() == 2) {
                  int key = getKey(tokenizer.nextToken());
                  if (key != -1) {
                     try {
                        float value = Float.parseFloat(tokenizer.nextToken());
                        dos.writeInt(PIDConstants.WRITE_COMMAND);
                        dos.flush();
                        dos.writeInt(key);
                        dos.flush();
                        dos.writeFloat(value);
                        dos.flush();
                        System.out.println("received: " + dis.readFloat());
                     } catch (NumberFormatException e) {
                        System.out.println("wrong number format!");
                     }
                  } else {
                     System.out.println("wrong key!");
                  }
               } else {
                  System.out.println("wrong format!");
               }               
            }
         }
      } catch (IOException e) {
         e.printStackTrace();
      }

      try {
         dis.close();
         dos.close();
         connector.close();
      } catch (IOException ioe) {
         System.out.println("IOException closing connection");
      }
   }
}


Some constants used both on PC and NXT:

Code: Select all


public interface PIDConstants {
   
   public static int SHUTDOWN_COMMAND = -1;
   public static int STOP_COMMAND = 0;
   public static int START_COMMAND = 1;
   public static int READ_COMMAND = 2;
   public static int WRITE_COMMAND = 3;
   
   public static int LIGHTVALUE = 100;
   
   public static int TRAVEL_SPEED = 110;
   public static int ROTATE_SPEED = 111;
   public static int MAX_TRAVEL_SPEED = 112;
   public static int MAX_ROTATE_SPEED = 113;
   
   public static int INTERVAL = 120;
   public static int RAW_OUTPUT_MV = 121;
}


How to use it:
* Deploy PIDLineFollow on the NXT
* Start PIDTuner on PC
** Now we can read and write variables on the NXT ie.: "read setpoint" or "write kp 11.0" etc.
*** Following variables are available: setpoint, deadband, kp, ki, kd, limithigh, limitlow, integrallimitlow, integrallimithigh, integral, lightvalue, travelspeed, rotatespeed, interval
** After variable configuration, we can start and stop the lego with "start" or "stop"
** After "stop" we can configure variables again
** If "exit" called, the NXT is shutdown.

My problem is: I cant tune the variables properly.

best regards
temp

[kirkpthompson]

Hi Temp12.

Your PIDController implementation looks semantically correct. I agree, tuning PID loops is tricky. Wikipedia has some good basic info and some common methods and links to other resources and I recommend you have a look at that.
http://en.wikipedia.org/wiki/PID_controller#Loop_tuning

I did notice you have a fairly large intial Kp (P) value. Have you tried to start out with a small value (0.1f) and observe behavior during loop execution? Increase the value until you observe oscillation/instability and back it off some. That is a starting point.

Also, you may be adding time to dt by virtue of listening for BT data (I didn't examine that code) and PID loops are time-dependent by definition.

I have been working on a PID tuner class that uses the NXT keys and LCD to provide a tuning interface and like the idea of wireless tuning (in case the brick is swinging around, etc. which makes it hard to read and press keys). It actively runs the PID loop while tuning with manual incremental parameter adjustments which gives immediate feedback during the tuning process. It has low overhead time and CPU-wise so the tuning for parameters doesn't change too drastically when tuning is complete and the tuner reference is removed.

Best,
-K

[kirkpthompson]

Here is a graph that shows PID control loop instability. Output = MV, actual_position=PV. Notice the excessive proportional response to process disturbance (PV) between 4000 ms and 5200 ms. Try to get the process to respond (it might be a slow response) without oscillation/overshoot and tune the Ki from there. (generally, Ki provides how fast an error is stabilized, Kp provides the "magnitude" or proportional response to error). You can play around with output ramping which will attenuate the response but try to get things fairly stable before you add too many "cooks in the kitchen".


Best,
-K

[temp12]

...
I did notice you have a fairly large intial Kp (P) value. Have you tried to start out with a small value (0.1f) and observe behavior during loop execution? Increase the value until you observe oscillation/instability and back it off some. That is a starting point.
...

Hallo Kirk,

I have started first with only the KP variable. If I choose the KP < 3.0, the nxt couldn't follow the line (is it called overshoot?). If I choose the KP > 4.0, the nxt oscillated too much. So 3.5 was the middle value. Then I started to set the KI variable according to the Ziegler-Nichols method. But then the integral grows too much. So I must set integral limits. But what are the optimal integral limits? How they are computed?

best regards
temp

[gloomyandy]

Hi,
A possible problem here is that you may not have a sufficient input range to make the PID control work smoothly. Basically all you have is on line/off line with a very small on the edge section. It may help to scan the sensor over the line using an extra motor, to gather extra data points. In effect by scanning the sensor you create a set of data points rather like the purpose built line leader from Mindsensors.

See this thread for some more background...

https://sourceforge.net/apps/phpbb/mind ... +pid#p6775

Andy

[kirkpthompson]

Hi All. Hope it is not an issue to revive an old thread but is is applicable to the context.

There is now PID Tuning functionality in the SVN snapshot if anyone feels brave enough to test it.

It is one implementation of a new messaging architecture I have been building into NXT Charting Logger (http://bit.ly/IKorbq) and lejos.util.NXTDataLogger that will enable 2-way communication between the PC and NXT via the NXTDataLogger protocol as a tunneled message. This is done simultaneously while the NXTDataLogger/NXJChartingLogger are gathering, sending, logging and charting data.

The vision is when the programmer/user "plugs-in" a type handler in the code (i.e. a PIDtuner instance), the NXJChartingLogger will automatically display the appropriate GUI JPanel in a new tab in the lower tabbed pane on connection. This user interface is then used to get user input and pass it to the NXTDataLogger via a COMMAND_PASSTHROUGH message which then routes it through a message handler singleton which then parses it and routes it to the appropriate MessageTypeHandler (i.e. PIDTuner again). the NXT-to-PC routing basically works the same.

Client (NXT) code example that shows all that is needed to set up a PID tuner for lejos.util.PIDController:

Code: Select all

NXTDataLogger dlog = new NXTDataLogger();
NXTConnection conn = Bluetooth.waitForConnection(5000, NXTConnection.PACKET);
try {
    dlog.startRealtimeLog(conn);
} catch (IOException e) {
    // Do nothing
}
   
// Test passthrough message management and PID tuning framework
LogMessageManager lmm = LogMessageManager.getLogMessageManager(dlog);
PIDController pid = new PIDController(0, 50);
pid.setPIDParam(PIDController.PID_KP, 2);
PIDTuner pidTuner = new PIDTuner(pid, lmm);
... PID control code here ...



Any takers? To test, you will have to use the snapshot for the latest code so knowing how to do that is a plus.

Best,
-K

[eidmanna]

Hi!

The PID Chart Logger is really cool

In my test, everything works fine and the robot can follow the line. The only problem was, that the connection timeout for the bluetooth connection of 5 sec. was to short for me.

Any ideas for improvement of the code are very welcome.

Here is my test:

Code: Select all

package my.lejos.linefollower;

import lejos.nxt.Button;
import lejos.nxt.ColorSensor;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.comm.Bluetooth;
import lejos.nxt.comm.NXTConnection;
import lejos.util.LogMessageManager;
import lejos.util.NXTDataLogger;
import lejos.util.PIDController;
import lejos.util.PIDTuner;

public class LineFollower {

   static int SPEED = 200;
   public static void main(String[] aArg) throws Exception {
      
      
      NXTDataLogger dlog = new NXTDataLogger();
      NXTConnection conn = Bluetooth.waitForConnection(15000, NXTConnection.PACKET);
      try {
          dlog.startRealtimeLog(conn);
      } catch (Exception e) {
          // Do nothing
      }
         
      // Test passthrough message management and PID tuning framework
      LogMessageManager lmm = LogMessageManager.getLogMessageManager(dlog);
      PIDController pid = new PIDController(340, 10);
      pid.setPIDParam(PIDController.PID_KP, 2.0f);
//      pid.setPIDParam(PIDController.PID_KI, 0.05f);
//      pid.setPIDParam(PIDController.PID_KD, 10f);
      PIDTuner pidTuner = new PIDTuner(pid, lmm);
      Motor.B.setSpeed(SPEED);
      Motor.C.setSpeed(SPEED);
      Motor.B.forward();
      Motor.C.forward();
      final ColorSensor detector = new ColorSensor(SensorPort.S2);
      while (!Button.ESCAPE.isDown()) {
                        // range is from 200 to 500
         int sensor = detector.getRawLightValue();         
         //System.out.println(sensor);
         int speedDelta = pid.doPID(sensor);
         // System.out.println(speedDelta);
      
         Motor.B.setSpeed(SPEED-speedDelta);
                        Motor.C.setSpeed(SPEED+speedDelta);
         
         
      }
   }
}



[kirkpthompson]

Hi eidmanna.

So, I assume you used the snapshot files and ran the NXT Charting Logger tool to see the PID parameters used in your code? I would be interested in seeing the graph that was generated when trying to tune your line follower. As described in previous posts, using PID control for that task may not work well.

Good job on getting it to work!

Best,
-K

[eidmanna]

Hi Kirk!

My goal is not to build the best line follower, but to learn all about a PID Controller.

Like in this articel: http://www.inpharmix.com/jps/PID_Contro ... obots.html, but with lejos.

A motor speed faster than 200 seams not to be possible, but the implementation of the line follower with you PIDController class is very short and easy to understand.

I will post some charts soon. Are you interessted in special values for Kp, Ki and Kd?

Best, Andreas

[eidmanna]

Here is a simple chart with Kp = 1 Ki and Kd = 0

https://picasaweb.google.com/116130071017252186783/PID?authuser=0&authkey=Gv1sRgCK3Atpeo1rfJ0QE&feat=directlink

[kirkpthompson]

It's interesting how using getRawLightValue() from the color sensor as PV works in this case. Very interesting... And, the link you provided is good information. Thanks!

The plot looks right given your tuning params: 1:1 inverse proportional response to error. FYI, an example of using an accelerometer sensor with PID control can be found at http://www.youtube.com/watch?v=qGxGO5YicE4.

Best,
-K