Elevator.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 <vector>
#include <wpi/commands2/CommandPtr.hpp>
#include <wpi/commands2/button/Trigger.hpp>
#include <wpi/math/geometry/Pose3d.hpp>
#include <wpi/simulation/ElevatorSim.hpp>
#include <wpi/smartdashboard/MechanismLigament2d.hpp>
#include <wpi/units/length.hpp>
 
#include "SmartPositionalMechanism.hpp"
#include "yams/mechanisms/config/ElevatorConfig.hpp"
#include "yams/motorcontrollers/SmartMotorController.hpp"
 
namespace yams::mechanisms::positional {
 
class Elevator : public SmartPositionalMechanism {
 public:
  Elevator(config::ElevatorConfig* config, motorcontrollers::SmartMotorController* smc);
 
  // ---- SmartMechanism overrides ---------------------------------------------
 
  void SimIterate() override;
 
  void UpdateTelemetry() override;
 
  void VisualizationUpdate() override;
 
  std::string GetName() const override;
 
  // ---- SmartPositionalMechanism overrides -----------------------------------
 
  wpi::cmd::Trigger Max() override;
 
  wpi::cmd::Trigger Min() override;
 
  // ---- Elevator-specific interface ------------------------------------------
 
  wpi::cmd::CommandPtr Run(wpi::units::meter_t height);
 
  wpi::cmd::CommandPtr Run(std::function<wpi::units::meter_t()> height);
 
  wpi::cmd::CommandPtr RunTo(wpi::units::meter_t height,
                             wpi::units::meter_t tolerance = wpi::units::meter_t{0.01});
 
  wpi::cmd::CommandPtr RunTo(std::function<wpi::units::meter_t()> height,
                             wpi::units::meter_t tolerance = wpi::units::meter_t{0.01});
 
  wpi::units::meter_t GetHeight() const;
 
  wpi::cmd::Trigger Gte(wpi::units::meter_t height);
 
  wpi::cmd::Trigger Lte(wpi::units::meter_t height);
 
  wpi::cmd::Trigger Between(wpi::units::meter_t start, wpi::units::meter_t end);
 
  wpi::cmd::Trigger IsNear(wpi::units::meter_t height,
                           wpi::units::meter_t within = wpi::units::meter_t{0.01});
 
  const config::ElevatorConfig& GetConfig() const;
 
  wpi::math::Pose3d GetPose3d() const override;
 
  std::vector<wpi::math::Pose3d> GetCascadePose3d() const;
 
  std::vector<wpi::math::Pose3d> GetContinuousPose3d(bool carriageFirst) const;
 
  void SetHeight(wpi::units::meter_t height);
 
 
 private:
  config::ElevatorConfig* m_elevatorConfig{nullptr};
  std::string m_name{"Elevator"};
  std::optional<wpi::sim::ElevatorSim> m_elevatorSim;
  wpi::MechanismLigament2d* m_setpointLigament{nullptr};
};
 
}  // namespace yams::mechanisms::positional