(maximum-minimum) ){
+ setMaxIOutput(maximum-minimum);
}
+}
+
+/** Set the operating direction of the PID controller
+ * @param reversed Set true to reverse PID output
+ */
+void MiniPID::setDirection(bool reversed){
+ this->reversed=reversed;
+}
+
+//**********************************
+//Primary operating functions
+//**********************************
+
+/**Set the target for the PID calculations
+ * @param setpoint
+ */
+void MiniPID::setSetpoint(double setpoint){
+ this->setpoint=setpoint;
+}
- /** Set the operating direction of the PID controller
- * @param reversed Set true to reverse PID output
- */
- public: void setDirection(bool reversed){
- this->reversed=reversed;
+/** Calculate the PID value needed to hit the target setpoint.
+* Automatically re-calculates the output at each call.
+* @param actual The monitored value
+* @param target The target value
+* @return calculated output value for driving the actual to the target
+*/
+double MiniPID::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);
}
- //**********************************
- //Primary operating functions
- //**********************************
+ //Do the simple parts of the calculations
+ double error=setpoint-actual;
- /**Set the target for the PID calculations
- * @param setpoint
- */
- public: void setSetpoint(double setpoint){
- this->setpoint=setpoint;
- }
-
- /** Calculate the PID value needed to hit the target setpoint.
- * Automatically re-calculates the output at each call.
- * @param actual The monitored value
- * @param target The target value
- * @return calculated output value for driving the actual to the target
- */
- public: double getOutput(double actual, double setpoint){
- double output;
- double Poutput;
- double Ioutput;
- double Doutput;
- double Foutput;
-
- this->setpoint=setpoint;
+ //Calculate F output. Notice, this->depends only on the setpoint, and not the error.
+ Foutput=F*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 P term
+ Poutput=P*error;
- //Calculate F output. Notice, this->depends only on the setpoint, and not the error.
- Foutput=F*setpoint;
+ //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 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
+ //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;
+ 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
- // /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;
- }
- /**
- * Calculates the PID value using the last provided setpoint and actual valuess
- * @return calculated output value for driving the actual to the target
- */
- public: double getOutput(){
- return getOutput(lastActual,setpoint);
- }
+ //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);
+ }
- /**
- *
- * @param actual
- * @return calculated output value for driving the actual to the target
- */
- 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.
- */
- public: void reset(){
- firstRun=true;
- errorSum=0;
- }
- /**Set the maximum rate the output can increase per cycle.
- * @param rate
- */
- public: void setOutputRampRate(double rate){
- outputRampRate=rate;
- }
+ //And, finally, we can just add the terms up
+ output=Foutput + Poutput + Ioutput + Doutput;
- /** Set a limit on how far the setpoint can be from the current position
- *
Can simplify tuning by helping tuning over a small range applies to a much larger range.
- *
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
- */
- public: void setSetpointRange(double range){
- setpointRange=range;
+ //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
}
-
- /**Set a filter on the output to reduce sharp oscillations.
- * 0.1 is likely a sane starting value. Larger values P and D oscillations, but force larger I values.
- * Uses an exponential rolling sum filter, according to a simple
- * output*(1-strength)*sum(0..n){output*strength^n}
- * @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;
+ 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);
}
-
- //**************************************
- // 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;
+
+ //Get a test printline
+// /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;
}
+
+/**
+ * Calculates the PID value using the last provided setpoint and actual valuess
+ * @return calculated output value for driving the actual to the target
+ */
+double MiniPID::getOutput(){
+ return getOutput(lastActual,setpoint);
+}
+
+/**
+ *
+ * @param actual
+ * @return calculated output value for driving the actual to the target
+ */
+double MiniPID::getOutput(double actual){
+ return getOutput(actual,setpoint);
+}
- /**
- * 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
- */
- private: bool bounded(double value, double min, double max){
- return (minerases the I term buildup, and removes D gain on the next loop.
+ */
+void MiniPID::reset(){
+ firstRun=true;
+ errorSum=0;
+}
+
+/**Set the maximum rate the output can increase per cycle.
+ * @param rate
+ */
+void MiniPID::setOutputRampRate(double rate){
+ outputRampRate=rate;
+}
+
+/** Set a limit on how far the setpoint can be from the current position
+ *
Can simplify tuning by helping tuning over a small range applies to a much larger range.
+ *
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
+ */
+void MiniPID::setSetpointRange(double range){
+ setpointRange=range;
+}
+
+/**Set a filter on the output to reduce sharp oscillations.
+ * 0.1 is likely a sane starting value. Larger values P and D oscillations, but force larger I values.
+ * Uses an exponential rolling sum filter, according to a simple
+ * output*(1-strength)*sum(0..n){output*strength^n}
+ * @param output valid between [0..1), meaning [current output only.. historical output only)
+ */
+void MiniPID::setOutputFilter(double strength){
+ if(strength==0 || bounded(strength,0,1)){
+ outputFilter=strength;
}
-
- /**
- * 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;
- }
+}
+
+//**************************************
+// 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
+ */
+double MiniPID::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
+ */
+bool MiniPID::bounded(double value, double min, double max){
+ return (min0) 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;
}
-};
+}
diff --git a/MiniPID.h b/MiniPID.h
index b5bd483..1495647 100644
--- a/MiniPID.h
+++ b/MiniPID.h
@@ -23,5 +23,37 @@ class MiniPID{
double getOutput();
double getOutput(double actual);
double getOutput(double actual, double setpoint);
+
+private:
+ double constrain(double value, double min, double max);
+ bool bounded(double value, double min, double max);
+ void checkSigns();
+ void init();
+
+ double P;
+ double I;
+ double D;
+ double F;
+
+ double maxIOutput;
+ double maxError;
+ double errorSum;
+
+ double maxOutput;
+ double minOutput;
+
+ double setpoint;
+
+ double lastActual;
+
+ bool firstRun; //default true
+ bool reversed; //default false
+
+ double outputRampRate;
+ double lastOutput;
+
+ double outputFilter;
+
+ double setpointRange;
};
#endif