Pivot.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/smartdashboard/MechanismLigament2d.hpp>
#include <wpi/units/angle.hpp>
 
#include "SmartPositionalMechanism.hpp"
#include "yams/mechanisms/config/PivotConfig.hpp"
 
namespace yams::mechanisms::positional {
 
class Pivot : public SmartPositionalMechanism {
 public:
  Pivot(config::PivotConfig* 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;
 
  // ---- Pivot-specific interface ---------------------------------------------
 
  wpi::cmd::CommandPtr Run(wpi::units::degree_t angle);
 
  wpi::cmd::CommandPtr Run(std::function<wpi::units::degree_t()> angle);
 
  wpi::cmd::CommandPtr RunTo(wpi::units::degree_t angle,
                             wpi::units::degree_t tolerance = wpi::units::degree_t{1.0});
 
  wpi::cmd::CommandPtr RunTo(std::function<wpi::units::degree_t()> angle,
                             wpi::units::degree_t tolerance = wpi::units::degree_t{1.0});
 
  wpi::units::degree_t GetAngle() const;
 
  wpi::cmd::Trigger Gte(wpi::units::degree_t angle);
 
  wpi::cmd::Trigger Lte(wpi::units::degree_t angle);
 
  wpi::cmd::Trigger Between(wpi::units::degree_t start, wpi::units::degree_t end);
 
  wpi::cmd::Trigger IsNear(wpi::units::degree_t angle,
                           wpi::units::degree_t within = wpi::units::degree_t{1.0});
 
  const config::PivotConfig& GetConfig() const;
 
  wpi::math::Pose3d GetPose3d() const override;
 
  void SetAngle(wpi::units::degree_t angle);
 
 private:
  config::PivotConfig* m_pivotConfig{nullptr};
  std::string m_name{"Pivot"};
  std::optional<wpi::sim::DCMotorSim> m_dcMotorSim;
  wpi::MechanismLigament2d* m_setpointLigament{nullptr};
};
 
}  // namespace yams::mechanisms::positional