pid.h

#pragma once
 
#include "../core/include/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 target_vel = 0; 
  double sensor_val = 0; 
  double out = 0;        
 
  bool is_checking_on_target = false; 
 
  timer pid_timer; 
};