void FreeFloatingPids::InitPID(control_toolbox::Pid &_pid, const ros::NodeHandle&_node, const bool &_use_dynamic_reconfig)
{
if(_use_dynamic_reconfig)
{
// classical PID init
_pid.init(_node);
}
else
{
control_toolbox::Pid::Gains gains;
// Load PID gains from parameter server
if (!_node.getParam("p", gains.p_gain_))
{
ROS_ERROR("No p gain specified for pid. Namespace: %s", _node.getNamespace().c_str());
return;
}
// Only the P gain is required, the I and D gains are optional and default to 0:
_node.param("i", gains.i_gain_, 0.0);
_node.param("d", gains.d_gain_, 0.0);
// Load integral clamp from param server or default to 0
double i_clamp;
_node.param("i_clamp", i_clamp, 0.0);
gains.i_max_ = std::abs(i_clamp);
gains.i_min_ = -std::abs(i_clamp);
_pid.setGains(gains);
}
}
void PositionCommand::pidGainLoad (control_toolbox::Pid &pid , control_toolbox::Pid::Gains & G, const ros::NodeHandle &n )
{
double Kp,Ki,Kd,IMax,IMin;
n.param<double>("Kp" ,G.p_gain_ , 0.0);
n.param<double>("Ki" ,G.i_gain_ , 0.0);
n.param<double>("Kd" ,G.d_gain_ , 0.0);
n.param<double>("IMax" ,G.i_max_ , 0.0);
n.param<double>("IMin" ,G.i_min_ , 0.0);
std::cout << "Kp = " << G.p_gain_
<< " Ki = " << G.i_gain_
<< " Kd = " << G.d_gain_
<< " IMax = " << G.i_max_
<< " IMin = " << G.i_min_
<< std::endl;
pid.setGains(Kp,Ki,Kd,IMax,IMin);
}
void config_callback(rate_control::RateControlConfig &config, uint32_t level)
{
Pid_.setGains(config.kp, config.ki, config.kd, config.imax, -config.imax);
motor_zero_ = config.motor_zero;
input_scale_ = config.input_scale;
}