107 lines
3.6 KiB
C++
107 lines
3.6 KiB
C++
|
|
#include "hoverboard_driver/pid.hpp"
|
|
|
|
PID::PID(double p, double i, double d, double i_max, double i_min, bool antiwindup, double out_max, double out_min)
|
|
: control_toolbox::Pid()
|
|
{
|
|
f_ = 0.0;
|
|
initPid(p, i, d, i_max, i_min, antiwindup);
|
|
error_ = 0.0;
|
|
|
|
out_min_ = out_min;
|
|
out_max_ = out_max;
|
|
}
|
|
|
|
void PID::init(double f, double p, double i, double d, double i_max, double i_min, bool antiwindup, double out_max, double out_min)
|
|
{
|
|
RCLCPP_INFO(rclcpp::get_logger("hoverboard_driver"), "Initialize PID");
|
|
f_ = f;
|
|
initPid(p, i, d, i_max, i_min, antiwindup);
|
|
error_ = 0.0;
|
|
|
|
out_min_ = out_min;
|
|
out_max_ = out_max;
|
|
|
|
Gains gains = getGains();
|
|
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("hoverboard_driver"), "Initialized PID: F=" << f << ", P=" << gains.p_gain_ << ", I=" << gains.i_gain_ << ", D=" << gains.d_gain_ << ", out_min=" << out_min_ << ", out_max=" << out_max_);
|
|
}
|
|
|
|
double PID::operator()(const double &measured_value, const double &setpoint, const rclcpp::Duration &dt)
|
|
{
|
|
// Compute error terms
|
|
error_ = setpoint - measured_value;
|
|
// rclcpp::Clock clock; // NOTA: Puede mamar
|
|
// RCLCPP_DEBUG_STREAM_THROTTLE(
|
|
// node_->get_logger(), clock, 1000000000, "Error: " << error_);
|
|
|
|
// Reset the i_error in case the p_error and the setpoint is zero
|
|
// Otherwise there will always be a constant i_error_ that won't vanish
|
|
if (0.0 == setpoint && 0.0 == error_)
|
|
{
|
|
// reset() will reset
|
|
// p_error_last_ = 0.0;
|
|
// p_error_ = 0.0;
|
|
// i_error_ = 0.0;
|
|
// d_error_ = 0.0;
|
|
// cmd_ = 0.0;
|
|
reset();
|
|
}
|
|
|
|
// Use control_toolbox::Pid::computeCommand()
|
|
double output = computeCommand(error_, dt.nanoseconds());
|
|
// RCLCPP_DEBUG_STREAM_THROTTLE(node_->get_logger(), clock, dt.nanoseconds(),
|
|
// "PID computed command: " << output);
|
|
|
|
// Compute final output including feed forward term
|
|
output = f_ * setpoint + output;
|
|
//output = clamp(output, out_min_, out_max_);
|
|
|
|
return output;
|
|
}
|
|
|
|
void PID::getParameters(double &f, double &p, double &i, double &d, double &i_max, double &i_min)
|
|
{
|
|
bool antiwindup;
|
|
getParameters(f, p, i, d, i_max, i_min, antiwindup);
|
|
}
|
|
|
|
void PID::getParameters(double &f, double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup)
|
|
{
|
|
f = f_;
|
|
// Call getGains from control_toolbox
|
|
getGains(p, i, d, i_max, i_min, antiwindup);
|
|
}
|
|
|
|
void PID::setParameters(double f, double p, double i, double d, double i_max, double i_min, bool antiwindup)
|
|
{
|
|
f_ = f;
|
|
setGains(p, i, d, i_max, i_min, antiwindup);
|
|
|
|
Gains gains = getGains();
|
|
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("hoverboard_driver"), "Update PID Gains: F=" << f << ", P=" << gains.p_gain_ << ", I=" << gains.i_gain_ << ", D=" << gains.d_gain_ << ", out_min=" << out_min_ << ", out_max=" << out_max_);
|
|
}
|
|
|
|
void PID::setOutputLimits(double output_max, double output_min)
|
|
{
|
|
out_max_ = output_max;
|
|
out_min_ = output_min;
|
|
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("hoverboard_driver"), "Update PID output limits: lower=" << out_min_ << ", upper=" << out_max_);
|
|
}
|
|
|
|
|
|
double PID::clamp(const double& value, const double& lower_limit, const double& upper_limit)
|
|
{
|
|
rclcpp::Clock clock;
|
|
if (value > upper_limit){
|
|
RCLCPP_DEBUG_STREAM_THROTTLE(
|
|
rclcpp::get_logger("hoverboard_driver"), clock, 1000000000,
|
|
"Clamp " << value << " to upper limit " << upper_limit);
|
|
return upper_limit;
|
|
}else if (value < lower_limit){
|
|
RCLCPP_DEBUG_STREAM_THROTTLE(
|
|
rclcpp::get_logger("hoverboard_driver"), clock, 1000000000,
|
|
"Clamp " << value << " to lower limit " << upper_limit);
|
|
return lower_limit;
|
|
}
|
|
return value;
|
|
} |