pid.h

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

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

    double kp;

    double ki;

    double kd;

    double deadband;

    double on_target_time;

    ERROR_TYPE error_method;

    PID(double kp, double ki, double kd, double deadband, double on_target_time, ERROR_TYPE error_method = PID::LINEAR);

    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_output();

    double get_target() const;

    void set_target(double target);
 
  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 target_vel = 0; 
    double sensor_val = 0; 
    double out = 0;        
 
    bool is_checking_on_target = false; 
 
    vex::timer pid_timer; 
};