Skip to content

Commit b712b0b

Browse files
ViktorCVSViktorCVS
committed
Add new antiwindup trapezoidal test
1 parent 5c473e3 commit b712b0b

File tree

2 files changed

+140
-10
lines changed

2 files changed

+140
-10
lines changed

‎control_toolbox/src/pid.cpp‎

Lines changed: 4 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -441,15 +441,13 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s)
441441
gains_.antiwindup_strat_.type == AntiWindupStrategy::BACK_CALCULATION &&
442442
!is_zero(gains_.i_gain_) && gains_.i_method_ == "trapezoidal")
443443
{
444-
const double num_i_last =
445-
(1.0 - dt_s / (2.0 * gains_.antiwindup_strat_.tracking_time_constant)) * i_term_last_;
444+
const double alpha = dt_s / (2.0 * gains_.antiwindup_strat_.tracking_time_constant);
445+
const double num_i_last = (1.0 - alpha) * i_term_last_;
446446
const double trap_inc = gains_.i_gain_ * (dt_s * 0.5) * (p_error_ + p_error_last_);
447447
const double aw_sum = (cmd_ - (p_term + d_term)) + aw_term_last_;
448-
const double denom = (1.0 + dt_s / (2.0 * gains_.antiwindup_strat_.tracking_time_constant));
448+
const double denom = (1.0 + alpha);
449449

450-
i_term_ = (num_i_last + trap_inc +
451-
(dt_s / (2.0 * gains_.antiwindup_strat_.tracking_time_constant)) * aw_sum) /
452-
denom;
450+
i_term_ = (num_i_last + trap_inc + alpha * aw_sum) / denom;
453451
}
454452
}
455453

‎control_toolbox/test/pid_tests.cpp‎

Lines changed: 136 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1031,12 +1031,12 @@ TEST(CommandTest, completePIDTest)
10311031
EXPECT_EQ(-0.5, cmd);
10321032
}
10331033

1034-
TEST(CommandTest, backCalculationPIDTest)
1034+
TEST(CommandTest, backCalculationForwardPIDTest)
10351035
{
10361036
RecordProperty(
10371037
"description",
1038-
"This test checks that a command is computed correctly using a complete PID controller with "
1039-
"back calculation technique.");
1038+
"This test checks that a command is computed correctly using a PID controller with "
1039+
"back calculation technique and forward discretization.");
10401040

10411041
// Pid(double p, double i, double d, double u_max, double u_min,
10421042
// AntiWindupStrategy antiwindup_strat);
@@ -1046,7 +1046,7 @@ TEST(CommandTest, backCalculationPIDTest)
10461046
antiwindup_strat.i_max = 10.0;
10471047
antiwindup_strat.i_min = -10.0;
10481048
antiwindup_strat.tracking_time_constant = 1.0; // Set to 0.0 to use the default value
1049-
Pid pid(0.0, 1.0, 0.0, 5.0, -5.0, antiwindup_strat);
1049+
Pid pid(0.0, 1.0, 0.0, 0.0, 5.0, -5.0, antiwindup_strat, "forward_euler", "forward_euler");
10501050

10511051
double cmd = 0.0;
10521052
double pe, ie, de;
@@ -1096,6 +1096,138 @@ TEST(CommandTest, backCalculationPIDTest)
10961096
EXPECT_EQ(4.0, cmd);
10971097
}
10981098

1099+
TEST(CommandTest, backCalculationBackwardPIDTest)
1100+
{
1101+
RecordProperty(
1102+
"description",
1103+
"This test checks that a command is computed correctly using a PID controller with "
1104+
"back calculation technique and backward discretization.");
1105+
1106+
// Pid(double p, double i, double d, double u_max, double u_min,
1107+
// AntiWindupStrategy antiwindup_strat);
1108+
// Setting u_max = 5.0 and u_min = -5.0 to test clamping
1109+
AntiWindupStrategy antiwindup_strat;
1110+
antiwindup_strat.type = AntiWindupStrategy::BACK_CALCULATION;
1111+
antiwindup_strat.i_max = 10.0;
1112+
antiwindup_strat.i_min = -10.0;
1113+
antiwindup_strat.tracking_time_constant = 1.0; // Set to 0.0 to use the default value
1114+
Pid pid(0.0, 1.0, 0.0, 0.0, 5.0, -5.0, antiwindup_strat, "backward_euler", "forward_euler");
1115+
1116+
double cmd = 0.0;
1117+
double pe, ie, de;
1118+
1119+
// Small error to not have saturation
1120+
cmd = pid.compute_command(1.0, 1.0);
1121+
pid.get_current_pid_errors(pe, ie, de);
1122+
EXPECT_EQ(1.0, ie);
1123+
EXPECT_EQ(1.0, cmd);
1124+
1125+
// Small error to not have saturation
1126+
cmd = pid.compute_command(2.0, 1.0);
1127+
pid.get_current_pid_errors(pe, ie, de);
1128+
EXPECT_EQ(3.0, ie);
1129+
EXPECT_EQ(3.0, cmd);
1130+
1131+
// Error to cause saturation
1132+
cmd = pid.compute_command(3.0, 1.0);
1133+
pid.get_current_pid_errors(pe, ie, de);
1134+
EXPECT_EQ(5.5, ie); // Reduced from 6.0 (1.0 + 2.0 + 3.0) to 5.5 due to back-calculation
1135+
EXPECT_EQ(5.0, cmd);
1136+
1137+
// Saturation applied, back calculation now reduces the integral term
1138+
cmd = pid.compute_command(1.0, 1.0);
1139+
pid.get_current_pid_errors(pe, ie, de);
1140+
EXPECT_EQ(5.75, ie); // Reduced from 6.5 (5.5 + 1.0) to 5.75 due to back-calculation
1141+
EXPECT_EQ(5.0, cmd);
1142+
1143+
// Saturation applied, back calculation now reduces the integral term
1144+
cmd = pid.compute_command(2.0, 1.0);
1145+
pid.get_current_pid_errors(pe, ie, de);
1146+
EXPECT_EQ(6.375, ie); // Reduced from 7.75 (5.75 + 2.0) to 6.375 due to back-calculation
1147+
EXPECT_EQ(5.0, cmd);
1148+
1149+
// Saturation applied, back calculation now reduces the integral term
1150+
cmd = pid.compute_command(-1.0, 1.0);
1151+
pid.get_current_pid_errors(pe, ie, de);
1152+
EXPECT_EQ(5.1875, ie); // Reduced from 5.375 (6.375 - 1.0) to 5.1875 due to back-calculation
1153+
EXPECT_EQ(5.0, cmd);
1154+
1155+
// PID recover from the windup/saturation
1156+
cmd = pid.compute_command(-1.0, 1.0);
1157+
pid.get_current_pid_errors(pe, ie, de);
1158+
EXPECT_EQ(4.1875, ie);
1159+
EXPECT_EQ(4.1875, cmd);
1160+
}
1161+
1162+
TEST(CommandTest, backCalculationTrapezoidalPIDTest)
1163+
{
1164+
RecordProperty(
1165+
"description",
1166+
"This test checks that a command is computed correctly using a PID controller with "
1167+
"back calculation technique and trapezoidal discretization.");
1168+
1169+
// Pid(double p, double i, double d, double u_max, double u_min,
1170+
// AntiWindupStrategy antiwindup_strat);
1171+
// Setting u_max = 5.0 and u_min = -5.0 to test clamping
1172+
AntiWindupStrategy antiwindup_strat;
1173+
antiwindup_strat.type = AntiWindupStrategy::BACK_CALCULATION;
1174+
antiwindup_strat.i_max = 10.0;
1175+
antiwindup_strat.i_min = -10.0;
1176+
antiwindup_strat.tracking_time_constant = 1.0; // Set to 0.0 to use the default value
1177+
Pid pid(0.0, 1.0, 0.0, 0.0, 5.0, -5.0, antiwindup_strat, "trapezoidal", "forward_euler");
1178+
1179+
double cmd = 0.0;
1180+
double pe, ie, de;
1181+
1182+
// Small error to not have saturation
1183+
cmd = pid.compute_command(1.0, 1.0);
1184+
pid.get_current_pid_errors(pe, ie, de);
1185+
EXPECT_NEAR(0.5, ie, EPS);
1186+
EXPECT_NEAR(0.5, cmd, EPS);
1187+
1188+
// Small error to not have saturation
1189+
cmd = pid.compute_command(2.0, 1.0);
1190+
pid.get_current_pid_errors(pe, ie, de);
1191+
EXPECT_NEAR(2.0, ie, EPS);
1192+
EXPECT_NEAR(2.0, cmd, EPS);
1193+
1194+
// Error to cause saturation
1195+
cmd = pid.compute_command(3.0, 1.0);
1196+
pid.get_current_pid_errors(pe, ie, de);
1197+
EXPECT_NEAR(4.5, ie, EPS);
1198+
EXPECT_NEAR(4.5, cmd, EPS);
1199+
1200+
// Error to cause saturation
1201+
cmd = pid.compute_command(5.0, 1.0);
1202+
pid.get_current_pid_errors(pe, ie, de);
1203+
EXPECT_NEAR(22.0 / 3.0, ie, EPS); // 7.33...
1204+
EXPECT_NEAR(5.0, cmd, EPS);
1205+
1206+
// Saturation applied, back calculation now reduces the integral term
1207+
cmd = pid.compute_command(1.0, 1.0);
1208+
pid.get_current_pid_errors(pe, ie, de);
1209+
EXPECT_NEAR(70.0 / 9.0, ie, EPS); // 7.77...
1210+
EXPECT_NEAR(5.0, cmd, EPS);
1211+
1212+
// Saturation applied, back calculation now reduces the integral term
1213+
cmd = pid.compute_command(2.0, 1.0);
1214+
pid.get_current_pid_errors(pe, ie, de);
1215+
EXPECT_NEAR(187.0 / 27.0, ie, EPS); // 6.92592592
1216+
EXPECT_NEAR(5.0, cmd, EPS);
1217+
1218+
// Saturation applied, back calculation now reduces the integral term
1219+
cmd = pid.compute_command(-2.0, 1.0);
1220+
pid.get_current_pid_errors(pe, ie, de);
1221+
EXPECT_NEAR(457.0 / 81.0, ie, EPS); // 5.12962963
1222+
EXPECT_NEAR(5.0, cmd, EPS);
1223+
1224+
// PID recover from the windup/saturation
1225+
cmd = pid.compute_command(-1.0, 1.0);
1226+
pid.get_current_pid_errors(pe, ie, de);
1227+
EXPECT_NEAR(1909.0 / 486.0, ie, EPS); // 3.92798353
1228+
EXPECT_NEAR(671.0 / 162.0, cmd, EPS); // 4.14197531
1229+
}
1230+
10991231
TEST(CommandTest, conditionalIntegrationPIDTest)
11001232
{
11011233
RecordProperty(

0 commit comments

Comments
 (0)