Skip to content

Commit

Permalink
Clean up docs and improve Arduino support
Browse files Browse the repository at this point in the history
This is mainly to improve functionality with Arduino
- remove `constrain` function, which conflicts with macros
- add example sketch to get started
- remove compiler warnings due to Arduino's .h/.cpp mangling
- various white space fixes
- test sketch conflicted with library management
- Add keywords.txt for highlighting
  • Loading branch information
tekdemo committed May 24, 2016
1 parent e2dd85c commit 4daf467
Show file tree
Hide file tree
Showing 6 changed files with 150 additions and 140 deletions.
103 changes: 46 additions & 57 deletions MiniPID.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -11,43 +11,36 @@

#include "MiniPID.h"

//**********************************
//Constructor functions
//**********************************
//**********************************
//Constructor functions
//**********************************
MiniPID::MiniPID(double p, double i, double d){
init();
P=p; I=i; D=d;
}
}
MiniPID::MiniPID(double p, double i, double d, double f){
init();
P=p; I=i; D=d; F=f;
}
}
void MiniPID::init(){
double P=0;
double I=0;
double D=0;
double F=0;

double maxIOutput=0;
double maxError=0;
double errorSum=0;

double maxOutput=0;
double minOutput=0;
P=0;
I=0;
D=0;
F=0;

double setpoint=0;

double lastActual=0;

bool firstRun=true;
bool reversed=false;

double outputRampRate=0;
double lastOutput=0;

double outputFilter=0;

double setpointRange=0;
maxIOutput=0;
maxError=0;
errorSum=0;
maxOutput=0;
minOutput=0;
setpoint=0;
lastActual=0;
firstRun=true;
reversed=false;
outputRampRate=0;
lastOutput=0;
outputFilter=0;
setpointRange=0;
}

//**********************************
Expand All @@ -66,7 +59,7 @@ void MiniPID::init(){
void MiniPID::setP(double p){
P=p;
checkSigns();
}
}

/**
* Changes the I parameter <br>
Expand Down Expand Up @@ -94,10 +87,11 @@ void MiniPID::setI(double i){
*
*/
}

void MiniPID::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
Expand All @@ -109,7 +103,7 @@ void MiniPID::setD(double d){
void MiniPID::setF(double f){
F=f;
checkSigns();
}
}

/** Create a new PID object.
* @param p Proportional gain. Large if large difference between setpoint and target.
Expand Down Expand Up @@ -141,14 +135,14 @@ void MiniPID::setMaxIOutput(double maximum){
}
}

/**Specify a maximum output. If a single parameter is specified, the minimum is
/**Specify a maximum output. If a single parameter is specified, the minimum is
* set to (-maximum).
* @param output
*/
void MiniPID::setOutputLimits(double output){ setOutputLimits(-output,output);}

/**
* Specify a maximum output.
* Specify a maximum output.
* @param minimum possible output value
* @param maximum possible output value
*/
Expand Down Expand Up @@ -194,11 +188,11 @@ double MiniPID::getOutput(double actual, double setpoint){
double Doutput;
double Foutput;

this->setpoint=setpoint;
this->setpoint=setpoint;

//Ramp the setpoint used for calculations if user has opted to do so
//Ramp the setpoint used for calculations if user has opted to do so
if(setpointRange!=0){
setpoint=constrain(setpoint,actual-setpointRange,actual+setpointRange);
setpoint=clamp(setpoint,actual-setpointRange,actual+setpointRange);
}

//Do the simple parts of the calculations
Expand Down Expand Up @@ -233,12 +227,11 @@ this->setpoint=setpoint;
// 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;
Ioutput=I*errorSum;
if(maxIOutput!=0){
Ioutput=constrain(Ioutput,-maxIOutput,maxIOutput);
Ioutput=clamp(Ioutput,-maxIOutput,maxIOutput);
}


//And, finally, we can just add the terms up
output=Foutput + Poutput + Ioutput + Doutput;

Expand All @@ -254,32 +247,28 @@ this->setpoint=setpoint;
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;
}
errorSum=clamp(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);
output=clamp(output, lastOutput-outputRampRate,lastOutput+outputRampRate);
}
if(minOutput!=maxOutput){
output=constrain(output, minOutput,maxOutput);
output=clamp(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
Expand All @@ -292,7 +281,7 @@ double MiniPID::getOutput(){
/**
*
* @param actual
* @return calculated output value for driving the actual to the target
* @return calculated output value for driving the actual to the target
*/
double MiniPID::getOutput(double actual){
return getOutput(actual,setpoint);
Expand Down Expand Up @@ -331,7 +320,7 @@ void MiniPID::setSetpointRange(double range){
*/
void MiniPID::setOutputFilter(double strength){
if(strength==0 || bounded(strength,0,1)){
outputFilter=strength;
outputFilter=strength;
}
}

Expand All @@ -346,7 +335,7 @@ void MiniPID::setOutputFilter(double strength){
* @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){
double MiniPID::clamp(double value, double min, double max){
if(value > max){ return max;}
if(value < min){ return min;}
return value;
Expand Down
45 changes: 22 additions & 23 deletions MiniPID.h
Original file line number Diff line number Diff line change
Expand Up @@ -3,33 +3,32 @@

class MiniPID{
public:
MiniPID(double p, double i, double d);
MiniPID(double p, double i, double d, double f);
void setP(double p);
void setI(double i);
void setD(double d);
void setF(double f);
void setPID(double p, double i, double d);
void setPID(double p, double i, double d,double f);
void setMaxIOutput(double maximum);
void setOutputLimits(double output);
void setOutputLimits(double minimum,double maximum);
void setDirection(bool reversed);
void setSetpoint(double setpoint);
MiniPID(double, double, double);
MiniPID(double, double, double, double);
void setP(double);
void setI(double);
void setD(double);
void setF(double);
void setPID(double, double, double);
void setPID(double, double, double, double);
void setMaxIOutput(double);
void setOutputLimits(double);
void setOutputLimits(double,double);
void setDirection(bool);
void setSetpoint(double);
void reset();
void setOutputRampRate(double rate);
void setSetpointRange(double range);
void setOutputFilter(double strength);
void setOutputRampRate(double);
void setSetpointRange(double);
void setOutputFilter(double);
double getOutput();
double getOutput(double actual);
double getOutput(double actual, double setpoint);
double getOutput(double);
double getOutput(double, double);

private:
double constrain(double value, double min, double max);
bool bounded(double value, double min, double max);
double clamp(double, double, double);
bool bounded(double, double, double);
void checkSigns();
void init();

double P;
double I;
double D;
Expand All @@ -46,8 +45,8 @@ class MiniPID{

double lastActual;

bool firstRun; //default true
bool reversed; //default false
bool firstRun;
bool reversed;

double outputRampRate;
double lastOutput;
Expand Down
56 changes: 0 additions & 56 deletions MiniPID.ino

This file was deleted.

9 changes: 5 additions & 4 deletions README.md
Original file line number Diff line number Diff line change
@@ -1,7 +1,8 @@
# minPID

This is a small, fully self contained PID class designed to help provide simple, efficient tuning, and simple integration wherever any level of PID control might be needed.

All code is contained in a single C++ class, and does't depend on any specific compiler or toolchain. However, example Arduino sketches are included.

## Design Goals
- Provide all expected features of a quality PID loop.
- Allow for stability without extensive tuning.
Expand Down Expand Up @@ -41,7 +42,7 @@ No need for lots of convoluted calculation functions, or asyncronous calculation
## Usage
A bare bones PID system could look like this.

``` java
``` cpp
MiniPID pid=MiniPID(1,0,0);
//set any other PID configuration options here.

Expand All @@ -50,7 +51,7 @@ while(true){
//set some sort of target value
double output=pid.getOutput(sensor,target);
//do something with the output
Timer.delay(50);
delay(50);
}
```
That's it. No fuss, no muss. A few lines of code and some basic tuning, and your PID system is in place.
Expand Down Expand Up @@ -98,7 +99,7 @@ Reverses the output. Use this if the controller attempts to go the wrong way dur
#### `reset()`
Resets the PID controller. This primarily clears the I and D terms clears the previous sensor state, and sets the target setpoint the the current position.

This is useful if the output system's state may have changed since the last time the PID system output was applied. This is generally the case when the output system is in a manual control mode (such as a joystick), or was disabled and may have been physically moved.
This is useful if the output system's state may have changed since the last time the PID system output was applied. This is generally the case when the output system is in a manual control mode (such as a joystick), or was disabled and may have been physically moved.

#### `setOutputRampRate(double rate)`
Set the maximum rate of change in a single calculation cycle. This is particularly useful for adding "inertial" to the system, preventing jerks during setpoint changes.
Expand Down
Loading

0 comments on commit 4daf467

Please sign in to comment.