Initial commit of PID algorithm for closed loop (NOT WORKING!)
This commit is contained in:
Josh Stewart
parent
a8e7066b41
commit
2b49db7639
|
|
@ -151,6 +151,9 @@ This continues until either:
|
|||
PID (Best suited to wideband sensors):
|
||||
|
||||
*/
|
||||
double PID_O2, PID_output, PID_AFRTarget;
|
||||
PID egoPID(&PID_O2, &PID_output, &PID_AFRTarget, configPage3.egoKP, configPage3.egoKI, configPage3.egoKD, REVERSE); //This is the PID object if that algorithm is used. Needs to be global as it maintains state outside of each function call
|
||||
|
||||
byte correctionsAFRClosedLoop()
|
||||
{
|
||||
if( (configPage3.egoAlgorithm == 3) || (configPage3.egoType == 0)) { return 100; } //An egoAlgorithm value of 3 means NO CORRECTION, egoType of 0 means no O2 sensor
|
||||
|
|
@ -196,6 +199,14 @@ byte correctionsAFRClosedLoop()
|
|||
{
|
||||
//*************************************************************************************************************************************
|
||||
//PID algorithm
|
||||
egoPID.SetOutputLimits((double)(-configPage3.egoLimit), (double)(configPage3.egoLimit)); //Set the limits again, just incase the user has changed them since the last loop. Note that these are sent to the PID library as (Eg:) -15 and +15
|
||||
egoPID.SetTunings(configPage3.egoKP, configPage3.egoKI, configPage3.egoKD); //Set the PID values again, just incase the user has changed them since the last loop
|
||||
PID_O2 = (double)(currentStatus.O2);
|
||||
PID_AFRTarget = (double)(currentStatus.afrTarget);
|
||||
|
||||
egoPID.Compute();
|
||||
//currentStatus.egoCorrection = 100 + PID_output;
|
||||
return (100 + PID_output);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,53 @@
|
|||
/********************************************************
|
||||
* PID Adaptive Tuning Example
|
||||
* One of the benefits of the PID library is that you can
|
||||
* change the tuning parameters at any time. this can be
|
||||
* helpful if we want the controller to be agressive at some
|
||||
* times, and conservative at others. in the example below
|
||||
* we set the controller to use Conservative Tuning Parameters
|
||||
* when we're near setpoint and more agressive Tuning
|
||||
* Parameters when we're farther away.
|
||||
********************************************************/
|
||||
|
||||
#include <PID_v1.h>
|
||||
|
||||
//Define Variables we'll be connecting to
|
||||
double Setpoint, Input, Output;
|
||||
|
||||
//Define the aggressive and conservative Tuning Parameters
|
||||
double aggKp=4, aggKi=0.2, aggKd=1;
|
||||
double consKp=1, consKi=0.05, consKd=0.25;
|
||||
|
||||
//Specify the links and initial tuning parameters
|
||||
PID myPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, DIRECT);
|
||||
|
||||
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);
|
||||
|
||||
double gap = abs(Setpoint-Input); //distance away from setpoint
|
||||
if(gap<10)
|
||||
{ //we're close to setpoint, use conservative tuning parameters
|
||||
myPID.SetTunings(consKp, consKi, consKd);
|
||||
}
|
||||
else
|
||||
{
|
||||
//we're far from setpoint, use aggressive tuning parameters
|
||||
myPID.SetTunings(aggKp, aggKi, aggKd);
|
||||
}
|
||||
|
||||
myPID.Compute();
|
||||
analogWrite(3,Output);
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,31 @@
|
|||
/********************************************************
|
||||
* PID Basic Example
|
||||
* Reading analog input 0 to control analog PWM output 3
|
||||
********************************************************/
|
||||
|
||||
#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, DIRECT);
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,60 @@
|
|||
/********************************************************
|
||||
* PID RelayOutput Example
|
||||
* Same as basic example, except that this time, the output
|
||||
* is going to a digital pin which (we presume) is controlling
|
||||
* a relay. the pid is designed to Output an analog value,
|
||||
* but the relay can only be On/Off.
|
||||
*
|
||||
* 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
|
||||
* 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
|
||||
* window being "Relay Off Time"
|
||||
********************************************************/
|
||||
|
||||
#include <PID_v1.h>
|
||||
#define RelayPin 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);
|
||||
|
||||
int WindowSize = 5000;
|
||||
unsigned long windowStartTime;
|
||||
void setup()
|
||||
{
|
||||
windowStartTime = millis();
|
||||
|
||||
//initialize the variables we're linked to
|
||||
Setpoint = 100;
|
||||
|
||||
//tell the PID to range between 0 and the full window size
|
||||
myPID.SetOutputLimits(0, WindowSize);
|
||||
|
||||
//turn the PID on
|
||||
myPID.SetMode(AUTOMATIC);
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
Input = analogRead(0);
|
||||
myPID.Compute();
|
||||
|
||||
/************************************************
|
||||
* turn the output pin on/off based on pid output
|
||||
************************************************/
|
||||
if(millis() - windowStartTime>WindowSize)
|
||||
{ //time to shift the Relay Window
|
||||
windowStartTime += WindowSize;
|
||||
}
|
||||
if(Output < millis() - windowStartTime) digitalWrite(RelayPin,HIGH);
|
||||
else digitalWrite(RelayPin,LOW);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,195 @@
|
|||
/**********************************************************************************************
|
||||
* Arduino PID Library - Version 1.0.1
|
||||
* by Brett Beauregard <br3ttb@gmail.com> brettbeauregard.com
|
||||
*
|
||||
* This Library is licensed under a GPLv3 License
|
||||
**********************************************************************************************/
|
||||
|
||||
#if ARDUINO >= 100
|
||||
#include "Arduino.h"
|
||||
#else
|
||||
#include "WProgram.h"
|
||||
#endif
|
||||
|
||||
#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.
|
||||
***************************************************************************/
|
||||
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
|
||||
|
||||
//SampleTime = 100; //default Controller Sample Time is 0.1 seconds
|
||||
|
||||
PID::SetControllerDirection(ControllerDirection);
|
||||
PID::SetTunings(Kp, Ki, Kd);
|
||||
|
||||
//lastTime = millis()-SampleTime;
|
||||
}
|
||||
|
||||
|
||||
/* 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();
|
||||
SampleTime = (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;
|
||||
}
|
||||
|
||||
|
||||
/* 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);
|
||||
}
|
||||
}
|
||||
|
||||
/* 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;
|
||||
}
|
||||
}
|
||||
|
||||
/* 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;
|
||||
}
|
||||
|
||||
/* 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;
|
||||
}
|
||||
|
||||
/* 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;
|
||||
}
|
||||
|
||||
/* 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;}
|
||||
|
||||
|
|
@ -0,0 +1,80 @@
|
|||
#ifndef PID_v1_h
|
||||
#define PID_v1_h
|
||||
#define LIBRARY_VERSION 1.0.0
|
||||
|
||||
class PID
|
||||
{
|
||||
|
||||
|
||||
public:
|
||||
|
||||
//Constants used in some of the functions below
|
||||
#define AUTOMATIC 1
|
||||
#define MANUAL 0
|
||||
#define DIRECT 0
|
||||
#define REVERSE 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)
|
||||
|
||||
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
|
||||
|
||||
|
||||
|
||||
//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
|
||||
// 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 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(); //
|
||||
|
||||
private:
|
||||
void Initialize();
|
||||
|
||||
double dispKp; // * we'll hold on to the tuning parameters in user-entered
|
||||
double dispKi; // format for display purposes
|
||||
double dispKd; //
|
||||
|
||||
double kp; // * (P)roportional Tuning Parameter
|
||||
double ki; // * (I)ntegral Tuning Parameter
|
||||
double kd; // * (D)erivative Tuning Parameter
|
||||
|
||||
int controllerDirection;
|
||||
|
||||
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;
|
||||
|
||||
unsigned long SampleTime;
|
||||
double outMin, outMax;
|
||||
bool inAuto;
|
||||
};
|
||||
#endif
|
||||
|
||||
|
|
@ -0,0 +1,34 @@
|
|||
#######################################
|
||||
# Syntax Coloring Map For PID Library
|
||||
#######################################
|
||||
|
||||
#######################################
|
||||
# Datatypes (KEYWORD1)
|
||||
#######################################
|
||||
|
||||
PID KEYWORD1
|
||||
|
||||
#######################################
|
||||
# Methods and Functions (KEYWORD2)
|
||||
#######################################
|
||||
|
||||
SetMode KEYWORD2
|
||||
Compute KEYWORD2
|
||||
SetOutputLimits KEYWORD2
|
||||
SetTunings KEYWORD2
|
||||
SetControllerDirection KEYWORD2
|
||||
SetSampleTime KEYWORD2
|
||||
GetKp KEYWORD2
|
||||
GetKi KEYWORD2
|
||||
GetKd KEYWORD2
|
||||
GetMode KEYWORD2
|
||||
GetDirection KEYWORD2
|
||||
|
||||
#######################################
|
||||
# Constants (LITERAL1)
|
||||
#######################################
|
||||
|
||||
AUTOMATIC LITERAL1
|
||||
MANUAL LITERAL1
|
||||
DIRECT LITERAL1
|
||||
REVERSE LITERAL1
|
||||
Loading…
Reference in New Issue