FlyWheel.hpp

Go to the documentation of this file.

// Copyright (c) 2026 Yet Another Software Suite
// SPDX-License-Identifier: LGPL-3.0-or-later
 
#pragma once
 
#include <functional>
#include <optional>
#include <string>
#include <wpi/commands2/CommandPtr.hpp>
#include <wpi/commands2/button/Trigger.hpp>
#include <wpi/math/geometry/Pose3d.hpp>
#include <wpi/simulation/DCMotorSim.hpp>
#include <wpi/units/angle.hpp>
#include <wpi/units/angular_velocity.hpp>
#include <wpi/units/length.hpp>
#include <wpi/units/velocity.hpp>
 
#include "SmartVelocityMechanism.hpp"
#include "yams/mechanisms/config/FlyWheelConfig.hpp"
 
namespace yams::mechanisms::velocity {
 
class FlyWheel : public SmartVelocityMechanism {
 public:
  FlyWheel(config::FlyWheelConfig* config, motorcontrollers::SmartMotorController* smc);
 
  // ---- SmartMechanism overrides ---------------------------------------------
 
  void SimIterate() override;
 
  void UpdateTelemetry() override;
 
  void VisualizationUpdate() override;
 
  std::string GetName() const override;
 
  // ---- SmartVelocityMechanism overrides -------------------------------------
 
  wpi::cmd::Trigger Max() override;
 
  wpi::cmd::Trigger Min() override;
 
  // ---- FlyWheel-specific interface ------------------------------------------
 
  wpi::units::degrees_per_second_t GetVelocity() const;
 
  // ---- Run / RunTo interface ------------------------------------------------
 
  wpi::cmd::CommandPtr Run(wpi::units::degrees_per_second_t velocity);
 
  wpi::cmd::CommandPtr Run(std::function<wpi::units::degrees_per_second_t()> velocity);
 
  wpi::cmd::CommandPtr Run(wpi::units::meters_per_second_t surfaceSpeed);
 
  wpi::cmd::CommandPtr Run(std::function<wpi::units::meters_per_second_t()> surfaceSpeed);
 
  wpi::cmd::CommandPtr RunTo(
      wpi::units::degrees_per_second_t velocity,
      wpi::units::degrees_per_second_t tolerance = wpi::units::degrees_per_second_t{5.0});
 
  wpi::cmd::CommandPtr RunTo(
      std::function<wpi::units::degrees_per_second_t()> velocity,
      wpi::units::degrees_per_second_t tolerance = wpi::units::degrees_per_second_t{5.0});
 
  wpi::cmd::CommandPtr RunTo(wpi::units::meters_per_second_t velocity,
                             wpi::units::meters_per_second_t tolerance);
 
  wpi::cmd::CommandPtr RunTo(std::function<wpi::units::meters_per_second_t()> velocity,
                             wpi::units::meters_per_second_t tolerance);
 
  // ---- Comparison triggers ---------------------------------------------------
 
  wpi::cmd::Trigger Gte(wpi::units::degrees_per_second_t velocity);
 
  wpi::cmd::Trigger Lte(wpi::units::degrees_per_second_t velocity);
 
  wpi::cmd::Trigger Between(wpi::units::degrees_per_second_t start,
                            wpi::units::degrees_per_second_t end);
 
  wpi::cmd::Trigger IsNear(
      wpi::units::degrees_per_second_t velocity,
      wpi::units::degrees_per_second_t within = wpi::units::degrees_per_second_t{5.0}) const;
 
  // ---- Direct setpoint setters -----------------------------------------------
 
  void SetVelocity(wpi::units::degrees_per_second_t velocity);
 
  void SetSurfaceSpeed(wpi::units::meters_per_second_t speed);
 
  void SetMeasurementVelocitySetpoint(wpi::units::meters_per_second_t velocity);
 
  // ---- Misc ------------------------------------------------------------------
 
  wpi::math::Pose3d GetPose3d() const override;
 
  const config::FlyWheelConfig& GetConfig() const;
 
 private:
  config::FlyWheelConfig* m_flyWheelConfig{nullptr};
  std::string m_name{"FlyWheel"};
  std::optional<wpi::sim::DCMotorSim> m_dcMotorSim;
};
 
}  // namespace yams::mechanisms::velocity