math/controls/pid.h

#pragma once
 
#include <cmath>
#include <algorithm>
#include <limits>
 
#include "math/math_util.h"
 
namespace core {

class PID {
public:
    typedef struct {
        double kP = 0.0;
        double kI = 0.0;
        double kD = 0.0;
 
        double tolerance = 0.05; // default tolerance
 
        double dt = 0.01; // default period in seconds for derivative
 
        double i_zone = 0.0; // integral zone
        double i_min = -std::numeric_limits<double>::infinity(); // minimum integral limit
        double i_max = std::numeric_limits<double>::infinity(); // maximum integral limit
        
        double minimum_output = -12.0; // default minimum output limit
        double maximum_output = 12.0; // default maximum output limit
 
        bool continuous = false; // whether the PID is in continuous mode
        double minimum_input = -1.0; // minimum input for continuous mode
        double maximum_input = 1.0; // maximum input for continuous mode
    } pid_config_t;
 
 
    PID(double kP, double kI, double kD, double tolerance, double dt = 0.01) : kP_(kP), kI_(kI), kD_(kD), dt_(dt) {}
    PID(PID::pid_config_t config)
        : kP_(config.kP), kI_(config.kI), kD_(config.kD),
          tolerance_(config.tolerance), dt_(config.dt),
          i_zone_(config.i_zone), i_min_(config.i_min), i_max_(config.i_max),
          minimum_output_(config.minimum_output), maximum_output_(config.maximum_output),
          continuous_(config.continuous), minimum_input_(config.minimum_input),
          maximum_input_(config.maximum_input) {}
          
    PID(const core::PID&) = default;
    PID(core::PID&&) = default;
    PID& operator=(const core::PID&) = default;
    PID& operator=(core::PID&&) = default;

    void set_pid(double kP, double kI, double kD) {
        kP_ = kP;
        kI_ = kI;
        kD_ = kD;
    }

    void set_p(double kP) {
        kP_ = kP;
    }

    void set_i(double kI) {
        kI_ = kI;
    }

    void set_d(double kD) {
        kD_ = kD;
    }

    void set_tolerance(double tolerance) {
        tolerance_ = tolerance;
    }

    void set_i_zone(double i_zone) {
        i_zone_ = i_zone;
    }

    double p() const { return kP_; }

    double i() const { return kI_; }

    double d() const { return kD_; }

    double tolerance() const { return tolerance_; }

    double i_zone() const { return i_zone_; }

    void set_i_limits(double i_min, double i_max) {
        i_min_ = i_min;
        i_max_ = i_max;
    }

    void set_output_limits(double minimum_output, double maximum_output) {
        minimum_output_ = minimum_output;
        maximum_output_ = maximum_output;
    }

    double dt() const { return dt_; }

    void set_dt(double dt) {
        dt_ = dt;
    }

    void set_setpoint(double setpoint) {
        setpoint_ = setpoint;
    }

    double setpoint() const {
        return setpoint_;
    }

    bool is_continuous() const {
        return continuous_;
    }

    void enable_continuous(double minimum_input = -1, double maximum_input = 1) {
        continuous_ = true;
        minimum_input_ = minimum_input;
        maximum_input_ = maximum_input;
    }

    void disable_continuous() {
        continuous_ = false;
    }

    bool at_setpoint() const {
        return std::abs(error_) < tolerance_;
    }

    double calculate(double measurement) {
        measurement_ = measurement;
        prev_error_ = error_;
 
        if (continuous_) {
            
        } else {
            error_ = setpoint_ - measurement_;
        }
 
        error_derivative_ = (error_ - prev_error_) / dt_;
 
        if (std::abs(error_) < i_zone_) {
            total_error_ = core::clamp(total_error_ + error_ * dt_, i_min_, i_max_);
        } else {
            total_error_ = 0;
        }
 
        return core::clamp(
            kP_ * error_ + kI_ * total_error_ + kD_ * error_derivative_,
            minimum_output_,
            maximum_output_
        );
    }

    double calculate(double setpoint, double measurement) {
        setpoint_ = setpoint;
        return calculate(measurement);
    }

    void reset() {
        error_ = 0;
        prev_error_ = 0;
        error_derivative_ = 0;
        total_error_ = 0;
    }

    void reset_i() {
        total_error_ = 0;
    }
 
private:
    double kP_;
    double kI_;
    double kD_;
    double dt_;
    double i_zone_ = std::numeric_limits<double>::infinity();
    double i_max_ = std::numeric_limits<double>::infinity();
    double i_min_ = -std::numeric_limits<double>::infinity();
    double tolerance_ = 0.05;
 
    bool continuous_ = false;
    double minimum_input_ = -1;
    double maximum_input_ = 1;
 
    double setpoint_;
    double measurement_;
 
    double error_;
    double error_derivative_;
    double prev_error_;
    double total_error_;
 
    double maximum_output_ = std::numeric_limits<double>::infinity();
    double minimum_output_ = -std::numeric_limits<double>::infinity();
};
 
} // namespace core