package com.somitsolutions.java.pid;
/**
* Small, easy to use PID implementation with advanced controller capability.<br>
* Minimal usage:<br>
* MiniPID pid = new MiniPID(p,i,d); <br>
* ...looping code...{ <br>
* output= pid.getOutput(sensorvalue,target); <br>
* }
*
* @see http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-direction/improving-the-beginners-pid-introduction
*/
public class PID{
//**********************************
// Class private variables
//**********************************
private double P=0;
private double I=0;
private double D=0;
private double F=0;
private double maxIOutput=0;
private double maxError=0;
private double errorSum=0;
private double maxOutput=0;
private double minOutput=0;
private double setpoint=0;
private double lastActual=0;
private boolean firstRun=true;
private boolean reversed=false;
private double outputRampRate=0;
private double lastOutput=0;
private double outputFilter=0;
private double setpointRange=0;
//**********************************
// Constructor functions
//**********************************
/**
* Create a MiniPID class object.
* See setP, setI, setD methods for more detailed parameters.
* @param p Proportional gain. Large if large difference between setpoint and target.
* @param i Integral gain. Becomes large if setpoint cannot reach target quickly.
* @param d Derivative gain. Responds quickly to large changes in error. Small values prevents P and I terms from causing overshoot.
*/
public PID(double p, double i, double d){
P=p; I=i; D=d;
checkSigns();
}
/**
* Create a MiniPID class object.
* See setP, setI, setD, setF methods for more detailed parameters.
* @param p Proportional gain. Large if large difference between setpoint and target.
* @param i Integral gain. Becomes large if setpoint cannot reach target quickly.
* @param d Derivative gain. Responds quickly to large changes in error. Small values prevents P and I terms from causing overshoot.
* @param f Feed-forward gain. Open loop "best guess" for the output should be. Only useful if setpoint represents a rate.
*/
public PID(double p, double i, double d, double f){
P=p; I=i; D=d; F=f;
checkSigns();
}
//**********************************
// Configuration functions
//**********************************
/**
* Configure the Proportional gain parameter. <br>
* This responds quickly to changes in setpoint, and provides most of the initial driving force
* to make corrections. <br>
* Some systems can be used with only a P gain, and many can be operated with only PI.<br>
* For position based controllers, this is the first parameter to tune, with I second. <br>
* For rate controlled systems, this is often the second after F.
*
* @param p Proportional gain. Affects output according to <b>output+=P*(setpoint-current_value)</b>
*/
public void setP(double p){
P=p;
checkSigns();
}
/**
* Changes the I parameter <br>
* This is used for overcoming disturbances, and ensuring that the controller always gets to the control mode.
* Typically tuned second for "Position" based modes, and third for "Rate" or continuous based modes. <br>
* Affects output through <b>output+=previous_errors*Igain ;previous_errors+=current_error</b>
*
* @see {@link #setMaxIOutput(double) setMaxIOutput} for how to restrict
*
* @param i New gain value for the Integral term
*/
public void setI(double i){
if(I!=0){
errorSum=errorSum*I/i;
}
if(maxIOutput!=0){
maxError=maxIOutput/i;
}
I=i;
checkSigns();
// Implementation note:
// This Scales the accumulated error to avoid output errors.
// As an example doubling the I term cuts the accumulated error in half, which results in the
// output change due to the I term constant during the transition.
}
/**
* Changes the D parameter <br>
* This has two primary effects:
* <list>
* <li> Adds a "startup kick" and speeds up system response during setpoint changes
* <li> Adds "drag" and slows the system when moving toward the target
* </list>
* A small D value can be useful for both improving response times, and preventing overshoot.
* However, in many systems a large D value will cause significant instability, particularly
* for large setpoint changes.
* <br>
* Affects output through <b>output += -D*(current_input_value - last_input_value)</b>
*
* @param d New gain value for the Derivative term
*/
public void setD(double d){
D=d;
checkSigns();
}
/**
* Configure the FeedForward parameter. <br>
* This is excellent for velocity, rate, and other continuous control modes where you can
* expect a rough output value based solely on the setpoint.<br>
* Should not be used in "position" based control modes.<br>
* Affects output according to <b>output+=F*Setpoint</b>. Note, that a F-only system is actually open loop.
*
* @param f Feed forward gain.
*/
public void setF(double f){
F=f;
checkSigns();
}
/**
* Configure the PID object.
* See setP, setI, setD methods for more detailed parameters.
* @param p Proportional gain. Large if large difference between setpoint and target.
* @param i Integral gain. Becomes large if setpoint cannot reach target quickly.
* @param d Derivative gain. Responds quickly to large changes in error. Small values prevents P and I terms from causing overshoot.
*/
public void setPID(double p, double i, double d){
P=p;D=d;
//Note: the I term has additional calculations, so we need to use it's
//specific method for setting it.
setI(i);
checkSigns();
}
/**
* Configure the PID object.
* See setP, setI, setD, setF methods for more detailed parameters.
* @param p Proportional gain. Large if large difference between setpoint and target.
* @param i Integral gain. Becomes large if setpoint cannot reach target quickly.
* @param d Derivative gain. Responds quickly to large changes in error. Small values prevents P and I terms from causing overshoot.
* @param f Feed-forward gain. Open loop "best guess" for the output should be. Only useful if setpoint represents a rate.
*/
public void setPID(double p, double i, double d,double f){
P=p;D=d;F=f;
//Note: the I term has additional calculations, so we need to use it's
//specific method for setting it.
setI(i);
checkSigns();
}
/**
* Set the maximum output value contributed by the I component of the system
* This can be used to prevent large windup issues and make tuning simpler
* @param maximum. Units are the same as the expected output value
*/
public void setMaxIOutput(double maximum){
// Internally maxError and Izone are similar, but scaled for different purposes.
// The maxError is generated for simplifying math, since calculations against
// the max error are far more common than changing the I term or Izone.
maxIOutput=maximum;
if(I!=0){
maxError=maxIOutput/I;
}
}
/**
* Specify a maximum output range. <br>
* When one input is specified, output range is configured to
* <b>[-output, output]</b>
* @param output
*/
public void setOutputLimits(double output){
setOutputLimits(-output,output);
}
/**
* Specify a maximum output.
* When two inputs specified, output range is configured to
* <b>[minimum, maximum]</b>
* @param minimum possible output value
* @param maximum possible output value
*/
public void setOutputLimits(double minimum,double maximum){
if(maximum<minimum)return;
maxOutput=maximum;
minOutput=minimum;
// Ensure the bounds of the I term are within the bounds of the allowable output swing
if(maxIOutput==0 || maxIOutput>(maximum-minimum) ){
setMaxIOutput(maximum-minimum);
}
}
/**
* Set the operating direction of the PID controller
* @param reversed Set true to reverse PID output
*/
public void setDirection(boolean reversed){
this.reversed=reversed;
}
//**********************************
// Primary operating functions
//**********************************
/**
* Configure setpoint for the PID calculations<br>
* This represents the target for the PID system's, such as a
* position, velocity, or angle. <br>
* @see PID#getOutput(actual) <br>
* @param setpoint
*/
public void setSetpoint(double setpoint){
this.setpoint=setpoint;
}
/**
* Calculate the output value for the current PID cycle.<br>
* @param actual The monitored value, typically as a sensor input.
* @param setpoint The target value for the system
* @return calculated output value for driving the system
*/
public double getOutput(double actual, double setpoint){
double output;
double Poutput;
double Ioutput;
double Doutput;
double Foutput;
this.setpoint=setpoint;
// Ramp the setpoint used for calculations if user has opted to do so
if(setpointRange!=0){
setpoint=constrain(setpoint,actual-setpointRange,actual+setpointRange);
}
// Do the simple parts of the calculations
double error=setpoint-actual;
// Calculate F output. Notice, this depends only on the setpoint, and not the error.
Foutput=F*setpoint;
// Calculate P term
Poutput=P*error;
// If this is our first time running this, we don't actually _have_ a previous input or output.
// For sensor, sanely assume it was exactly where it is now.
// For last output, we can assume it's the current time-independent outputs.
if(firstRun){
lastActual=actual;
lastOutput=Poutput+Foutput;
firstRun=false;
}
// Calculate D Term
// Note, this is negative. This actually "slows" the system if it's doing
// the correct thing, and small values helps prevent output spikes and overshoot
Doutput= -D*(actual-lastActual);
lastActual=actual;
// The Iterm is more complex. There's several things to factor in to make it easier to deal with.
// 1. maxIoutput restricts the amount of output contributed by the Iterm.
// 2. prevent windup by not increasing errorSum if we're already running against our max Ioutput
// 3. prevent windup by not increasing errorSum if output is output=maxOutput
Ioutput=I*errorSum;
if(maxIOutput!=0){
Ioutput=constrain(Ioutput,-maxIOutput,maxIOutput);
}
// And, finally, we can just add the terms up
output=Foutput + Poutput + Ioutput + Doutput;
// Figure out what we're doing with the error.
if(minOutput!=maxOutput && !bounded(output, minOutput,maxOutput) ){
errorSum=error;
// reset the error sum to a sane level
// Setting to current error ensures a smooth transition when the P term
// decreases enough for the I term to start acting upon the controller
// From that point the I term will build up as would be expected
}
else if(outputRampRate!=0 && !bounded(output, lastOutput-outputRampRate,lastOutput+outputRampRate) ){
errorSum=error;
}
else if(maxIOutput!=0){
errorSum=constrain(errorSum+error,-maxError,maxError);
// In addition to output limiting directly, we also want to prevent I term
// buildup, so restrict the error directly
}
else{
errorSum+=error;
}
// Restrict output to our specified output and ramp limits
if(outputRampRate!=0){
output=constrain(output, lastOutput-outputRampRate,lastOutput+outputRampRate);
}
if(minOutput!=maxOutput){
output=constrain(output, minOutput,maxOutput);
}
if(outputFilter!=0){
output=lastOutput*outputFilter+output*(1-outputFilter);
}
// Get a test printline with lots of details about the internal
// calculations. This can be useful for debugging.
// System.out.printf("Final output %5.2f [ %5.2f, %5.2f , %5.2f ], eSum %.2f\n",output,Poutput, Ioutput, Doutput,errorSum );
// System.out.printf("%5.2f\t%5.2f\t%5.2f\t%5.2f\n",output,Poutput, Ioutput, Doutput );
lastOutput=output;
return output;
}
/**
* Calculate the output value for the current PID cycle.<br>
* In no-parameter mode, this uses the last sensor value,
* and last setpoint value. <br>
* Not typically useful, and use of parameter modes is suggested. <br>
* @return calculated output value for driving the system
*/
public double getOutput(){
return getOutput(lastActual,setpoint);
}
/**
* Calculate the output value for the current PID cycle.<br>
* In one parameter mode, the last configured setpoint will be used.<br>
* @see PID#setSetpoint()
* @param actual The monitored value, typically as a sensor input.
* @param setpoint The target value for the system
* @return calculated output value for driving the system
*/
public double getOutput(double actual){
return getOutput(actual,setpoint);
}
/**
* Resets the controller. This erases the I term buildup, and removes
* D gain on the next loop.<br>
* This should be used any time the PID is disabled or inactive for extended
* duration, and the controlled portion of the system may have changed due to
* external forces.
*/
public void reset(){
firstRun=true;
errorSum=0;
}
/**
* Set the maximum rate the output can increase per cycle.<br>
* This can prevent sharp jumps in output when changing setpoints or
* enabling a PID system, which might cause stress on physical or electrical
* systems. <br>
* Can be very useful for fast-reacting control loops, such as ones
* with large P or D values and feed-forward systems.
*
* @param rate, with units being the same as the output
*/
public void setOutputRampRate(double rate){
outputRampRate=rate;
}
/**
* Set a limit on how far the setpoint can be from the current position
* <br>Can simplify tuning by helping tuning over a small range applies to a much larger range.
* <br>This limits the reactivity of P term, and restricts impact of large D term
* during large setpoint adjustments. Increases lag and I term if range is too small.
* @param range, with units being the same as the expected sensor range.
*/
public void setSetpointRange(double range){
setpointRange=range;
}
/**
* Set a filter on the output to reduce sharp oscillations. <br>
* 0.1 is likely a sane starting value. Larger values use historical data
* more heavily, with low values weigh newer data. 0 will disable, filtering, and use
* only the most recent value. <br>
* Increasing the filter strength will P and D oscillations, but force larger I
* values and increase I term overshoot.<br>
* Uses an exponential wieghted rolling sum filter, according to a simple <br>
* <pre>output*(1-strength)*sum(0..n){output*strength^n}</pre> algorithm.
* @param output valid between [0..1), meaning [current output only.. historical output only)
*/
public void setOutputFilter(double strength){
if(strength==0 || bounded(strength,0,1)){
outputFilter=strength;
}
}
//**************************************
// Helper functions
//**************************************
/**
* Forces a value into a specific range
* @param value input value
* @param min maximum returned value
* @param max minimum value in range
* @return Value if it's within provided range, min or max otherwise
*/
private double constrain(double value, double min, double max){
if(value > max){ return max;}
if(value < min){ return min;}
return value;
}
/**
* Test if the value is within the min and max, inclusive
* @param value to test
* @param min Minimum value of range
* @param max Maximum value of range
* @return true if value is within range, false otherwise
*/
private boolean bounded(double value, double min, double max){
// Note, this is an inclusive range. This is so tests like
// `bounded(constrain(0,0,1),0,1)` will return false.
// This is more helpful for determining edge-case behaviour
// than <= is.
return (min<value) && (value<max);
}
/**
* To operate correctly, all PID parameters require the same sign
* This should align with the {@literal}reversed value
*/
private void checkSigns(){
if(reversed){ // all values should be below zero
if(P>0) P*=-1;
if(I>0) I*=-1;
if(D>0) D*=-1;
if(F>0) F*=-1;
}
else{ // all values should be above zero
if(P<0) P*=-1;
if(I<0) I*=-1;
if(D<0) D*=-1;
if(F<0) F*=-1;
}
}
}
PIDDisplay
package com.somitsolutions.java.pid;
import org.jfree.chart.ChartPanel;
import java.awt.Color;
import org.jfree.chart.ChartFactory;
import org.jfree.chart.JFreeChart;
import org.jfree.ui.ApplicationFrame;
import org.jfree.ui.RefineryUtilities;
import org.jfree.chart.plot.PlotOrientation;
import org.jfree.chart.plot.ValueMarker;
import org.jfree.chart.plot.XYPlot;
import org.jfree.data.category.CategoryDataset;
import org.jfree.data.category.DefaultCategoryDataset;
import org.jfree.data.xy.DefaultXYDataset;
import org.jfree.data.xy.XYDataset;
public class PIDDisplay extends ApplicationFrame {
//static DefaultXYDataset datasetActual = new DefaultXYDataset();
//static DefaultXYDataset datasetTarget = new DefaultXYDataset();
//static double[] actualIteration = new double[100];
//static double[] actualValue = new double[100];
//static double[] target = new double[100];
//static double[][] data1 = new double[100][100];
static DefaultCategoryDataset dataset = new DefaultCategoryDataset( );
//static DefaultCategoryDataset datasetTarget = new DefaultCategoryDataset( );
//static DefaultCategoryDataset dataSetTarget = new DefaultCategoryDataset();
public PIDDisplay( String applicationTitle , String chartTitle ) {
super(applicationTitle);
//JFreeChart targetChart = ChartFactory.createLineChart("Target", "", "", dataSetTarget, PlotOrientation.VERTICAL,true,true,false);
//XYPlot plot = targetChart.getXYPlot();
JFreeChart lineChartActualData = ChartFactory.createLineChart(
chartTitle,
"Iteration","Actual Output",
dataset,
PlotOrientation.VERTICAL,
true,true,false);
ChartPanel chartPanel = new ChartPanel( lineChartActualData );
/*
* JFreeChart lineChartTarget = ChartFactory.createLineChart( chartTitle,
* "Iteration","Taeget", datasetTarget, PlotOrientation.VERTICAL,
* true,true,false);
*/
chartPanel.setPreferredSize( new java.awt.Dimension( 560 , 367 ) );
setContentPane( chartPanel );
//setContentPane( chartPanel1 );
}
public static void createDataset( double value, int i) {
dataset.addValue( value , "Actual Output" , Integer.toString(i) );
dataset.addValue(100, "Target", Integer.toString(i));
}
}
Main
package com.somitsolutions.java.pid;
import org.jfree.ui.*;
public class Main {
/**
* @param args Any arguments passed from stdin
*/
public static void main(String[] args) {
PID pid;
//Kp = 0.25, Ki = 0.01, Kd = 0.4
pid = new PID(0.25, 0.01, 0.4);
pid.setOutputLimits(10);
pid.setSetpointRange(40);
//pid.setMaxIOutput(2);
//pid.setOutputRampRate(3);
//pid.setOutputFilter(.3);
double target=100;
double actual=0;
double output=0;
pid.setSetpoint(0);
pid.setSetpoint(target);
//System.err.printf("Target\tActual\tOutput\tError\n");
//System.err.printf("Output\tP\tI\tD\n");
// Position based test code
for (int i = 0; i < 100; i++){
//if(i==50)miniPID.setI(.05);
/*
* if (i == 60) target = 50;
*/
//if(i==75)target=(100);
//if(i>50 && i%4==0)target=target+(Math.random()-.5)*50;
output = pid.getOutput(actual, target);
actual = actual + output;
PIDDisplay.createDataset(actual, i);
//System.out.println("==========================");
//System.out.printf("Current: %3.2f , Actual: %3.2f, Error: %3.2f\n",actual, output, (target-actual));
//System.err.printf("%3.2f\t%3.2f\t%3.2f\t%3.2f\n", target, actual, output, (target-actual));
//if(i>80 && i%5==0)actual+=(Math.random()-.5)*20;
}
PIDDisplay chart = new PIDDisplay(
"PID Experimentation" ,
"Actual Output Value");
chart.pack( );
RefineryUtilities.centerFrameOnScreen( chart );
chart.setVisible( true );
}
}
