/
controler_pid.c
233 lines (195 loc) · 8.08 KB
/
controler_pid.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
/**********************************************************************************************
* portage of Arduino PID Library - Version 1.1.1 TO C language, chibios 2.6 RTOS
* by Brett Beauregard <br3ttb@gmail.com> brettbeauregard.com
*
* This Library is licensed under a GPLv3 License
* portage by alexandre bustico <alexandre.bustico@enac.fr>
**********************************************************************************************/
#include "controler_pid.h"
#include "ch.h"
#include "hal.h"
#include "stdutil.h"
static inline timecnt_t timediff(timecnt_t start, timecnt_t stop) {
#if defined CONTROLER_PID_HIRES_TIMER
return stop - start;
#else
return chTimeDiffX(start, stop);
#endif
}
static inline float diff2seconds(timecnt_t diff) {
#if defined CONTROLER_PID_HIRES_TIMER
return ((float)diff) / STM32_SYSCLK;
#else
return TIME_I2US(diff) / 1e6f ;
#endif
}
static inline timecnt_t timegetnow(void) {
#if defined CONTROLER_PID_HIRES_TIMER
return chSysGetRealtimeCounterX();
#else
return chVTGetSystemTimeX();
#endif
}
static void pidAdjustCoeffs (ControlerPid* cpid);
/*Constructor (...)*********************************************************
* The parameters specified here are those for which we can't set up
* reliable defaults, so we need to have the user set them.
***************************************************************************/
void pidInit (ControlerPid* cpid, volatile float* Input,
volatile float* Output, volatile float* Setpoint,
float Kp, float Ki, float Kd,
PidDirection ControllerDirection)
{
cpid->myOutput = Output;
cpid->myInput = Input;
cpid->mySetpoint = Setpoint;
cpid->inAuto = false;
pidSetOutputLimits(cpid, -100.0f, 100.0f); //default output limit
pidSetControllerDirection(cpid, ControllerDirection);
pidSetTunings(cpid, Kp, Ki, Kd);
cpid->lastTime = timegetnow();
}
/* 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 pidCompute(ControlerPid* cpid)
{
if(!cpid->inAuto) return false;
const timecnt_t now = timegetnow();
const timecnt_t timeChange = timediff(cpid->lastTime, now);
cpid->sampleTime = timeChange;
pidAdjustCoeffs (cpid);
/*Compute all the working error variables*/
const float input = *(cpid->myInput);
const float error = *(cpid->mySetpoint) - input;
cpid->iTerm += (cpid->ki * error);
if (cpid->iTerm > cpid->outMax) {
cpid->iTerm= cpid->outMax;
} else if (cpid->iTerm < cpid->outMin) {
cpid->iTerm= cpid->outMin;
}
const float dInput = (input - cpid->lastInput);
/*Compute PID Output*/
float output = (cpid->kp * error) + cpid->iTerm - (cpid->kd * dInput);
if(output > cpid->outMax) {
output = cpid->outMax;
} else if(output < cpid->outMin) {
output = cpid->outMin;
}
*(cpid->myOutput) = output;
/*Remember some variables for next time*/
cpid->lastInput = input;
cpid->lastTime = now;
return true;
}
/* 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 pidSetTunings(ControlerPid* cpid, float Kp, float Ki, float Kd)
{
if (Kp<0.0f || Ki<0.0f || Kd<0.0f) return;
cpid->dispKp = Kp; cpid->dispKi = Ki; cpid->dispKd = Kd;
cpid->kp = Kp;
if (cpid->controllerDirection ==PID_REVERSE) {
cpid->kp = (0.0f - cpid->kp);
}
}
static void pidAdjustCoeffs (ControlerPid* cpid)
{
const float sampleTimeInSec = diff2seconds(cpid->sampleTime);
cpid->ki = cpid->dispKi * sampleTimeInSec;
cpid->kd = cpid->dispKd / sampleTimeInSec;
if (cpid->controllerDirection == PID_REVERSE) {
cpid->ki = (0.0f - cpid->ki);
cpid->kd = (0.0f - cpid->kd);
}
}
/* 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 pidSetOutputLimits(ControlerPid* cpid, float Min, float Max)
{
if(Min >= Max) return;
cpid->outMin = Min;
cpid->outMax = Max;
if(cpid->inAuto) {
if(*(cpid->myOutput) > cpid->outMax) {
*(cpid->myOutput) = cpid->outMax;
} else if(*(cpid->myOutput) < cpid->outMin) {
*(cpid->myOutput) = cpid->outMin;
}
if(cpid->iTerm > cpid->outMax) {
cpid->iTerm= cpid->outMax;
} else if(cpid->iTerm < cpid->outMin) {
cpid->iTerm= cpid->outMin;
}
}
}
void pidSetOutputLimitsRatio(ControlerPid* cpid, float ratio)
{
pidSetOutputLimits (cpid, cpid->outMin*ratio, cpid->outMax*ratio);
}
/* 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 pidSetMode(ControlerPid* cpid, PidMode Mode)
{
bool newAuto = (Mode == PID_AUTOMATIC);
if(newAuto == !cpid->inAuto) {
/*we just went from manual to auto*/
pidInitialize(cpid);
cpid->inAuto = newAuto;
}
}
/* Initialize()****************************************************************
* does all the things that need to happen to ensure a bumpless transfer
* from manual to automatic mode.
******************************************************************************/
void pidInitialize(ControlerPid* cpid)
{
cpid->iTerm = *(cpid->myOutput);
cpid->lastInput = *(cpid->myInput);
if(cpid->iTerm > cpid->outMax) {
cpid->iTerm = cpid->outMax;
} else if(cpid->iTerm < cpid->outMin) {
cpid->iTerm = cpid->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 pidSetControllerDirection(ControlerPid* cpid, PidDirection Direction)
{
if(cpid->inAuto && Direction !=cpid->controllerDirection) {
cpid->kp = (0.0f - cpid->kp);
cpid->ki = (0.0f - cpid->ki);
cpid->kd = (0.0f - cpid->kd);
}
cpid->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
******************************************************************************/
float pidGetKp(ControlerPid* cpid){ return cpid->dispKp; }
float pidGetKi(ControlerPid* cpid){ return cpid->dispKi;}
float pidGetKd(ControlerPid* cpid){ return cpid->dispKd;}
PidMode pidGetMode(ControlerPid* cpid){ return cpid->inAuto ? PID_AUTOMATIC : PID_MANUAL;}
PidDirection pidGetDirection(ControlerPid* cpid){ return cpid->controllerDirection;}
timecnt_t pidGetSampleTime (ControlerPid* cpid){ return cpid->sampleTime;}