flywheel.h

#pragma once
 
#include "vex/vex.h"
 
#include "core/screen/screen_book.h"
#include "core/competition/command_structure/auto_command.h"
#include "core/math/controls/feedforward.h"
#include "core/math/moving_average.h"
 
#include <atomic>

class Flywheel {
   public:
    // CONSTRUCTORS, GETTERS, AND SETTERS
    Flywheel(
            vex::motor_group& motors, Feedback& feedback, FeedForward& helper, const double ratio,
            Filter& filt
    );

    double get_target() const;

    double getRPM() const;

    vex::motor_group& get_motors() const;

    void spin_manual(double speed, vex::directionType dir = vex::fwd);

    void spin_rpm(double rpm);

    void stop();

    bool is_on_target() { return fb.is_on_target(); }

    ScreenPage* Page() const;

    AutoCommand* SpinRpmCmd(int rpm) {
        return new FunctionCommand([this, rpm]() {
            spin_rpm(rpm);
            return true;
        });
    }

    AutoCommand* WaitUntilUpToSpeedCmd() {
        return new WaitUntilCondition(new FunctionCondition([this]() { return is_on_target(); }));
    }
 
   private:
    friend class FlywheelPage;
    friend int spinRPMTask(void* wheelPointer);
 
    vex::motor_group& motors;   
    bool task_running = false;  
    Feedback& fb;               
    FeedForward& ff;            
    vex::mutex fb_mut;          
    double ratio;               
    std::atomic<double> target_rpm;  
    vex::task rpm_task;              
    Filter& avger;                   
 
    // Functions for internal use only
    void set_target(double value);

    double measure_RPM();

    void spin_raw(double speed, vex::directionType dir = vex::fwd);
};