Program Listing for File pid.hpp

↰ Return to documentation for file (/tmp/ws/src/control_toolbox/include/control_toolbox/pid.hpp)

// Copyright (c) 2008, Willow Garage, Inc.
// All rights reserved.
//
// Software License Agreement (BSD License 2.0)
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above
//    copyright notice, this list of conditions and the following
//    disclaimer in the documentation and/or other materials provided
//    with the distribution.
//  * Neither the name of the Willow Garage nor the names of its
//    contributors may be used to endorse or promote products derived
//    from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.

#ifndef CONTROL_TOOLBOX__PID_HPP_
#define CONTROL_TOOLBOX__PID_HPP_

#include <memory>
#include <string>

#include "rclcpp/clock.hpp"
#include "rclcpp/duration.hpp"
#include "rclcpp/node.hpp"

#include "realtime_tools/realtime_buffer.h"
#include "realtime_tools/realtime_publisher.h"

#include "control_toolbox/visibility_control.hpp"

namespace control_toolbox
{
/***************************************************/
/***************************************************/

class CONTROL_TOOLBOX_PUBLIC Pid
{
public:
  struct Gains
  {
    // Optional constructor for passing in values without antiwindup
    Gains(double p, double i, double d, double i_max, double i_min)
    : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), antiwindup_(false)
    {
    }
    // Optional constructor for passing in values
    Gains(double p, double i, double d, double i_max, double i_min, bool antiwindup)
    : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), antiwindup_(antiwindup)
    {
    }
    // Default constructor
    Gains() : p_gain_(0.0), i_gain_(0.0), d_gain_(0.0), i_max_(0.0), i_min_(0.0), antiwindup_(false)
    {
    }
    double p_gain_;
    double i_gain_;
    double d_gain_;
    double i_max_;
    double i_min_;
    bool antiwindup_;
  };

  Pid(
    double p = 0.0, double i = 0.0, double d = 0.0, double i_max = 0.0, double i_min = -0.0,
    bool antiwindup = false);

  Pid(const Pid & source);

  ~Pid();

  void initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup = false);

  void reset();

  void getGains(double & p, double & i, double & d, double & i_max, double & i_min);
  void getGains(
    double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup);

  Gains getGains();

  void setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false);

  void setGains(const Gains & gains);

  double computeCommand(double error, uint64_t dt);

  double computeCommand(double error, double error_dot, uint64_t dt);

  void setCurrentCmd(double cmd);

  double getCurrentCmd();

  double getDerivativeError();

  void getCurrentPIDErrors(double & pe, double & ie, double & de);

  Pid & operator=(const Pid & source)
  {
    if (this == &source) {
      return *this;
    }

    // Copy the realtime buffer to then new PID class
    gains_buffer_ = source.gains_buffer_;

    // Reset the state of this PID controller
    reset();

    return *this;
  }

protected:
  // Store the PID gains in a realtime buffer to allow dynamic reconfigure to update it without
  // blocking the realtime update loop
  realtime_tools::RealtimeBuffer<Gains> gains_buffer_;

  double p_error_last_;
  double p_error_;
  double i_error_;
  double d_error_;
  double cmd_;
  double error_dot_;
};

}  // namespace control_toolbox

#endif  // CONTROL_TOOLBOX__PID_HPP_