flywheel.h

#pragma once
 
#include "core/robot_specs.h"
#include "core/subsystems/screen.h"
#include "core/utils/command_structure/auto_command.h"
#include "core/utils/controls/feedforward.h"
#include "core/utils/controls/pid.h"
#include "vex.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, directionType dir = fwd);

  void spin_rpm(double rpm);

  void stop();

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

  screen::Page *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; 
  task rpm_task;                  
  Filter &avger;                  
 
  // Functions for internal use only
  void set_target(double value);
  double measure_RPM();

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