Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Improvement on the integral contribution implementation #33

Merged
merged 1 commit into from
Feb 3, 2015
Merged

Improvement on the integral contribution implementation #33

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions include/control_toolbox/pid.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -370,8 +370,8 @@ class Pid

double p_error_last_; /**< _Save position state for derivative state calculation. */
double p_error_; /**< Position error. */
double d_error_; /**< Derivative error. */
double i_term_; /**< Integral term. */
double i_error_; /**< Integral of position error. */
double d_error_; /**< Derivative of position error. */
double cmd_; /**< Command to send. */

// Dynamics reconfigure
Expand Down
74 changes: 47 additions & 27 deletions src/pid.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -175,8 +175,8 @@ void Pid::reset()
{
p_error_last_ = 0.0;
p_error_ = 0.0;
i_error_ = 0.0;
d_error_ = 0.0;
i_term_ = 0.0;
cmd_ = 0.0;
}

Expand Down Expand Up @@ -269,7 +269,7 @@ double Pid::computeCommand(double error, ros::Duration dt)
// Get the gain parameters from the realtime buffer
Gains gains = *gains_buffer_.readFromRT();

double p_term, d_term;
double p_term, d_term, i_term;
p_error_ = error; // this is error = target - state

if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error))
Expand All @@ -278,11 +278,14 @@ double Pid::computeCommand(double error, ros::Duration dt)
// Calculate proportional contribution to command
p_term = gains.p_gain_ * p_error_;

//Calculate integral contribution to command
i_term_ = i_term_ + gains.i_gain_ * dt.toSec() * p_error_;
// Calculate the integral of the position error
i_error_ += dt.toSec() * p_error_;

// Calculate integral contribution to command
i_term = gains.i_gain_ * i_error_;

// Limit i_term_ so that the limit is meaningful in the output
i_term_ = std::max( gains.i_min_, std::min( i_term_, gains.i_max_) );
// Limit i_term so that the limit is meaningful in the output
i_term = std::max( gains.i_min_, std::min( i_term, gains.i_max_) );

// Calculate the derivative error
if (dt.toSec() > 0.0)
Expand All @@ -292,7 +295,9 @@ double Pid::computeCommand(double error, ros::Duration dt)
}
// Calculate derivative contribution to command
d_term = gains.d_gain_ * d_error_;
cmd_ = p_term + i_term_ + d_term;

// Compute the command
cmd_ = p_term + i_term + d_term;

return cmd_;
}
Expand All @@ -302,7 +307,7 @@ double Pid::updatePid(double error, ros::Duration dt)
// Get the gain parameters from the realtime buffer
Gains gains = *gains_buffer_.readFromRT();

double p_term, d_term;
double p_term, d_term, i_term;
p_error_ = error; //this is pError = pState-pTarget

if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error))
Expand All @@ -311,11 +316,14 @@ double Pid::updatePid(double error, ros::Duration dt)
// Calculate proportional contribution to command
p_term = gains.p_gain_ * p_error_;

//Calculate integral contribution to command
i_term_ = i_term_ + gains.i_gain_ * dt.toSec() * p_error_;
// Calculate the integral of the position error
i_error_ += dt.toSec() * p_error_;

// Calculate integral contribution to command
i_term = gains.i_gain_ * i_error_;

// Limit i_term_ so that the limit is meaningful in the output
i_term_ = std::max( gains.i_min_, std::min( i_term_, gains.i_max_) );
// Limit i_term so that the limit is meaningful in the output
i_term = std::max( gains.i_min_, std::min( i_term, gains.i_max_) );

// Calculate the derivative error
if (dt.toSec() > 0.0)
Expand All @@ -325,7 +333,9 @@ double Pid::updatePid(double error, ros::Duration dt)
}
// Calculate derivative contribution to command
d_term = gains.d_gain_ * d_error_;
cmd_ = - p_term - i_term_ - d_term;

// Compute the command
cmd_ = - p_term - i_term - d_term;

return cmd_;
}
Expand All @@ -335,7 +345,7 @@ double Pid::computeCommand(double error, double error_dot, ros::Duration dt)
// Get the gain parameters from the realtime buffer
Gains gains = *gains_buffer_.readFromRT();

double p_term, d_term;
double p_term, d_term, i_term;
p_error_ = error; // this is error = target - state
d_error_ = error_dot;

Expand All @@ -346,15 +356,20 @@ double Pid::computeCommand(double error, double error_dot, ros::Duration dt)
// Calculate proportional contribution to command
p_term = gains.p_gain_ * p_error_;

//Calculate integral contribution to command
i_term_ = i_term_ + gains.i_gain_ * dt.toSec() * p_error_;
// Calculate the integral of the position error
i_error_ += dt.toSec() * p_error_;

// Calculate integral contribution to command
i_term = gains.i_gain_ * i_error_;

// Limit i_term_ so that the limit is meaningful in the output
i_term_ = std::max( gains.i_min_, std::min( i_term_, gains.i_max_) );
// Limit i_term so that the limit is meaningful in the output
i_term = std::max( gains.i_min_, std::min( i_term, gains.i_max_) );

// Calculate derivative contribution to command
d_term = gains.d_gain_ * d_error_;
cmd_ = p_term + i_term_ + d_term;

// Compute the command
cmd_ = - p_term - i_term - d_term;

return cmd_;
}
Expand All @@ -364,7 +379,7 @@ double Pid::updatePid(double error, double error_dot, ros::Duration dt)
// Get the gain parameters from the realtime buffer
Gains gains = *gains_buffer_.readFromRT();

double p_term, d_term;
double p_term, d_term, i_term;
p_error_ = error; //this is pError = pState-pTarget
d_error_ = error_dot;

Expand All @@ -375,15 +390,20 @@ double Pid::updatePid(double error, double error_dot, ros::Duration dt)
// Calculate proportional contribution to command
p_term = gains.p_gain_ * p_error_;

//Calculate integral contribution to command
i_term_ = i_term_ + gains.i_gain_ * dt.toSec() * p_error_;
// Calculate the integral of the position error
i_error_ += dt.toSec() * p_error_;

// Calculate integral contribution to command
i_term = gains.i_gain_ * i_error_;

// Limit i_term_ so that the limit is meaningful in the output
i_term_ = std::max( gains.i_min_, std::min( i_term_, gains.i_max_) );
// Limit i_term so that the limit is meaningful in the output
i_term = std::max( gains.i_min_, std::min( i_term, gains.i_max_) );

// Calculate derivative contribution to command
d_term = gains.d_gain_ * d_error_;
cmd_ = - p_term - i_term_ - d_term;

// Compute the command
cmd_ = - p_term - i_term - d_term;

return cmd_;
}
Expand All @@ -404,7 +424,7 @@ void Pid::getCurrentPIDErrors(double *pe, double *ie, double *de)
Gains gains = *gains_buffer_.readFromRT();

*pe = p_error_;
*ie = gains.i_gain_ ? i_term_/gains.i_gain_ : 0.0;
*ie = i_error_;
*de = d_error_;
}

Expand All @@ -420,8 +440,8 @@ void Pid::printValues()
<< " I_Min: " << gains.i_min_ << "\n"
<< " P_Error_Last: " << p_error_last_ << "\n"
<< " P_Error: " << p_error_ << "\n"
<< " I_Error: " << i_error_ << "\n"
<< " D_Error: " << d_error_ << "\n"
<< " I_Term: " << i_term_ << "\n"
<< " Command: " << cmd_
);

Expand Down
19 changes: 7 additions & 12 deletions test/pid_tests.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,27 +32,23 @@ TEST(ParameterTest, zeroITermBadIBoundsTest)

TEST(ParameterTest, integrationWindupTest)
{
RecordProperty("description","This test succeeds if the integral error is prevented from winding up when the integral gain is non-zero.");
RecordProperty("description","This test succeeds if the integral contribution is prevented from winding up when the integral gain is non-zero.");

Pid pid(0.0, 1.0, 0.0, 1.0, -1.0);
Pid pid(0.0, 2.0, 0.0, 1.0, -1.0);

double cmd = 0.0;
double pe,ie,de;

cmd = pid.computeCommand(-1.0, ros::Duration(1.0));
pid.getCurrentPIDErrors(&pe,&ie,&de);
EXPECT_EQ(-1.0, ie);
EXPECT_EQ(-1.0, cmd);

cmd = pid.computeCommand(-1.0, ros::Duration(1.0));
pid.getCurrentPIDErrors(&pe,&ie,&de);
EXPECT_EQ(-1.0, ie);
EXPECT_EQ(-1.0, cmd);
}

TEST(ParameterTest, integrationWindupZeroGainTest)
{
RecordProperty("description","This test succeeds if the integral error is prevented from winding up when the integral gain is zero. If the integral error is allowed to wind up while it is disabled, it can cause sudden jumps to the minimum or maximum bound in control command when re-enabled.");
RecordProperty("description","This test succeeds if the integral contribution is prevented from winding up when the integral gain is zero. If the integral contribution is allowed to wind up while it is disabled, it can cause sudden jumps to the minimum or maximum bound in control command when re-enabled.");

double i_gain = 0.0;
double i_min = -1.0;
Expand All @@ -64,14 +60,13 @@ TEST(ParameterTest, integrationWindupZeroGainTest)

cmd = pid.computeCommand(-1.0, ros::Duration(1.0));
pid.getCurrentPIDErrors(&pe,&ie,&de);
EXPECT_LE(i_min, ie);
EXPECT_LE(ie, i_max);
EXPECT_LE(i_min, cmd);
EXPECT_LE(cmd, i_max);
EXPECT_EQ(0.0, cmd);

cmd = pid.computeCommand(-1.0, ros::Duration(1.0));
pid.getCurrentPIDErrors(&pe,&ie,&de);
EXPECT_LE(i_min, ie);
EXPECT_LE(ie, i_max);
EXPECT_LE(i_min, cmd);
EXPECT_LE(cmd, i_max);
EXPECT_EQ(0.0, cmd);
}

Expand Down