SmartMechanism.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/Commands.hpp>
#include <wpi/commands2/SubsystemBase.hpp>
#include <wpi/math/geometry/Pose3d.hpp>
#include <wpi/smartdashboard/Mechanism2d.hpp>
#include <wpi/units/angle.hpp>
#include <wpi/units/angular_velocity.hpp>
#include <wpi/units/length.hpp>
#include <wpi/units/velocity.hpp>
#include <wpi/units/voltage.hpp>
 
#include "yams/motorcontrollers/SmartMotorController.hpp"
 
namespace yams::mechanisms {
 
class SmartMechanism {
 public:
  SmartMechanism() {};
  virtual ~SmartMechanism() = default;
 
  // ---- Pure virtual interface -----------------------------------------------
 
  virtual void SimIterate() = 0;
 
  virtual void UpdateTelemetry() = 0;
 
  virtual void VisualizationUpdate() = 0;
 
  virtual std::string GetName() const = 0;
 
  virtual wpi::math::Pose3d GetPose3d() const = 0;
 
  // ---- Open-loop command factories ------------------------------------------
 
  wpi::cmd::CommandPtr Set(double dutycycle);
 
  wpi::cmd::CommandPtr Set(std::function<double()> dutycycle);
 
  wpi::cmd::CommandPtr SetVoltage(wpi::units::volt_t volts);
 
  wpi::cmd::CommandPtr SetVoltage(std::function<wpi::units::volt_t()> volts);
 
  // ---- Direct setpoint helpers (no command wrapper) -------------------------
 
  void SetMechanismVelocitySetpoint(wpi::units::turns_per_second_t velocity);
 
  void SetMeasurementVelocitySetpoint(wpi::units::meters_per_second_t velocity);
 
  void SetMechanismPositionSetpoint(wpi::units::turn_t angle);
 
  void SetMeasurementPositionSetpoint(wpi::units::meter_t distance);
 
  void SetVoltageSetpoint(wpi::units::volt_t voltage);
 
  void SetDutyCycleSetpoint(double dutycycle);
 
  // ---- Accessors ------------------------------------------------------------
 
  motorcontrollers::SmartMotorController* GetMotorController();
 
  std::optional<wpi::units::turn_t> GetMechanismSetpoint();
 
  std::optional<wpi::units::meter_t> GetMeasurementSetpoint();
 
  wpi::Mechanism2d* GetMechanismWindow();
 
 protected:
  wpi::cmd::SubsystemBase* m_subsystem{nullptr};
 
  motorcontrollers::SmartMotorController* m_smc{nullptr};
 
  std::optional<wpi::Mechanism2d> m_mechanismWindow;
};
 
}  // namespace yams::mechanisms