diff --git a/include/control_toolbox/pid.hpp b/include/control_toolbox/pid.hpp index 8f3272a7..b00321ca 100644 --- a/include/control_toolbox/pid.hpp +++ b/include/control_toolbox/pid.hpp @@ -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 @@ -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 @@ -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 diff --git a/src/pid.cpp b/src/pid.cpp index c1f23a12..10d94abc 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -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(dt) / 1e9); + error_dot_ = (error - p_error_last_) / (static_cast(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(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(); @@ -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; } @@ -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(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_ diff --git a/test/pid_tests.cpp b/test/pid_tests.cpp index 22e3e3af..e5462b19 100644 --- a/test/pid_tests.cpp +++ b/test/pid_tests.cpp @@ -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); } @@ -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); @@ -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); } @@ -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); } @@ -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); @@ -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); } @@ -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); } @@ -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); @@ -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); } @@ -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); }