Formula Student Autonomous Systems
The code for the main driverless system
Loading...
Searching...
No Matches
pid_test.cpp
Go to the documentation of this file.
1#include "longitudinal_controller/pid.hpp"
2
3#include "gtest/gtest.h"
4
9TEST(PidTests, TestAntiWindUp1) {
10 ControlParameters params;
11 params.pid_kp_ = 0.4;
12 params.pid_ki_ = 0.3;
13 params.pid_kd_ = 0.09;
14 params.pid_tau_ = 0.5;
15 params.pid_t_ = 0.01;
16 params.pid_lim_min_ = -1;
17 params.pid_lim_max_ = 1;
18 params.pid_anti_windup_ = 0.7;
19 params.pid_max_positive_error_ = 3;
20 params.pid_max_negative_error_ = -3;
21 PID pid(params);
22 pid.proportional_ = 0.3;
23 pid.integrator_ = 0.7;
24 pid.differentiator_ = 0.2;
25 pid.anti_wind_up();
26 EXPECT_FLOAT_EQ(0.7 * params.pid_anti_windup_, pid.integrator_);
27}
28
33TEST(PidTests, TestAntiWindUp2) {
34 ControlParameters params;
35 params.pid_kp_ = 0.4;
36 params.pid_ki_ = 0.3;
37 params.pid_kd_ = 0.09;
38 params.pid_tau_ = 0.5;
39 params.pid_t_ = 0.01;
40 params.pid_lim_min_ = -1;
41 params.pid_lim_max_ = 1;
42 params.pid_anti_windup_ = 0.7;
43 params.pid_max_positive_error_ = 3;
44 params.pid_max_negative_error_ = -3;
45 PID pid(params);
46 pid.proportional_ = -0.3;
47 pid.integrator_ = -0.7;
48 pid.differentiator_ = -0.2;
49 pid.anti_wind_up();
50 EXPECT_FLOAT_EQ(-0.7 * params.pid_anti_windup_, pid.integrator_);
51}
52
57TEST(PidTests, TestAntiWindUp3) {
58 ControlParameters params;
59 params.pid_kp_ = 0.4;
60 params.pid_ki_ = 0.3;
61 params.pid_kd_ = 0.09;
62 params.pid_tau_ = 0.5;
63 params.pid_t_ = 0.01;
64 params.pid_lim_min_ = -1;
65 params.pid_lim_max_ = 1;
66 params.pid_anti_windup_ = 0.7;
67 params.pid_max_positive_error_ = 3;
68 params.pid_max_negative_error_ = -3;
69 PID pid(params);
70 pid.proportional_ = 0.3;
71 pid.integrator_ = 0.3;
72 pid.differentiator_ = 0.2;
73 pid.anti_wind_up();
74 EXPECT_FLOAT_EQ(0.3, pid.integrator_);
75}
76
80TEST(PidTests, ProportionalTerm) {
81 ControlParameters params;
82 params.pid_kp_ = 0.4;
83 params.pid_ki_ = 0.3;
84 params.pid_kd_ = 0.09;
85 params.pid_tau_ = 0.7;
86 params.pid_t_ = 0.1;
87 params.pid_lim_min_ = -1;
88 params.pid_lim_max_ = 1;
89 params.pid_anti_windup_ = 0.5;
90 params.pid_max_positive_error_ = 3;
91 params.pid_max_negative_error_ = -3;
92 PID pid(params);
93 float error = 4;
94 pid.calculate_proportional_term(error);
95 EXPECT_FLOAT_EQ(1.6, pid.proportional_);
96}
97
102TEST(PidTests, IntegralTerm1) {
103 ControlParameters params;
104 params.pid_kp_ = 0.4;
105 params.pid_ki_ = 0.3;
106 params.pid_kd_ = 0.09;
107 params.pid_tau_ = 0.7;
108 params.pid_t_ = 0.1;
109 params.pid_lim_min_ = -1;
110 params.pid_lim_max_ = 1;
111 params.pid_anti_windup_ = 0.5;
112 params.pid_max_positive_error_ = 3;
113 params.pid_max_negative_error_ = -3;
114 PID pid(params);
115 float error = 3;
116 pid.integrator_ = 0.3;
117 pid.prev_error_ = 4;
118 pid.calculate_integral_term(error);
119 EXPECT_FLOAT_EQ(0.405, pid.integrator_);
120}
121
126TEST(PidTests, IntegralTerm2) {
127 ControlParameters params;
128 params.pid_kp_ = 0.4;
129 params.pid_ki_ = 0.3;
130 params.pid_kd_ = 0.09;
131 params.pid_tau_ = 0.7;
132 params.pid_t_ = 0.1;
133 params.pid_lim_min_ = -1;
134 params.pid_lim_max_ = 1;
135 params.pid_anti_windup_ = 0.5;
136 params.pid_max_positive_error_ = 3;
137 params.pid_max_negative_error_ = -3;
138 PID pid(params);
139 float error = -3;
140 pid.integrator_ = 0.3;
141 pid.prev_error_ = -4;
142 pid.calculate_integral_term(error);
143 EXPECT_FLOAT_EQ(0.195, pid.integrator_);
144}
145
150TEST(PidTests, DerivativeTerm1) {
151 ControlParameters params;
152 params.pid_kp_ = 0.4;
153 params.pid_ki_ = 0.3;
154 params.pid_kd_ = 0.1;
155 params.pid_tau_ = 0.7;
156 params.pid_t_ = 0.1;
157 params.pid_lim_min_ = -1;
158 params.pid_lim_max_ = 1;
159 params.pid_anti_windup_ = 0.45;
160 params.pid_max_positive_error_ = 3;
161 params.pid_max_negative_error_ = -3;
162 PID pid(params);
163 float measurement = 3;
164 pid.differentiator_ = 0.4;
165 pid.prev_measurement_ = 4;
166 pid.calculate_derivative_term(measurement);
167 EXPECT_FLOAT_EQ(0.48, pid.differentiator_);
168}
169
174TEST(PidTests, DerivativeTerm2) {
175 ControlParameters params;
176 params.pid_kp_ = 0.4;
177 params.pid_ki_ = 0.3;
178 params.pid_kd_ = 0.1;
179 params.pid_tau_ = 0.7;
180 params.pid_t_ = 0.1;
181 params.pid_lim_min_ = -1;
182 params.pid_lim_max_ = 1;
183 params.pid_anti_windup_ = 0.45;
184 params.pid_max_positive_error_ = 3;
185 params.pid_max_negative_error_ = -3;
186 PID pid(params);
187 float measurement = -4;
188 pid.differentiator_ = 0.4;
189 pid.prev_measurement_ = -1.2;
190 pid.calculate_derivative_term(measurement);
191 EXPECT_FLOAT_EQ(0.72, pid.differentiator_);
192}
193
198TEST(PidTests, Output1) {
199 ControlParameters params;
200 params.pid_kp_ = 0.4;
201 params.pid_ki_ = 0.3;
202 params.pid_kd_ = 0.09;
203 params.pid_tau_ = 0.7;
204 params.pid_t_ = 0.1;
205 params.pid_lim_min_ = -1;
206 params.pid_lim_max_ = 1;
207 params.pid_anti_windup_ = 0.45;
208 params.pid_max_positive_error_ = 3;
209 params.pid_max_negative_error_ = -3;
210 PID pid(params);
211 pid.proportional_ = 0.3;
212 pid.integrator_ = 0.1;
213 pid.differentiator_ = 0.4;
214 pid.compute_output();
215 EXPECT_FLOAT_EQ(0.8, pid.out_);
216}
217
222TEST(PidTests, Output2) {
223 ControlParameters params;
224 params.pid_kp_ = 0.4;
225 params.pid_ki_ = 0.3;
226 params.pid_kd_ = 0.09;
227 params.pid_tau_ = 0.7;
228 params.pid_t_ = 0.1;
229 params.pid_lim_min_ = -1;
230 params.pid_lim_max_ = 1;
231 params.pid_anti_windup_ = 0.45;
232 params.pid_max_positive_error_ = 3;
233 params.pid_max_negative_error_ = -3;
234 PID pid(params);
235 pid.proportional_ = 0.3;
236 pid.integrator_ = 0.3;
237 pid.differentiator_ = 0.6;
238 pid.compute_output();
239 EXPECT_FLOAT_EQ(1, pid.out_);
240}
241
246TEST(PidTests, Output3) {
247 ControlParameters params;
248 params.pid_kp_ = 0.4;
249 params.pid_ki_ = 0.3;
250 params.pid_kd_ = 0.09;
251 params.pid_tau_ = 0.7;
252 params.pid_t_ = 0.1;
253 params.pid_lim_min_ = -1;
254 params.pid_lim_max_ = 1;
255 params.pid_anti_windup_ = 0.45;
256 params.pid_max_positive_error_ = 3;
257 params.pid_max_negative_error_ = -3;
258 PID pid(params);
259 pid.proportional_ = -0.3;
260 pid.integrator_ = -0.3;
261 pid.differentiator_ = -0.6;
262 pid.compute_output();
263 EXPECT_FLOAT_EQ(-1, pid.out_);
264}
265
266// -2*kd(Measur - oldMeasur)+((2*tua-T)*oldDiffere)/(2*tau+T) + kp*Error + oldIntegra +
267// 0.5*ki*T*(Error+OldErro)
268
273TEST(PidTests, Update1) {
274 ControlParameters params;
275 params.pid_kp_ = 0.4;
276 params.pid_ki_ = 0.3;
277 params.pid_kd_ = 0.1;
278 params.pid_tau_ = 0.7;
279 params.pid_t_ = 0.1;
280 params.pid_lim_min_ = -1;
281 params.pid_lim_max_ = 1;
282 params.pid_anti_windup_ = 0.45;
283 params.pid_max_positive_error_ = 3;
284 params.pid_max_negative_error_ = -3;
285 PID pid(params);
286 float measurement = 2;
287 float setpoint = 3;
288 pid.integrator_ = 0.2;
289 pid.differentiator_ = 0.1;
290 pid.prev_error_ = 1.5;
291 pid.prev_measurement_ = 3.5;
292 pid.update(setpoint, measurement);
293 EXPECT_NEAR(0.924, pid.out_, 0.001);
294}
PID
PI-D Controller class.
Definition pid.hpp:18
PID::calculate_integral_term
void calculate_integral_term(double error)
Calculate the integral term.
Definition pid.cpp:72
PID::calculate_derivative_term
void calculate_derivative_term(double measurement)
Calculate the derivative term (derivative on measurement)
Definition pid.cpp:84
PID::compute_output
void compute_output()
Compute the output value and apply limits.
Definition pid.cpp:91
PID::differentiator_
double differentiator_
Differentiator term current value.
Definition pid.hpp:31
PID::proportional_
double proportional_
Proportional term current value.
Definition pid.hpp:29
PID::update
double update(double setpoint, double measurement)
Calculate the output value.
Definition pid.cpp:19
PID::out_
double out_
Current output value.
Definition pid.hpp:36
PID::integrator_
double integrator_
Integrator term current value.
Definition pid.hpp:30
PID::anti_wind_up
void anti_wind_up()
Anti-wind-up via dynamic integrator clamping.
Definition pid.cpp:77
PID::prev_measurement_
double prev_measurement_
Previous measurement value, required for defferentiator.
Definition pid.hpp:34
PID::prev_error_
double prev_error_
Previous error value, required for integrator.
Definition pid.hpp:33
PID::calculate_proportional_term
void calculate_proportional_term(double error)
Calculate the proportional term.
Definition pid.cpp:68
TEST
TEST(PidTests, TestAntiWindUp1)
Test PID class - AntiWindUp Anti windup when output is saturated (limMax)
Definition pid_test.cpp:9
ControlParameters::pid_max_positive_error_
double pid_max_positive_error_
Definition parameters.hpp:32
ControlParameters::pid_max_negative_error_
double pid_max_negative_error_
Definition parameters.hpp:33