/**********************************************************************************************
* 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
#include "WProgram.h"
#endif
#include <PID_v1.h>
/*Constructor (...)*********************************************************
* 此处指定的参数是无法设置的参数
* 可靠的默认值，所以我们需要让用户设置它们。
***************************************************************************/
PID::PID(double* Input, double* Output, double* Setpoint,
double Kp, double Ki, double Kd, int POn, 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, POn);
lastTime = millis()-SampleTime;
}
/*Constructor (...)*********************************************************
* 允许 v1.1 的向后兼容性，或仅希望
* 使用错误比例， 而不明确这样说
***************************************************************************/
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() **********************************************************************
* 正如他们说的，这就是魔法发生的地方。 此函数应调用
* 每次执行"void 循环（）"。 函数将自行决定是否一个新的
* 需要计算 pid 输出。 计算输出时返回 true，
* 当什么都没做时是假的。
**********************************************************************************/
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;
}
/* SetTunings(...)*************************************************************
* 此功能允许调整控制器的动态性能。
* 它自动调用从构造函数， 但调优也可以
* 在正常操作期间进行飞行调整
******************************************************************************/
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;
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);
}
}
/* SetTunings(...)*************************************************************
* 使用上次重新记住的 POn 设置设置调整
******************************************************************************/
void PID::SetTunings(double Kp, double Ki, double Kd){
SetTunings(Kp, Ki, Kd, pOn);
}
/* SetSampleTime(...) *********************************************************
* 设置执行计算的期间（以毫秒为单位）
******************************************************************************/
void PID::SetSampleTime(int NewSampleTime)
{
if (NewSampleTime > 0)
{
double ratio = (double)NewSampleTime
/ (double)SampleTime;
ki *= ratio;
kd /= ratio;
SampleTime = (unsigned long)NewSampleTime;
}
}
/* SetOutputLimits(...)****************************************************
此函数的使用比 SetInputLimits 的频繁使用要频繁得多。 而对控制器的输入通常位于 0-1023 范围内（这是默认的），但输出将略有不同。 也许他们会做一个时间窗口， 将需要 0 - 80 或什么的。 或者他们会想从 0 - 125 夹住它 谁知道呢。 无论如何，这一切都可以在这里完成。
**************************************************************************/
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(outputSum > outMax) outputSum= outMax;
else if(outputSum < outMin) outputSum= outMin;
}
}
/* SetMode(...)****************************************************************
允许控制器模式设置为手动 （0） 或自动 （非零） 时， 从手动过渡到自动发生， 控制器自动初始化
******************************************************************************/
void PID::SetMode(int Mode)
{
bool newAuto = (Mode == AUTOMATIC);
if(newAuto && !inAuto)
{ /*we just went from manual to auto*/
PID::Initialize();
}
inAuto = newAuto;
}
/* Initialize()****************************************************************
*做所有需要发生的事情， 以确保从手动模式到自动模式的无颠簸传输。
******************************************************************************/
void PID::Initialize()
{
outputSum = *myOutput;
lastInput = *myInput;
if(outputSum > outMax) outputSum = outMax;
else if(outputSum < outMin) outputSum = outMin;
}
/* SetControllerDirection(...)*************************************************
*PID 将连接到直接作用过程（+输出导致 +输入）或反向作用过程（=输出引线到 -输入）。 我们需要知道是哪一个，否则我们可以增加产量时，我们应该减少。 这是从构造函数调用的。
******************************************************************************/
void PID::SetControllerDirection(int Direction)
{
if(inAuto && Direction !=controllerDirection)
{
kp = (0 - kp);
ki = (0 - ki);
kd = (0 - kd);
}
controllerDirection = Direction;
}
/* Status Funcions*************************************************************
*仅仅因为您设置了 Kp+-1 并不意味着它实际上发生了。 这些函数查询 PID 的内部状态。 他们来这里是为了显示目的。 这是 PID 前端使用的功能，例如
******************************************************************************/
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;}