motion_controller.h

#pragma once
#include "../core/include/subsystems/screen.h"
#include "../core/include/subsystems/tank_drive.h"
#include "../core/include/utils/controls/feedback_base.h"
#include "../core/include/utils/controls/feedforward.h"
#include "../core/include/utils/controls/pid.h"
#include "../core/include/utils/controls/trapezoid_profile.h"
#include "vex.h"
 
class MotionController : public Feedback {
public:
  typedef struct {
    double max_v;                    
    double accel;                    
    PID::pid_config_t pid_cfg;       
    FeedForward::ff_config_t ff_cfg; 
  } m_profile_cfg_t;
 
  MotionController(m_profile_cfg_t &config);
 
  void init(double start_pt, double end_pt) override;
 
  double update(double sensor_val) override;
 
  double get() override;
 
  void set_limits(double lower, double upper) override;
 
  bool is_on_target() override;
 
  motion_t get_motion() const;
 
  screen::Page *Page();
 
  static FeedForward::ff_config_t tune_feedforward(TankDrive &drive, OdometryTank &odometry, double pct = 0.6,
                                                   double duration = 2);
 
private:
  m_profile_cfg_t config;
 
  PID pid;
  FeedForward ff;
  TrapezoidProfile profile;
 
  double current_pos;
  double end_pt;
 
  double lower_limit = 0, upper_limit = 0;
  double out = 0;
  motion_t cur_motion;
 
  vex::timer tmr;
  friend class MotionControllerPage;
};