Skip to content

Commit

Permalink
Add computeCommand with double dt
Browse files Browse the repository at this point in the history
  • Loading branch information
@christophfroehlich
christophfroehlich committed Dec 1, 2024
1 parent e0c1873 commit 49e9a03
Show file tree
Hide file tree
Showing 3 changed files with 83 additions and 38 deletions.
29 changes: 27 additions & 2 deletions include/control_toolbox/pid.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -251,6 +251,18 @@ class CONTROL_TOOLBOX_PUBLIC Pid
*/
void setGains(const Gains & gains);

/*!
* \brief Set the PID error and compute the PID command with nonuniform time
* step size. The derivative error is computed from the change in the error
* and the timestep \c dt.
*
* \param error Error since last call (error = target - state)
* \param dt Change in time since last call in seconds
*
* \returns PID command
*/
[[nodiscard]] double computeCommand(double error, double dt_s);

/*!
* \brief Set the PID error and compute the PID command with nonuniform time
* step size. The derivative error is computed from the change in the error
Expand All @@ -261,7 +273,20 @@ class CONTROL_TOOLBOX_PUBLIC Pid
*
* \returns PID command
*/
[[nodiscard]] double computeCommand(double error, uint64_t dt);
[[nodiscard]] double computeCommand(double error, uint64_t dt_ns);

/*!
* \brief Set the PID error and compute the PID command with nonuniform
* time step size. This also allows the user to pass in a precomputed
* derivative error.
*
* \param error Error since last call (error = target - state)
* \param error_dot d(Error)/dt since last call
* \param dt Change in time since last call in seconds
*
* \returns PID command
*/
[[nodiscard]] double computeCommand(double error, double error_dot, double dt_s);

/*!
* \brief Set the PID error and compute the PID command with nonuniform
Expand All @@ -274,7 +299,7 @@ class CONTROL_TOOLBOX_PUBLIC Pid
*
* \returns PID command
*/
[[nodiscard]] double computeCommand(double error, double error_dot, uint64_t dt);
[[nodiscard]] double computeCommand(double error, double error_dot, uint64_t dt_ns);

/*!
* \brief Set current command for this PID controller
Expand Down
34 changes: 27 additions & 7 deletions src/pid.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -121,22 +121,42 @@ void Pid::setGains(const Gains & gains)
}
}

double Pid::computeCommand(double error, uint64_t dt)
double Pid::computeCommand(double error, uint64_t dt_ns)
{
if (dt == 0 || std::isnan(error) || std::isinf(error)) {
if (dt_ns == 0 || std::isnan(error) || std::isinf(error)) {
return 0.0;
}

error_dot_ = d_error_;

// Calculate the derivative error
error_dot_ = (error - p_error_last_) / (static_cast<double>(dt) / 1e9);
error_dot_ = (error - p_error_last_) / (static_cast<double>(dt_ns) / 1e9);
p_error_last_ = error;

return computeCommand(error, error_dot_, dt);
return computeCommand(error, error_dot_, dt_ns);
}

double Pid::computeCommand(double error, double error_dot, uint64_t dt)
double Pid::computeCommand(double error, double dt_s)
{
if (dt_s <= 0.0 || std::isnan(error) || std::isinf(error)) {
return 0.0;
}

error_dot_ = d_error_;

// Calculate the derivative error
error_dot_ = (error - p_error_last_) / dt_s;
p_error_last_ = error;

return computeCommand(error, error_dot_, dt_s);
}

double Pid::computeCommand(double error, double error_dot, uint64_t dt_ns)
{
return computeCommand(error, error_dot, static_cast<double>(dt_ns) / 1.e9);
}

double Pid::computeCommand(double error, double error_dot, double dt_s)
{
// Get the gain parameters from the realtime buffer
Gains gains = *gains_buffer_.readFromRT();
Expand All @@ -146,7 +166,7 @@ double Pid::computeCommand(double error, double error_dot, uint64_t dt)
d_error_ = error_dot;

if (
dt == 0 || std::isnan(error) || std::isinf(error) || std::isnan(error_dot) ||
dt_s <= 0.0 || std::isnan(error) || std::isinf(error) || std::isnan(error_dot) ||
std::isinf(error_dot)) {
return 0.0;
}
Expand All @@ -155,7 +175,7 @@ double Pid::computeCommand(double error, double error_dot, uint64_t dt)
p_term = gains.p_gain_ * p_error_;

// Calculate the integral of the position error
i_error_ += (static_cast<double>(dt) / 1e9) * p_error_;
i_error_ += dt_s * p_error_;

if (gains.antiwindup_ && gains.i_gain_ != 0) {
// Prevent i_error_ from climbing higher than permitted by i_max_/i_min_
Expand Down
58 changes: 29 additions & 29 deletions test/pid_tests.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -80,11 +80,11 @@ TEST(ParameterTest, integrationClampTest)
double cmd = 0.0;

// Test lower limit
cmd = pid.computeCommand(-10.03, 1.0 * 1e9);
cmd = pid.computeCommand(-10.03, 1.0);
EXPECT_EQ(-1.0, cmd);

// Test upper limit
cmd = pid.computeCommand(30.0, 1.0 * 1e9);
cmd = pid.computeCommand(30.0, 1.0);
EXPECT_EQ(1.0, cmd);
}

Expand All @@ -104,13 +104,13 @@ TEST(ParameterTest, integrationClampZeroGainTest)
double cmd = 0.0;
double pe, ie, de;

cmd = pid.computeCommand(-1.0, 1.0 * 1e9);
cmd = pid.computeCommand(-1.0, 1.0);
pid.getCurrentPIDErrors(pe, ie, de);
EXPECT_LE(i_min, cmd);
EXPECT_LE(cmd, i_max);
EXPECT_EQ(0.0, cmd);

cmd = pid.computeCommand(-1.0, 1.0 * 1e9);
cmd = pid.computeCommand(-1.0, 1.0);
EXPECT_LE(i_min, cmd);
EXPECT_LE(cmd, i_max);
EXPECT_EQ(0.0, cmd);
Expand All @@ -129,16 +129,16 @@ TEST(ParameterTest, integrationAntiwindupTest)

double cmd = 0.0;

cmd = pid.computeCommand(-1.0, 1.0 * 1e9);
cmd = pid.computeCommand(-1.0, 1.0);
EXPECT_EQ(-1.0, cmd);

cmd = pid.computeCommand(-1.0, 1.0 * 1e9);
cmd = pid.computeCommand(-1.0, 1.0);
EXPECT_EQ(-1.0, cmd);

cmd = pid.computeCommand(0.5, 1.0 * 1e9);
cmd = pid.computeCommand(0.5, 1.0);
EXPECT_EQ(0.0, cmd);

cmd = pid.computeCommand(-1.0, 1.0 * 1e9);
cmd = pid.computeCommand(-1.0, 1.0);
EXPECT_EQ(-1.0, cmd);
}

Expand All @@ -155,16 +155,16 @@ TEST(ParameterTest, negativeIntegrationAntiwindupTest)

double cmd = 0.0;

cmd = pid.computeCommand(0.1, 1.0 * 1e9);
cmd = pid.computeCommand(0.1, 1.0);
EXPECT_EQ(-0.2, cmd);

cmd = pid.computeCommand(0.1, 1.0 * 1e9);
cmd = pid.computeCommand(0.1, 1.0);
EXPECT_EQ(-0.2, cmd);

cmd = pid.computeCommand(-0.05, 1.0 * 1e9);
cmd = pid.computeCommand(-0.05, 1.0);
EXPECT_EQ(-0.075, cmd);

cmd = pid.computeCommand(0.1, 1.0 * 1e9);
cmd = pid.computeCommand(0.1, 1.0);
EXPECT_EQ(-0.2, cmd);
}

Expand Down Expand Up @@ -223,7 +223,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest)

// Send update command to populate errors -------------------------------------------------
pid1.setCurrentCmd(10);
(void) pid1.computeCommand(20, 1.0 * 1e9);
(void) pid1.computeCommand(20, 1.0);

// Test copy constructor -------------------------------------------------
Pid pid2(pid1);
Expand Down Expand Up @@ -291,22 +291,22 @@ TEST(CommandTest, proportionalOnlyTest)
double cmd = 0.0;

// If initial error = 0, p-gain = 1, dt = 1
cmd = pid.computeCommand(-0.5, 1.0 * 1e9);
cmd = pid.computeCommand(-0.5, 1.0);
// Then expect command = error
EXPECT_EQ(-0.5, cmd);

// If call again
cmd = pid.computeCommand(-0.5, 1.0 * 1e9);
cmd = pid.computeCommand(-0.5, 1.0);
// Then expect the same as before
EXPECT_EQ(-0.5, cmd);

// If call again doubling the error
cmd = pid.computeCommand(-1.0, 1.0 * 1e9);
cmd = pid.computeCommand(-1.0, 1.0);
// Then expect the command doubled
EXPECT_EQ(-1.0, cmd);

// If call with positive error
cmd = pid.computeCommand(0.5, 1.0 * 1e9);
cmd = pid.computeCommand(0.5, 1.0);
// Then expect always command = error
EXPECT_EQ(0.5, cmd);
}
Expand All @@ -323,26 +323,26 @@ TEST(CommandTest, integralOnlyTest)
double cmd = 0.0;

// If initial error = 0, i-gain = 1, dt = 1
cmd = pid.computeCommand(-0.5, 1.0 * 1e9);
cmd = pid.computeCommand(-0.5, 1.0);
// Then expect command = error
EXPECT_EQ(-0.5, cmd);

// If call again with same arguments
cmd = pid.computeCommand(-0.5, 1.0 * 1e9);
cmd = pid.computeCommand(-0.5, 1.0);
// Then expect the integral part to double the command
EXPECT_EQ(-1.0, cmd);

// Call again with no error
cmd = pid.computeCommand(0.0, 1.0 * 1e9);
cmd = pid.computeCommand(0.0, 1.0);
// Expect the integral part to keep the previous command because it ensures error = 0
EXPECT_EQ(-1.0, cmd);

// Double check that the integral contribution keep the previous command
cmd = pid.computeCommand(0.0, 1.0 * 1e9);
cmd = pid.computeCommand(0.0, 1.0);
EXPECT_EQ(-1.0, cmd);

// Finally call again with positive error to see if the command changes in the opposite direction
cmd = pid.computeCommand(1.0, 1.0 * 1e9);
cmd = pid.computeCommand(1.0, 1.0);
// Expect that the command is cleared since error = -1 * previous command, i-gain = 1, dt = 1
EXPECT_EQ(0.0, cmd);
}
Expand All @@ -359,12 +359,12 @@ TEST(CommandTest, derivativeOnlyTest)
double cmd = 0.0;

// If initial error = 0, d-gain = 1, dt = 1
cmd = pid.computeCommand(-0.5, 1.0 * 1e9);
cmd = pid.computeCommand(-0.5, 1.0);
// Then expect command = error
EXPECT_EQ(-0.5, cmd);

// If call again with same error
cmd = pid.computeCommand(-0.5, 1.0 * 1e9);
cmd = pid.computeCommand(-0.5, 1.0);
// Then expect command = 0 due to no variation on error
EXPECT_EQ(0.0, cmd);

Expand All @@ -374,12 +374,12 @@ TEST(CommandTest, derivativeOnlyTest)
EXPECT_EQ(0.0, cmd);

// If the error increases, with dt = 1
cmd = pid.computeCommand(-1.0, 1.0 * 1e9);
cmd = pid.computeCommand(-1.0, 1.0);
// Then expect the command = change in dt
EXPECT_EQ(-0.5, cmd);

// If error decreases, with dt = 1
cmd = pid.computeCommand(-0.5, 1.0 * 1e9);
cmd = pid.computeCommand(-0.5, 1.0);
// Then expect always the command = change in dt (note the sign flip)
EXPECT_EQ(0.5, cmd);
}
Expand All @@ -396,17 +396,17 @@ TEST(CommandTest, completePIDTest)

// All contributions are tested, here few tests check that they sum up correctly
// If initial error = 0, all gains = 1, dt = 1
cmd = pid.computeCommand(-0.5, 1.0 * 1e9);
cmd = pid.computeCommand(-0.5, 1.0);
// Then expect command = 3x error
EXPECT_EQ(-1.5, cmd);

// If call again with same arguments, no error change, but integration do its part
cmd = pid.computeCommand(-0.5, 1.0 * 1e9);
cmd = pid.computeCommand(-0.5, 1.0);
// Then expect command = 3x error again
EXPECT_EQ(-1.5, cmd);

// If call again increasing the error
cmd = pid.computeCommand(-1.0, 1.0 * 1e9);
cmd = pid.computeCommand(-1.0, 1.0);
// Then expect command equals to p = -1, i = -2.0 (i.e. - 0.5 - 0.5 - 1.0), d = -0.5
EXPECT_EQ(-3.5, cmd);
}
Expand Down

0 comments on commit 49e9a03

Please sign in to comment.