motion_controller.h

#pragma once
#include "core/subsystems/screen.h"
#include "core/subsystems/tank_drive.h"
#include "core/utils/controls/feedback_base.h"
#include "core/utils/controls/feedforward.h"
#include "core/utils/controls/pid.h"
#include "core/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;
};