Compare commits

..

11 Commits

Author SHA1 Message Date
Chris Giacofei
2dde69d575 Point to Internet Archive.
Just in case this awesome site goes down in the future.
2024-05-02 13:01:34 -04:00
Chris Giacofei
d565cded53 Tracking down compile errors. 2024-05-01 16:13:35 -04:00
Chris Giacofei
b3255d94d4 Initial changes, merging PID and Autotune. 2024-05-01 15:38:01 -04:00
Chris Giacofei
7787498eda incorporate tuning into main library.
Not yet tested.
2024-04-26 08:42:17 -04:00
Brett
9b4ca0e5b6 version 2017-06-20 07:12:13 -07:00
Brett
691f1d7af8 version / license
Incorrect version and license in cpp and h
2017-06-20 07:09:25 -07:00
Brett
5cefbae8e5 Version 1.2
-Added capability for Proportional On Measurement
-Changed license from GPLv3 to MIT
2017-06-19 16:29:36 -07:00
br3ttb
5adeed52b0 Merge pull request #56 from zcx119/patch-1
Condition update for mode change from manual to auto.
2017-02-15 05:05:27 -08:00
zcx119
21b19e7ab0 Condition update for mode change from manual to auto.
Minor update on the condition of initialization when changing mode. To only detect mode change from manual to auto.
2017-02-14 23:38:57 +08:00
br3ttb
fb095d8cfc Merge pull request #27 from sciunto/examples
enhance coding style Examples
2015-05-10 14:38:16 -04:00
francois
d7c61d2556 enhance coding style 2015-05-09 20:20:53 +02:00
9 changed files with 459 additions and 234 deletions

View File

@ -1,195 +1,335 @@
/**********************************************************************************************
* Arduino PID Library - Version 1.1.1
* by Brett Beauregard <br3ttb@gmail.com> brettbeauregard.com
*
* This Library is licensed under a GPLv3 License
**********************************************************************************************/
* Arduino PID Library - Version 1.2.1
* by Brett Beauregard <br3ttb@gmail.com> brettbeauregard.com
*
* This Library is licensed under the MIT License
**********************************************************************************************/
#if ARDUINO >= 100
#include "Arduino.h"
#else
#else
#include "WProgram.h"
#endif
#include <PID_v1.h>
#include "PID_v1.h"
/*Constructor (...)*********************************************************
* The parameters specified here are those for for which we can't set up
* reliable defaults, so we need to have the user set them.
***************************************************************************/
* The parameters specified here are those for for which we can't set up
* reliable defaults, so we need to have the user set them.
***************************************************************************/
PID::PID(double* Input, double* Output, double* Setpoint,
double Kp, double Ki, double Kd, int ControllerDirection)
{
myOutput = Output;
myInput = Input;
mySetpoint = Setpoint;
inAuto = false;
PID::SetOutputLimits(0, 255); //default output limit corresponds to
//the arduino pwm limits
double Kp, double Ki, double Kd, int POn, int ControllerDirection) {
myOutput = Output;
myInput = Input;
mySetpoint = Setpoint;
inAuto = false;
//Tuner = new PID_ATune(Input, Output);
PID::OutputLimits(0, 100);
PID::Direction(ControllerDirection);
PID::SetTunings(Kp, Ki, Kd, POn);
SampleTime = 100; //default Controller Sample Time is 0.1 seconds
// Autotune defaults
noise_band = 0.5;
autotune_running = false;
oStep = 30;
LookbackSec(10);
PID::SetControllerDirection(ControllerDirection);
PID::SetTunings(Kp, Ki, Kd);
lastTime = millis()-SampleTime;
SampleTime = 100; //default Controller Sample Time is 0.1 seconds
lastTime = millis()-SampleTime;
}
/*Constructor (...)*********************************************************
* To allow backwards compatability for v1.1, or for people that just want
* to use Proportional on Error without explicitly saying so
***************************************************************************/
PID::PID(double* Input, double* Output, double* Setpoint,
double Kp, double Ki, double Kd, int ControllerDirection)
:PID::PID(Input, Output, Setpoint, Kp, Ki, Kd, P_ON_E, ControllerDirection) {
}
/* Compute() **********************************************************************
* This, as they say, is where the magic happens. this function should be called
* every time "void loop()" executes. the function will decide for itself whether a new
* pid Output needs to be computed. returns true when the output is computed,
* false when nothing has been done.
**********************************************************************************/
bool PID::Compute()
{
if(!inAuto) return false;
unsigned long now = millis();
unsigned long timeChange = (now - lastTime);
if(timeChange>=SampleTime)
{
/*Compute all the working error variables*/
double input = *myInput;
double error = *mySetpoint - input;
ITerm+= (ki * error);
if(ITerm > outMax) ITerm= outMax;
else if(ITerm < outMin) ITerm= outMin;
double dInput = (input - lastInput);
/*Compute PID Output*/
double output = kp * error + ITerm- kd * dInput;
if(output > outMax) output = outMax;
else if(output < outMin) output = outMin;
*myOutput = output;
/*Remember some variables for next time*/
lastInput = input;
lastTime = now;
return true;
}
else return false;
* This, as they say, is where the magic happens. this function should be called
* every time "void loop()" executes. the function will decide for itself whether a new
* pid Output needs to be computed. returns true when the output is computed,
* false when nothing has been done.
**********************************************************************************/
bool PID::Compute() {
if(!inAuto) return false;
unsigned long now = millis();
unsigned long timeChange = (now - lastTime);
if(timeChange>=SampleTime) {
/*Compute all the working error variables*/
double input = *myInput;
double error = *mySetpoint - input;
double dInput = (input - lastInput);
outputSum+= (ki * error);
/*Add Proportional on Measurement, if P_ON_M is specified*/
if(!pOnE) outputSum-= kp * dInput;
if(outputSum > outMax) outputSum= outMax;
else if(outputSum < outMin) outputSum= outMin;
/*Add Proportional on Error, if P_ON_E is specified*/
double output;
if(pOnE) output = kp * error;
else output = 0;
/*Compute Rest of PID Output*/
output += outputSum - kd * dInput;
if(output > outMax) output = outMax;
else if(output < outMin) output = outMin;
*myOutput = output;
/*Remember some variables for next time*/
lastInput = input;
lastTime = now;
return true;
}
else return false;
}
int PID::ComputeTune() {
//~ justevaled=false;
if(peakCount>9 && autotune_running) {
autotune_running = false;
FinishUp();
return 1;
}
unsigned long now = millis();
if((now-lastTime)<SampleTime) return false;
lastTime = now;
double refVal = *myInput;
//~ justevaled=true;
if(!autotune_running) {
//initialize working variables the first time around
peakType = 0;
peakCount=0;
atune_peak_change=false;
absMax=refVal;
absMin=refVal;
*mySetpoint = refVal;
autotune_running = true;
outputStart = *myOutput;
*myOutput = outputStart+oStep;
} else {
if(refVal>absMax)absMax=refVal;
if(refVal<absMin)absMin=refVal;
}
//oscillate the output base on the input's relation to the setpoint
if(refVal>*mySetpoint+noise_band) *myOutput = outputStart-oStep;
else if (refVal<*mySetpoint-noise_band) *myOutput = outputStart+oStep;
//bool isMax=true, isMin=true;
isMax=true;isMin=true;
//id peaks
for(int i=nLookBack-1;i>=0;i--) {
double val = lastInputs[i];
if(isMax) isMax = refVal>val;
if(isMin) isMin = refVal<val;
lastInputs[i+1] = lastInputs[i];
}
lastInputs[0] = refVal;
if(nLookBack<9){
//we don't want to trust the maxes or mins until the inputs array has been filled
return 0;
}
if(isMax) {
if(peakType==0)peakType=1;
if(peakType==-1) {
peakType = 1;
atune_peak_change=true;
peak2 = peak1;
}
peak1 = now;
peaks[peakCount] = refVal;
} else if(isMin) {
if(peakType==0)peakType=-1;
if(peakType==1){
peakType=-1;
peakCount++;
atune_peak_change=true;
}
if(peakCount<10)peaks[peakCount] = refVal;
}
if(atune_peak_change && peakCount>2) {
//we've transitioned. check if we can autotune based on the last peaks
double avgSeparation = (abs(peaks[peakCount-1]-peaks[peakCount-2])+abs(peaks[peakCount-2]-peaks[peakCount-3]))/2;
if( avgSeparation < 0.05*(absMax-absMin)){
FinishUp();
autotune_running = false;
return 1;
}
}
atune_peak_change=false;
return 0;
}
void PID::FinishUp(){
*myOutput = outputStart;
//we can generate tuning parameters!
Ku = 4*(2*oStep)/((absMax-absMin)*3.14159);
Pu = (double)(peak1-peak2) / 1000;
}
/* SetTunings(...)*************************************************************
* This function allows the controller's dynamic performance to be adjusted.
* it's called automatically from the constructor, but tunings can also
* be adjusted on the fly during normal operation
******************************************************************************/
void PID::SetTunings(double Kp, double Ki, double Kd)
{
if (Kp<0 || Ki<0 || Kd<0) return;
dispKp = Kp; dispKi = Ki; dispKd = Kd;
double SampleTimeInSec = ((double)SampleTime)/1000;
kp = Kp;
ki = Ki * SampleTimeInSec;
kd = Kd / SampleTimeInSec;
if(controllerDirection ==REVERSE)
{
kp = (0 - kp);
ki = (0 - ki);
kd = (0 - kd);
}
* This function allows the controller's dynamic performance to be adjusted.
* it's called automatically from the constructor, but tunings can also
* be adjusted on the fly during normal operation
******************************************************************************/
void PID::SetTunings(double Kp, double Ki, double Kd, int POn){
if (Kp<0 || Ki<0 || Kd<0) return;
pOn = POn;
pOnE = POn == P_ON_E;
double SampleTimeInSec = ((double)SampleTime)/1000;
kp = Kp;
ki = Ki * SampleTimeInSec;
kd = Kd / SampleTimeInSec;
if(controllerDirection ==REVERSE){
kp = (0 - kp);
ki = (0 - ki);
kd = (0 - kd);
}
}
/* SetTunings(...)*************************************************************
* Set Tunings using the last-rembered POn setting
******************************************************************************/
void PID::SetTunings(double Kp, double Ki, double Kd){
SetTunings(Kp, Ki, Kd, pOn);
}
/* SetSampleTime(...) *********************************************************
* sets the period, in Milliseconds, at which the calculation is performed
******************************************************************************/
void PID::SetSampleTime(int NewSampleTime)
{
if (NewSampleTime > 0)
{
double ratio = (double)NewSampleTime
/ (double)SampleTime;
ki *= ratio;
kd /= ratio;
SampleTime = (unsigned long)NewSampleTime;
}
}
/* SetOutputLimits(...)****************************************************
* This function will be used far more often than SetInputLimits. while
* the input to the controller will generally be in the 0-1023 range (which is
* the default already,) the output will be a little different. maybe they'll
* be doing a time window and will need 0-8000 or something. or maybe they'll
* want to clamp it from 0-125. who knows. at any rate, that can all be done
* here.
**************************************************************************/
void PID::SetOutputLimits(double Min, double Max)
{
if(Min >= Max) return;
outMin = Min;
outMax = Max;
if(inAuto)
{
if(*myOutput > outMax) *myOutput = outMax;
else if(*myOutput < outMin) *myOutput = outMin;
if(ITerm > outMax) ITerm= outMax;
else if(ITerm < outMin) ITerm= outMin;
}
* sets the period, in Milliseconds, at which the calculation is performed
******************************************************************************/
void PID::SetSampleTime(int NewSampleTime){
if (NewSampleTime > 0){
double ratio = (double)NewSampleTime
/ (double)SampleTime;
ki *= ratio;
kd /= ratio;
SampleTime = (unsigned long)NewSampleTime;
}
}
/* SetMode(...)****************************************************************
* Allows the controller Mode to be set to manual (0) or Automatic (non-zero)
* when the transition from manual to auto occurs, the controller is
* automatically initialized
******************************************************************************/
void PID::SetMode(int Mode)
{
bool newAuto = (Mode == AUTOMATIC);
if(newAuto == !inAuto)
{ /*we just went from manual to auto*/
PID::Initialize();
}
inAuto = newAuto;
/* OutputLimits(...)*******************************************************
* This function will be used far more often than SetInputLimits. while
* the input to the controller will generally be in the 0-1023 range (which is
* the default already,) the output will be a little different. maybe they'll
* be doing a time window and will need 0-8000 or something. or maybe they'll
* want to clamp it from 0-125. who knows. at any rate, that can all be done
* here.
**************************************************************************/
void PID::OutputLimits(double Min, double Max){
if(Min >= Max) return;
outMin = Min;
outMax = Max;
if(inAuto){
if(*myOutput > outMax) *myOutput = outMax;
else if(*myOutput < outMin) *myOutput = outMin;
if(outputSum > outMax) outputSum= outMax;
else if(outputSum < outMin) outputSum= outMin;
}
}
/* Mode(...)*******************************************************************
* Allows the controller Mode to be set to manual (0) or Automatic (non-zero)
* when the transition from manual to auto occurs, the controller is
* automatically initialized
******************************************************************************/
void PID::Mode(int Mode){
bool newAuto = (Mode == 1);
if(newAuto && !inAuto){ /*we just went from manual to auto*/
PID::Initialize();
}
inAuto = newAuto;
OpMode = Mode;
}
int PID::CycleMode() {
if (OpMode + 1 == 3) {
PID::Mode(0);
} else {
PID::Mode(OpMode + 1);
}
return OpMode;
}
/* Initialize()****************************************************************
* does all the things that need to happen to ensure a bumpless transfer
* from manual to automatic mode.
******************************************************************************/
void PID::Initialize()
{
ITerm = *myOutput;
lastInput = *myInput;
if(ITerm > outMax) ITerm = outMax;
else if(ITerm < outMin) ITerm = outMin;
* does all the things that need to happen to ensure a bumpless transfer
* from manual to automatic mode.
******************************************************************************/
void PID::Initialize(){
outputSum = *myOutput;
lastInput = *myInput;
if(outputSum > outMax) outputSum = outMax;
else if(outputSum < outMin) outputSum = outMin;
}
/* SetControllerDirection(...)*************************************************
* The PID will either be connected to a DIRECT acting process (+Output leads
* to +Input) or a REVERSE acting process(+Output leads to -Input.) we need to
* know which one, because otherwise we may increase the output when we should
* be decreasing. This is called from the constructor.
******************************************************************************/
void PID::SetControllerDirection(int Direction)
{
if(inAuto && Direction !=controllerDirection)
{
kp = (0 - kp);
ki = (0 - ki);
kd = (0 - kd);
}
controllerDirection = Direction;
/* Direction(...)**************************************************************
* The PID will either be connected to a DIRECT acting process (+Output leads
* to +Input) or a REVERSE acting process(+Output leads to -Input.) we need to
* know which one, because otherwise we may increase the output when we should
* be decreasing. This is called from the constructor.
******************************************************************************/
void PID::Direction(int Direction){
if(inAuto && Direction !=controllerDirection){
kp = (0 - kp);
ki = (0 - ki);
kd = (0 - kd);
}
controllerDirection = Direction;
}
/* Status Funcions*************************************************************
* Just because you set the Kp=-1 doesn't mean it actually happened. these
* functions query the internal state of the PID. they're here for display
* purposes. this are the functions the PID Front-end uses for example
******************************************************************************/
double PID::GetKp(){ return dispKp; }
double PID::GetKi(){ return dispKi;}
double PID::GetKd(){ return dispKd;}
int PID::GetMode(){ return inAuto ? AUTOMATIC : MANUAL;}
int PID::GetDirection(){ return controllerDirection;}
* Just because you set the Kp=-1 doesn't mean it actually happened. these
* functions query the internal state of the PID. they're here for display
* purposes. this are the functions the PID Front-end uses for example
******************************************************************************/
double PID::Kp(){ return kp; }
double PID::Ki(){ return ki;}
double PID::Kd(){ return kd;}
modes PID::Mode(){ return OpMode;}
int PID::Direction(){ return controllerDirection;}
void PID::Cancel(){ autotune_running = false;}
double PID::TunedKp(){ return 0.6 * Ku;}
double PID::TunedKi(){ return 1.2*Ku / Pu ;} // Ki = Kc/Ti}
double PID::TunedKd(){ return 0.075 * Ku * Pu;} //Kd = Kc * Td}
void PID::OutputStep(double Step){ oStep = Step;}
double PID::OutputStep(){ return oStep;}
void PID::NoiseBand(double Band){ noise_band = Band;}
double PID::NoiseBand(){ return noise_band;}
void PID::LookbackSec(int value){
if (value<1) value = 1;
if(value<25) {
nLookBack = value * 4;
SampleTime = 250;
} else {
nLookBack = 100;
SampleTime = value*10;
}
}
int PID::LookbackSec(){ return nLookBack * SampleTime / 1000;}

126
PID_v1.h
View File

@ -1,80 +1,116 @@
#ifndef PID_v1_h
#define PID_v1_h
#define LIBRARY_VERSION 1.1.1
#define LIBRARY_VERSION 1.2.1
enum modes : uint8_t {OFF, AUTOMATIC, MANUAL, OVERFLOW};
class PID
{
public:
//Constants used in some of the functions below
#define AUTOMATIC 1
#define MANUAL 0
#define DIRECT 0
#define REVERSE 1
#define P_ON_M 0
#define P_ON_E 1
//commonly used functions **************************************************************************
PID(double*, double*, double*, // * constructor. links the PID to the Input, Output, and
double, double, double, int); // Setpoint. Initial tuning parameters are also set here
void SetMode(int Mode); // * sets PID to either Manual (0) or Auto (non-0)
double, double, double, int, int);// Setpoint. Initial tuning parameters are also set here.
// (overload for specifying proportional mode)
PID(double*, double*, double*, // * constructor. links the PID to the Input, Output, and
double, double, double, int); // Setpoint. Initial tuning parameters are also set here
void Mode(int Mode); // * sets PID to either Manual or Auto
int CycleMode();
bool Compute(); // * performs the PID calculation. it should be
// called every time loop() cycles. ON/OFF and
// calculation frequency can be set using SetMode
// SetSampleTime respectively
void SetOutputLimits(double, double); //clamps the output to a specific range. 0-255 by default, but
//it's likely the user will want to change this depending on
//the application
int ComputeTune();
void OutputLimits(double, double); // * clamps the output to a specific range. 0-255 by default, but
// it's likely the user will want to change this depending on
// the application
//available but not commonly used functions ********************************************************
void SetTunings(double, double, // * While most users will set the tunings once in the
double); // constructor, this function gives the user the option
double); // constructor, this function gives the user the option
// of changing tunings during runtime for Adaptive control
void SetControllerDirection(int); // * Sets the Direction, or "Action" of the controller. DIRECT
// means the output will increase when error is positive. REVERSE
// means the opposite. it's very unlikely that this will be needed
// once it is set in the constructor.
void SetTunings(double, double, // * overload for specifying proportional mode
double, int);
void Direction(int); // * Sets the Direction, or "Action" of the controller. DIRECT
// means the output will increase when error is positive. REVERSE
// means the opposite. it's very unlikely that this will be needed
// once it is set in the constructor.
void SetSampleTime(int); // * sets the frequency, in Milliseconds, with which
// the PID calculation is performed. default is 100
//Display functions ****************************************************************
double GetKp(); // These functions query the pid for interal values.
double GetKi(); // they were created mainly for the pid front-end,
double GetKd(); // where it's important to know what is actually
int GetMode(); // inside the PID.
int GetDirection(); //
double Kp(); // These functions query the pid for interal values.
double Ki(); // they were created mainly for the pid front-end,
double Kd(); // where it's important to know what is actually
modes Mode(); // inside the PID.
int Direction(); //
// Auto Tune Public
void Cancel(); // * Stops the AutoTune
void OutputStep(double); // * how far above and below the starting value will the output step?
double OutputStep(); //
void LookbackSec(int); // * how far back are we looking to identify peaks
int LookbackSec(); //
void NoiseBand(double); // * the autotune will ignore signal chatter smaller than this value
double NoiseBand(); // this should be acurately set
double TunedKp(); // * once autotune is complete, these functions contain the
double TunedKi(); // computed tuning parameters.
double TunedKd();
private:
void Initialize();
double dispKp; // * we'll hold on to the tuning parameters in user-entered
double dispKi; // format for display purposes
double dispKd; //
void Initialize();
double kp; // * (P)roportional Tuning Parameter
double ki; // * (I)ntegral Tuning Parameter
double kd; // * (D)erivative Tuning Parameter
double kp; // * (P)roportional Tuning Parameter
double ki; // * (I)ntegral Tuning Parameter
double kd; // * (D)erivative Tuning Parameter
int controllerDirection;
int controllerDirection;
int pOn;
double *myInput; // * Pointers to the Input, Output, and Setpoint variables
double *myOutput; // This creates a hard link between the variables and the
double *mySetpoint; // PID, freeing the user from having to constantly tell us
// what these values are. with pointers we'll just know.
unsigned long lastTime;
double ITerm, lastInput;
double *myInput; // * Pointers to the Input, Output, and Setpoint variables
double *myOutput; // This creates a hard link between the variables and the
double *mySetpoint; // PID, freeing the user from having to constantly tell us
// what these values are. with pointers we'll just know.
unsigned long lastTime;
double outputSum, lastInput;
unsigned long SampleTime;
double outMin, outMax;
bool inAuto;
double outMin, outMax;
bool inAuto, pOnE;
modes OpMode;
// Autotune stuff
void FinishUp();
bool isMax, isMin;
double noise_band;
bool autotune_running;
unsigned long peak1, peak2;
int SampleTime;
int nLookBack;
int peakType;
double lastInputs[101];
double peaks[10];
int peakCount;
bool atune_peak_change;
double absMax, absMin;
double oStep;
double outputStart;
double Ku, Pu;
};
#endif

View File

@ -1,11 +1,11 @@
***************************************************************
* Arduino PID Library - Version 1.1.1
* Arduino PID Library - Version 1.2.1
* by Brett Beauregard <br3ttb@gmail.com> brettbeauregard.com
*
* This Library is licensed under a GPLv3 License
* This Library is licensed under the MIT License
***************************************************************
- For an ultra-detailed explanation of why the code is the way it is, please visit:
http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/
https://web.archive.org/web/20200607033952/http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction
- For function documentation see: http://playground.arduino.cc/Code/PIDLibrary

View File

@ -11,6 +11,9 @@
#include <PID_v1.h>
#define PIN_INPUT 0
#define PIN_OUTPUT 3
//Define Variables we'll be connecting to
double Setpoint, Input, Output;
@ -24,7 +27,7 @@ PID myPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, DIRECT);
void setup()
{
//initialize the variables we're linked to
Input = analogRead(0);
Input = analogRead(PIN_INPUT);
Setpoint = 100;
//turn the PID on
@ -33,10 +36,10 @@ void setup()
void loop()
{
Input = analogRead(0);
Input = analogRead(PIN_INPUT);
double gap = abs(Setpoint-Input); //distance away from setpoint
if(gap<10)
if (gap < 10)
{ //we're close to setpoint, use conservative tuning parameters
myPID.SetTunings(consKp, consKi, consKd);
}
@ -45,9 +48,9 @@ void loop()
//we're far from setpoint, use aggressive tuning parameters
myPID.SetTunings(aggKp, aggKi, aggKd);
}
myPID.Compute();
analogWrite(3,Output);
analogWrite(PIN_OUTPUT, Output);
}

View File

@ -5,16 +5,20 @@
#include <PID_v1.h>
#define PIN_INPUT 0
#define PIN_OUTPUT 3
//Define Variables we'll be connecting to
double Setpoint, Input, Output;
//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint,2,5,1, DIRECT);
double Kp=2, Ki=5, Kd=1;
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
void setup()
{
//initialize the variables we're linked to
Input = analogRead(0);
Input = analogRead(PIN_INPUT);
Setpoint = 100;
//turn the PID on
@ -23,9 +27,9 @@ void setup()
void loop()
{
Input = analogRead(0);
Input = analogRead(PIN_INPUT);
myPID.Compute();
analogWrite(3,Output);
analogWrite(PIN_OUTPUT, Output);
}

View File

@ -0,0 +1,36 @@
/********************************************************
* PID Proportional on measurement Example
* Setting the PID to use Proportional on measurement will
* make the output move more smoothly when the setpoint
* is changed. In addition, it can eliminate overshoot
* in certain processes like sous-vides.
********************************************************/
#include <PID_v1.h>
//Define Variables we'll be connecting to
double Setpoint, Input, Output;
//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint,2,5,1,P_ON_M, DIRECT); //P_ON_M specifies that Proportional on Measurement be used
//P_ON_E (Proportional on Error) is the default behavior
void setup()
{
//initialize the variables we're linked to
Input = analogRead(0);
Setpoint = 100;
//turn the PID on
myPID.SetMode(AUTOMATIC);
}
void loop()
{
Input = analogRead(0);
myPID.Compute();
analogWrite(3,Output);
}

View File

@ -7,28 +7,32 @@
*
* to connect them together we use "time proportioning
* control" it's essentially a really slow version of PWM.
* first we decide on a window size (5000mS say.) we then
* first we decide on a window size (5000mS say.) we then
* set the pid to adjust its output between 0 and that window
* size. lastly, we add some logic that translates the PID
* output into "Relay On Time" with the remainder of the
* output into "Relay On Time" with the remainder of the
* window being "Relay Off Time"
********************************************************/
#include <PID_v1.h>
#define RelayPin 6
#define PIN_INPUT 0
#define RELAY_PIN 6
//Define Variables we'll be connecting to
double Setpoint, Input, Output;
//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint,2,5,1, DIRECT);
double Kp=2, Ki=5, Kd=1;
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
int WindowSize = 5000;
unsigned long windowStartTime;
void setup()
{
windowStartTime = millis();
//initialize the variables we're linked to
Setpoint = 100;
@ -41,18 +45,18 @@ void setup()
void loop()
{
Input = analogRead(0);
Input = analogRead(PIN_INPUT);
myPID.Compute();
/************************************************
* turn the output pin on/off based on pid output
************************************************/
if(millis() - windowStartTime>WindowSize)
if (millis() - windowStartTime > WindowSize)
{ //time to shift the Relay Window
windowStartTime += WindowSize;
}
if(Output < millis() - windowStartTime) digitalWrite(RelayPin,HIGH);
else digitalWrite(RelayPin,LOW);
if (Output < millis() - windowStartTime) digitalWrite(RELAY_PIN, HIGH);
else digitalWrite(RELAY_PIN, LOW);
}

View File

@ -31,4 +31,6 @@ GetDirection KEYWORD2
AUTOMATIC LITERAL1
MANUAL LITERAL1
DIRECT LITERAL1
REVERSE LITERAL1
REVERSE LITERAL1
P_ON_E LITERAL1
P_ON_M LITERAL1

View File

@ -1,5 +1,5 @@
name=PID
version=1.1.1
version=1.2.1
author=Brett Beauregard
maintainer=Brett Beauregard
sentence=PID controller