14
14
#include < PID_v1.h>
15
15
16
16
/* Constructor (...)*********************************************************
17
- * The parameters specified here are those for for which we can't set up
17
+ * The parameters specified here are those for for which we can't set up
18
18
* reliable defaults, so we need to have the user set them.
19
19
***************************************************************************/
20
20
PID::PID (double * Input, double * Output, double * Setpoint,
21
- double Kp, double Ki, double Kd, int ControllerDirection)
21
+ double Kp, double Ki, double Kd, int POn, int ControllerDirection)
22
22
{
23
-
24
23
myOutput = Output;
25
24
myInput = Input;
26
25
mySetpoint = Setpoint;
27
- inAuto = false ;
28
-
29
- PID::SetOutputLimits (0 , 255 ); // default output limit corresponds to
26
+ inAuto = false ;
27
+
28
+ PID::SetOutputLimits (0 , 255 ); // default output limit corresponds to
30
29
// the arduino pwm limits
31
30
32
31
SampleTime = 100 ; // default Controller Sample Time is 0.1 seconds
33
32
34
33
PID::SetControllerDirection (ControllerDirection);
35
- PID::SetTunings (Kp, Ki, Kd);
34
+ PID::SetTunings (Kp, Ki, Kd, POn );
36
35
37
- lastTime = millis ()-SampleTime;
36
+ lastTime = millis ()-SampleTime;
38
37
}
39
-
40
-
38
+
39
+ /* Constructor (...)*********************************************************
40
+ * To allow backwards compatability for v1.1, or for people that just want
41
+ * to use Proportional on Error without explicitly saying so
42
+ ***************************************************************************/
43
+
44
+ PID::PID (double * Input, double * Output, double * Setpoint,
45
+ double Kp, double Ki, double Kd, int ControllerDirection)
46
+ :PID::PID(Input, Output, Setpoint, Kp, Ki, Kd, P_ON_E, ControllerDirection)
47
+ {
48
+
49
+ }
50
+
51
+
41
52
/* Compute() **********************************************************************
42
53
* This, as they say, is where the magic happens. this function should be called
43
54
* every time "void loop()" executes. the function will decide for itself whether a new
44
55
* pid Output needs to be computed. returns true when the output is computed,
45
56
* false when nothing has been done.
46
- **********************************************************************************/
57
+ **********************************************************************************/
47
58
bool PID::Compute ()
48
59
{
49
60
if (!inAuto) return false ;
@@ -52,55 +63,73 @@ bool PID::Compute()
52
63
if (timeChange>=SampleTime)
53
64
{
54
65
/* Compute all the working error variables*/
55
- double input = *myInput;
66
+ double input = *myInput;
56
67
double error = *mySetpoint - input;
57
- ITerm+= (ki * error);
58
- if (ITerm > outMax) ITerm= outMax;
59
- else if (ITerm < outMin) ITerm= outMin;
60
68
double dInput = (input - lastInput);
61
-
62
- /* Compute PID Output*/
63
- double output = kp * error + ITerm- kd * dInput;
64
-
65
- if (output > outMax) output = outMax;
69
+ outputSum+= (ki * error);
70
+
71
+ /* Add Proportional on Measurement, if P_ON_M is specified*/
72
+ if (!pOnE) outputSum-= kp * dInput;
73
+
74
+ if (outputSum > outMax) outputSum= outMax;
75
+ else if (outputSum < outMin) outputSum= outMin;
76
+
77
+ /* Add Proportional on Error, if P_ON_E is specified*/
78
+ double output;
79
+ if (pOnE) output = kp * error;
80
+ else output = 0 ;
81
+
82
+ /* Compute Rest of PID Output*/
83
+ output += outputSum - kd * dInput;
84
+
85
+ if (output > outMax) output = outMax;
66
86
else if (output < outMin) output = outMin;
67
- *myOutput = output;
68
-
87
+ *myOutput = output;
88
+
69
89
/* Remember some variables for next time*/
70
90
lastInput = input;
71
91
lastTime = now;
72
- return true ;
92
+ return true ;
73
93
}
74
94
else return false ;
75
95
}
76
96
77
-
78
97
/* SetTunings(...)*************************************************************
79
- * This function allows the controller's dynamic performance to be adjusted.
98
+ * This function allows the controller's dynamic performance to be adjusted.
80
99
* it's called automatically from the constructor, but tunings can also
81
100
* be adjusted on the fly during normal operation
82
- ******************************************************************************/
83
- void PID::SetTunings (double Kp, double Ki, double Kd)
101
+ ******************************************************************************/
102
+ void PID::SetTunings (double Kp, double Ki, double Kd, int POn )
84
103
{
85
104
if (Kp<0 || Ki<0 || Kd<0 ) return ;
86
-
105
+
106
+ pOn = POn;
107
+ pOnE = POn == P_ON_E;
108
+
87
109
dispKp = Kp; dispKi = Ki; dispKd = Kd;
88
-
89
- double SampleTimeInSec = ((double )SampleTime)/1000 ;
110
+
111
+ double SampleTimeInSec = ((double )SampleTime)/1000 ;
90
112
kp = Kp;
91
113
ki = Ki * SampleTimeInSec;
92
114
kd = Kd / SampleTimeInSec;
93
-
115
+
94
116
if (controllerDirection ==REVERSE)
95
117
{
96
118
kp = (0 - kp);
97
119
ki = (0 - ki);
98
120
kd = (0 - kd);
99
121
}
100
122
}
101
-
123
+
124
+ /* SetTunings(...)*************************************************************
125
+ * Set Tunings using the last-rembered POn setting
126
+ ******************************************************************************/
127
+ void PID::SetTunings (double Kp, double Ki, double Kd){
128
+ SetTunings (Kp, Ki, Kd, pOn);
129
+ }
130
+
102
131
/* SetSampleTime(...) *********************************************************
103
- * sets the period, in Milliseconds, at which the calculation is performed
132
+ * sets the period, in Milliseconds, at which the calculation is performed
104
133
******************************************************************************/
105
134
void PID::SetSampleTime (int NewSampleTime)
106
135
{
@@ -113,7 +142,7 @@ void PID::SetSampleTime(int NewSampleTime)
113
142
SampleTime = (unsigned long )NewSampleTime;
114
143
}
115
144
}
116
-
145
+
117
146
/* SetOutputLimits(...)****************************************************
118
147
* This function will be used far more often than SetInputLimits. while
119
148
* the input to the controller will generally be in the 0-1023 range (which is
@@ -127,22 +156,22 @@ void PID::SetOutputLimits(double Min, double Max)
127
156
if (Min >= Max) return ;
128
157
outMin = Min;
129
158
outMax = Max;
130
-
159
+
131
160
if (inAuto)
132
161
{
133
162
if (*myOutput > outMax) *myOutput = outMax;
134
163
else if (*myOutput < outMin) *myOutput = outMin;
135
-
136
- if (ITerm > outMax) ITerm = outMax;
137
- else if (ITerm < outMin) ITerm = outMin;
164
+
165
+ if (outputSum > outMax) outputSum = outMax;
166
+ else if (outputSum < outMin) outputSum = outMin;
138
167
}
139
168
}
140
169
141
170
/* SetMode(...)****************************************************************
142
171
* Allows the controller Mode to be set to manual (0) or Automatic (non-zero)
143
172
* when the transition from manual to auto occurs, the controller is
144
173
* automatically initialized
145
- ******************************************************************************/
174
+ ******************************************************************************/
146
175
void PID::SetMode (int Mode)
147
176
{
148
177
bool newAuto = (Mode == AUTOMATIC);
@@ -152,21 +181,21 @@ void PID::SetMode(int Mode)
152
181
}
153
182
inAuto = newAuto;
154
183
}
155
-
184
+
156
185
/* Initialize()****************************************************************
157
186
* does all the things that need to happen to ensure a bumpless transfer
158
187
* from manual to automatic mode.
159
- ******************************************************************************/
188
+ ******************************************************************************/
160
189
void PID::Initialize ()
161
190
{
162
- ITerm = *myOutput;
191
+ outputSum = *myOutput;
163
192
lastInput = *myInput;
164
- if (ITerm > outMax) ITerm = outMax;
165
- else if (ITerm < outMin) ITerm = outMin;
193
+ if (outputSum > outMax) outputSum = outMax;
194
+ else if (outputSum < outMin) outputSum = outMin;
166
195
}
167
196
168
197
/* SetControllerDirection(...)*************************************************
169
- * The PID will either be connected to a DIRECT acting process (+Output leads
198
+ * The PID will either be connected to a DIRECT acting process (+Output leads
170
199
* to +Input) or a REVERSE acting process(+Output leads to -Input.) we need to
171
200
* know which one, because otherwise we may increase the output when we should
172
201
* be decreasing. This is called from the constructor.
@@ -175,16 +204,16 @@ void PID::SetControllerDirection(int Direction)
175
204
{
176
205
if (inAuto && Direction !=controllerDirection)
177
206
{
178
- kp = (0 - kp);
207
+ kp = (0 - kp);
179
208
ki = (0 - ki);
180
209
kd = (0 - kd);
181
- }
210
+ }
182
211
controllerDirection = Direction;
183
212
}
184
213
185
214
/* Status Funcions*************************************************************
186
215
* Just because you set the Kp=-1 doesn't mean it actually happened. these
187
- * functions query the internal state of the PID. they're here for display
216
+ * functions query the internal state of the PID. they're here for display
188
217
* purposes. this are the functions the PID Front-end uses for example
189
218
******************************************************************************/
190
219
double PID::GetKp (){ return dispKp; }
0 commit comments