Vamos Automotive Simulator git

//  PID.cc - 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/>.

#include "PID.hpp"

#include <algorithm>
#include <iostream>

using namespace Vamos_Geometry;

PID::PID(double p, double i, double d)
    : m_kp(p), m_ki(i), m_kd(d)
{
}

void PID::reset()
{
    m_integral = 0.0;
    m_previous_error.reset();
}

double PID::propagate(double signal, double dt)
{
    const double error = m_setpoint - signal;
    m_proportional = m_kp * error;
    m_integral += m_ki * error * dt;
    m_derivative = !m_previous_error || dt == 0.0 ? 0.0 : m_kd * (error - *m_previous_error) / dt;
    m_previous_error = error;
    return control();
}

double PID::control() const
{
    return m_proportional + m_integral + m_derivative;
}

std::tuple<double, double, double> PID::terms() const
{
    return std::make_tuple(m_proportional, m_integral, m_derivative);
}