Skip to content

Commit b419fc3

Browse files
christophfroehlichchristophfroehlich
committed
Remove double check
1 parent 64011ec commit b419fc3

File tree

5 files changed

+34
-35
lines changed

5 files changed

+34
-35
lines changed

‎control_toolbox/include/control_toolbox/pid.hpp‎

Lines changed: 10 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -139,6 +139,13 @@ struct AntiWindupStrategy
139139
}
140140
}
141141

142+
void print() const
143+
{
144+
std::cout << "antiwindup_strat: " << to_string() << "\ti_max: " << i_max << ", i_min: " << i_min
145+
<< "\ttracking_time_constant: " << tracking_time_constant
146+
<< "\terror_deadband: " << error_deadband << std::endl;
147+
}
148+
142149
operator std::string() const { return to_string(); }
143150

144151
constexpr bool operator==(Value other) const { return type == other; }
@@ -282,8 +289,6 @@ class Pid
282289
: p_gain_(p),
283290
i_gain_(i),
284291
d_gain_(d),
285-
i_max_(antiwindup_strat.i_max),
286-
i_min_(antiwindup_strat.i_min),
287292
u_max_(u_max),
288293
u_min_(u_min),
289294
antiwindup_strat_(antiwindup_strat)
@@ -303,12 +308,7 @@ class Pid
303308

304309
bool validate(std::string & error_msg) const
305310
{
306-
if (i_min_ > i_max_)
307-
{
308-
error_msg = fmt::format("Gains: i_min ({}) must be less than i_max ({})", i_min_, i_max_);
309-
return false;
310-
}
311-
else if (u_min_ >= u_max_)
311+
if (u_min_ >= u_max_)
312312
{
313313
error_msg = fmt::format("Gains: u_min ({}) must be less than u_max ({})", u_min_, u_max_);
314314
return false;
@@ -333,9 +333,8 @@ class Pid
333333
void print() const
334334
{
335335
std::cout << "Gains: p: " << p_gain_ << ", i: " << i_gain_ << ", d: " << d_gain_
336-
<< ", i_max: " << i_max_ << ", i_min: " << i_min_ << ", u_max: " << u_max_
337-
<< ", u_min: " << u_min_ << ", antiwindup_strat: " << antiwindup_strat_.to_string()
338-
<< std::endl;
336+
<< ", u_max: " << u_max_ << ", u_min: " << u_min_ << std::endl;
337+
antiwindup_strat_.print();
339338
}
340339

341340
double p_gain_ = 0.0; /**< Proportional gain. */

‎control_toolbox/src/pid.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -321,7 +321,7 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s)
321321
}
322322
}
323323

324-
i_term_ = std::clamp(i_term_, gains_.i_min_, gains_.i_max_);
324+
i_term_ = std::clamp(i_term_, gains_.antiwindup_strat_.i_min, gains_.antiwindup_strat_.i_max);
325325

326326
return cmd_;
327327
}

‎control_toolbox/src/pid_ros.cpp‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -410,12 +410,12 @@ void PidROS::print_values()
410410
<< " P Gain: " << gains.p_gain_ << "\n"
411411
<< " I Gain: " << gains.i_gain_ << "\n"
412412
<< " D Gain: " << gains.d_gain_ << "\n"
413-
<< " I Max: " << gains.i_max_ << "\n"
414-
<< " I Min: " << gains.i_min_ << "\n"
415413
<< " U_Max: " << gains.u_max_ << "\n"
416414
<< " U_Min: " << gains.u_min_ << "\n"
417-
<< " Tracking_Time_Constant: " << gains.antiwindup_strat_.tracking_time_constant << "\n"
418415
<< " Antiwindup_Strategy: " << gains.antiwindup_strat_.to_string() << "\n"
416+
<< " Tracking_Time_Constant: " << gains.antiwindup_strat_.tracking_time_constant << "\n"
417+
<< " I Max: " << gains.antiwindup_strat_.i_max << "\n"
418+
<< " I Min: " << gains.antiwindup_strat_.i_min << "\n"
419419
<< "\n"
420420
<< " P Error: " << p_error << "\n"
421421
<< " I Term: " << i_term << "\n"
@@ -502,12 +502,12 @@ void PidROS::set_parameter_event_callback()
502502
}
503503
else if (param_name == param_prefix_ + "i_clamp_max")
504504
{
505-
gains.i_max_ = parameter.get_value<double>();
505+
gains.antiwindup_strat_.i_max = parameter.get_value<double>();
506506
changed = true;
507507
}
508508
else if (param_name == param_prefix_ + "i_clamp_min")
509509
{
510-
gains.i_min_ = parameter.get_value<double>();
510+
gains.antiwindup_strat_.i_min = parameter.get_value<double>();
511511
changed = true;
512512
}
513513
else if (param_name == param_prefix_ + "u_clamp_max")

‎control_toolbox/test/pid_ros_parameters_tests.cpp‎

Lines changed: 12 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -105,8 +105,8 @@ void check_set_parameters(
105105
ASSERT_EQ(gains.p_gain_, P);
106106
ASSERT_EQ(gains.i_gain_, I);
107107
ASSERT_EQ(gains.d_gain_, D);
108-
ASSERT_EQ(gains.i_max_, I_MAX);
109-
ASSERT_EQ(gains.i_min_, I_MIN);
108+
ASSERT_EQ(gains.antiwindup_strat_.i_max, I_MAX);
109+
ASSERT_EQ(gains.antiwindup_strat_.i_min, I_MIN);
110110
ASSERT_EQ(gains.u_max_, U_MAX);
111111
ASSERT_EQ(gains.u_min_, U_MIN);
112112
ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, TRK_TC);
@@ -170,8 +170,8 @@ TEST(PidParametersTest, InitPidTestBadParameter)
170170
ASSERT_EQ(gains.p_gain_, 0.0);
171171
ASSERT_EQ(gains.i_gain_, 0.0);
172172
ASSERT_EQ(gains.d_gain_, 0.0);
173-
ASSERT_EQ(gains.i_max_, std::numeric_limits<double>::infinity());
174-
ASSERT_EQ(gains.i_min_, -std::numeric_limits<double>::infinity());
173+
ASSERT_EQ(gains.antiwindup_strat_.i_max, std::numeric_limits<double>::infinity());
174+
ASSERT_EQ(gains.antiwindup_strat_.i_min, -std::numeric_limits<double>::infinity());
175175
ASSERT_EQ(gains.u_max_, std::numeric_limits<double>::infinity());
176176
ASSERT_EQ(gains.u_min_, -std::numeric_limits<double>::infinity());
177177
ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, 0.0);
@@ -303,8 +303,8 @@ TEST(PidParametersTest, SetParametersTest)
303303
ASSERT_EQ(gains.p_gain_, P);
304304
ASSERT_EQ(gains.i_gain_, I);
305305
ASSERT_EQ(gains.d_gain_, D);
306-
ASSERT_EQ(gains.i_max_, I_MAX);
307-
ASSERT_EQ(gains.i_min_, I_MIN);
306+
ASSERT_EQ(gains.antiwindup_strat_.i_max, I_MAX);
307+
ASSERT_EQ(gains.antiwindup_strat_.i_min, I_MIN);
308308
ASSERT_EQ(gains.u_max_, U_MAX);
309309
ASSERT_EQ(gains.u_min_, U_MIN);
310310
ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, TRK_TC);
@@ -378,8 +378,8 @@ TEST(PidParametersTest, SetBadParametersTest)
378378
ASSERT_EQ(gains.p_gain_, P);
379379
ASSERT_EQ(gains.i_gain_, I);
380380
ASSERT_EQ(gains.d_gain_, D);
381-
ASSERT_EQ(gains.i_max_, I_MAX);
382-
ASSERT_EQ(gains.i_min_, I_MIN);
381+
ASSERT_EQ(gains.antiwindup_strat_.i_max, I_MAX);
382+
ASSERT_EQ(gains.antiwindup_strat_.i_min, I_MIN);
383383
ASSERT_EQ(gains.u_max_, std::numeric_limits<double>::infinity());
384384
ASSERT_EQ(gains.u_min_, -std::numeric_limits<double>::infinity());
385385
ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, TRK_TC);
@@ -400,8 +400,8 @@ TEST(PidParametersTest, SetBadParametersTest)
400400
ASSERT_EQ(gains.p_gain_, P);
401401
ASSERT_EQ(gains.i_gain_, I);
402402
ASSERT_EQ(gains.d_gain_, D);
403-
ASSERT_EQ(gains.i_max_, I_MAX);
404-
ASSERT_EQ(gains.i_min_, I_MIN);
403+
ASSERT_EQ(gains.antiwindup_strat_.i_max, I_MAX);
404+
ASSERT_EQ(gains.antiwindup_strat_.i_min, I_MIN);
405405
ASSERT_EQ(gains.u_max_, std::numeric_limits<double>::infinity());
406406
ASSERT_EQ(gains.u_min_, -std::numeric_limits<double>::infinity());
407407
ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, TRK_TC);
@@ -419,8 +419,8 @@ TEST(PidParametersTest, SetBadParametersTest)
419419
ASSERT_EQ(updated_gains.p_gain_, P);
420420
ASSERT_EQ(updated_gains.i_gain_, I);
421421
ASSERT_EQ(updated_gains.d_gain_, D);
422-
ASSERT_EQ(updated_gains.i_max_, I_MAX);
423-
ASSERT_EQ(updated_gains.i_min_, I_MIN);
422+
ASSERT_EQ(updated_gains.antiwindup_strat_.i_max, I_MAX);
423+
ASSERT_EQ(updated_gains.antiwindup_strat_.i_min, I_MIN);
424424
ASSERT_EQ(updated_gains.u_max_, U_MAX);
425425
ASSERT_EQ(updated_gains.u_min_, U_MIN);
426426
ASSERT_EQ(updated_gains.antiwindup_strat_.tracking_time_constant, TRK_TC);

‎control_toolbox/test/pid_tests.cpp‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -420,8 +420,8 @@ TEST(ParameterTest, gainSettingCopyPIDTest)
420420
EXPECT_EQ(p_gain, g1.p_gain_);
421421
EXPECT_EQ(i_gain, g1.i_gain_);
422422
EXPECT_EQ(d_gain, g1.d_gain_);
423-
EXPECT_EQ(i_max, g1.i_max_);
424-
EXPECT_EQ(i_min, g1.i_min_);
423+
EXPECT_EQ(i_max, g1.antiwindup_strat_.i_max);
424+
EXPECT_EQ(i_min, g1.antiwindup_strat_.i_min);
425425
EXPECT_EQ(u_max, g1.u_max_);
426426
EXPECT_EQ(u_min, g1.u_min_);
427427
EXPECT_EQ(tracking_time_constant, g1.antiwindup_strat_.tracking_time_constant);
@@ -443,8 +443,8 @@ TEST(ParameterTest, gainSettingCopyPIDTest)
443443
EXPECT_EQ(p_gain_return, g1.p_gain_);
444444
EXPECT_EQ(i_gain_return, g1.i_gain_);
445445
EXPECT_EQ(d_gain_return, g1.d_gain_);
446-
EXPECT_EQ(antiwindup_strat_return.i_max, g1.i_max_);
447-
EXPECT_EQ(antiwindup_strat_return.i_min, g1.i_min_);
446+
EXPECT_EQ(antiwindup_strat_return.i_max, g1.antiwindup_strat_.i_max);
447+
EXPECT_EQ(antiwindup_strat_return.i_min, g1.antiwindup_strat_.i_min);
448448
EXPECT_EQ(u_max, g1.u_max_);
449449
EXPECT_EQ(u_min, g1.u_min_);
450450
EXPECT_EQ(tracking_time_constant, g1.antiwindup_strat_.tracking_time_constant);
@@ -470,8 +470,8 @@ TEST(ParameterTest, gainSettingCopyPIDTest)
470470
EXPECT_EQ(p_gain_return, g1.p_gain_);
471471
EXPECT_EQ(i_gain_return, g1.i_gain_);
472472
EXPECT_EQ(d_gain_return, g1.d_gain_);
473-
EXPECT_EQ(antiwindup_strat_return.i_max, g1.i_max_);
474-
EXPECT_EQ(antiwindup_strat_return.i_min, g1.i_min_);
473+
EXPECT_EQ(antiwindup_strat_return.i_max, g1.antiwindup_strat_.i_max);
474+
EXPECT_EQ(antiwindup_strat_return.i_min, g1.antiwindup_strat_.i_min);
475475
EXPECT_EQ(u_max, g1.u_max_);
476476
EXPECT_EQ(u_min, g1.u_min_);
477477
EXPECT_EQ(tracking_time_constant, g1.antiwindup_strat_.tracking_time_constant);

0 commit comments

Comments
 (0)