Vamos Automotive Simulator git

//  PID.h - a proportional, integral, derivative controller.
//
//  Copyright (C) 2008-2019 Sam Varner
//
//  This file is part of Vamos Automotive Simulator.
//
//  Vamos is free software: you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation, either version 3 of the License, or
//  (at your option) any later version.
//
//  Vamos is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU General Public License for more details.
//
//  You should have received a copy of the GNU General Public License
//  along with Vamos.  If not, see <http://www.gnu.org/licenses/>.

#ifndef _PID_H_
#define _PID_H_

#include <ostream>
#include <optional>
#include <tuple>

/// A PID controller
///
/// The controller provides a value to be used to as an input parameter for
/// some process.  The output value is calculated to minimize the difference
/// between a 'process variable' and its desired value -- the 'setpoint'.
///
/// For example, during acceleration, we wish to keep tire slip at a value that
/// gives maximum acceleration, say, 10%.  The process variable is the actual
/// slip ratio and the setpoint is 10%.  The output of the controller is the
/// throttle setting.  The P, I, and D parameters must be chosen so that the
/// throttle is set appropriately when the slip ratio is not at the setpoint.
namespace Vamos_Geometry
{
class PID
{
public:
    /// Set the proportional, integral, and derivative gains.
    PID(double p, double i, double d);
    /// Give a new setpoint.
    void set(double setpoint) { m_setpoint = setpoint; }
    /// @return The current setpoint.
    double setpoint() const { return m_setpoint; }
    /// Erase history; zero the integral and unset the previous error.
    void reset();
    /// @param input The process variable that we are trying to control at the
    ///   setpoint.
    /// @param dt The time since the last reading.  We assume \p input has been
    ///   at its current value for this long.  This is usually a good assumption
    ///   if \p dt is short.
    /// @return The new control value.
    double propagate(double input, double dt);
    /// @return The current control value.
    double control() const;
    /// @return A tuple of proportional, intergal, and derivative error terms.
    std::tuple<double, double, double> terms() const;

private:
    double m_kp; //< Proportional gain
    double m_ki; //< Integral gain
    double m_kd; //< Derivative gain
    double m_proportional = 0.0; //< Proportional error term
    double m_integral = 0.0;     //< Integral error term
    double m_derivative = 0.0;   //< Derivative error term
    /// The previous error for the derivative.
    std::optional<double> m_previous_error;
    /// The current recent setpoint.
    double m_setpoint = 0.0;
};
} // namespace Vamos_Geometry

inline std::ostream& operator<<(std::ostream& os, Vamos_Geometry::PID const& pid)
{
    auto [p, i, d] = pid.terms();
    os << pid.setpoint() << ' ' << p << ' ' << i << ' ' << d;
    return os;
}

#endif // not _PID_H_