core/utils/controls/pid.h

#pragma once
 
#include "core/utils/controls/feedback_base.h"
#include "vex.h"
#include <cmath>
 
using namespace vex;

class PID : public Feedback {
  public:
    enum ERROR_TYPE {
        LINEAR,
        ANGULAR // assumes degrees
    };

    struct pid_config_t {
        double p;                
        double i;                
        double d;                
        double deadband;         
        double on_target_time;   
        ERROR_TYPE error_method; 
    };

    PID(pid_config_t &config);

    void init(double start_pt, double set_pt) override;

    double update(double sensor_val) override;

    double update(double sensor_val, double v_setpt);

    double get_sensor_val() const;

    double get() override;

    void set_limits(double lower, double upper) override;

    bool is_on_target() override;

    void reset();

    double get_error();

    double get_target() const;

    void set_target(double target);
 
    pid_config_t &config; 
 
  private:
    double last_error = 0;  
    double accum_error = 0; 
 
    double last_time = 0;           
    double on_target_last_time = 0; 
 
    double lower_limit = 0; 
    double upper_limit = 0; 
 
    double target = 0; 
    double start_pt = 0;
    double target_vel = 0; 
    double sensor_val = 0; 
    double out = 0;        
 
    bool is_checking_on_target = false; 
 
    timer pid_timer; 
};