PidController.cpp

// PID Controller
// Copyright 2017 Sean Farrell <sean.farrell@rioki.org>
//
// This program is free software. It comes without any warranty, to
// the extent permitted by applicable law. You can redistribute it
// and/or modify it under the terms of the Do What The Fuck You Want
// To Public License, Version 2, as published by Sam Hocevar.
// See http://www.wtfpl.net/ for more details.

#include "PidController.h"

#include <algorithm>

PidController::PidController(float _kP, float _kI, float _kD)
: kP(_kP), kI(_kI), kD(_kD) {}

void PidController::calibrate(float _kP, float _kI, float _kD)
{
    kP = _kP;
    kI = _kI;
    kD = _kD;
}

float PidController::control(float setpoint, float processValue, float dt)
{
    float error = setpoint - processValue;
    // proportional
    float pOut = kP * error;
    // integral
    integral = (integral * std::max(INTEGRAL_WINDOW - dt, 0.0f)) + error * dt;
    float iOut = kI * integral;
    // derivative
    float derivative = (error - lastError) / dt;
    float dOut = kD * derivative;

    lastError = error;
    return pOut + iOut + dOut;
}

void PidController::reset()
{
    lastError = 0.0f;
    integral = 0.0f;
}

PidController.h

// PID Controller
// Copyright 2017 Sean Farrell <sean.farrell@rioki.org>
//
// This program is free software. It comes without any warranty, to
// the extent permitted by applicable law. You can redistribute it
// and/or modify it under the terms of the Do What The Fuck You Want
// To Public License, Version 2, as published by Sam Hocevar.
// See http://www.wtfpl.net/ for more details.

#ifndef _PID_CONTROLLER_H_
#define _PID_CONTROLLER_H_

//! PID Controller
//!
//! @note Only one process can be controlled with one instance of the
//! PID controller because error and integral values are carried over
//! from one call of control to the next.
class PidController
{
public:
    //! Construct a PID controller
    //!
    //! @param kP the proportional control factor
    //! @param kI the integral control factor
    //! @param kD the derivative control factor
    explicit PidController(float kP, float kI, float kD);

    //! Configure the controller with a new set of control factors.
    //!
    //! @param kP the proportional control factor
    //! @param kI the integral control factor
    //! @param kD the derivative control factor
    //!
    //! @note It may be advisable to call reset after recalibrating the PID
    //! controller to remove the control memory. This will result in
    //! control bounce though and may not be desired.
    void calibrate(float kP, float kI, float kD);

    //! Compute the control value with given setpoint and process value.
    //!
    //! @param setpoint the value to controll towards
    //! @param processValue the current value of the process under control
    //! @param dt the delta duration since the last call to control
    float control(float setpoint, float processValue, float dt);

    //! Reset the controller error and integral values
    //!
    //! @note You should call the reset function each time the process
    //! under control was significantly changed. This will remove any
    //! memory of the previous process state and reduce potential knock effect.
    void reset();

private:
    float kP = 1.0f;
    float kI = 0.0f;
    float kD = 0.0f;
    float lastError = 0.0f;
    float integral = 0.0f;
    // Size to consider for the integral memory in seconds.
    constexpr static auto INTEGRAL_WINDOW = 1.0f;
};

#endif