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);
}
Ejemplo n.º 3
0
 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;
 }