ArmSimSupplier.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 <wpi/math/filter/LinearFilter.hpp>
#include <wpi/simulation/SingleJointedArmSim.hpp>
#include <wpi/units/angle.hpp>
#include <wpi/units/angular_acceleration.hpp>
#include <wpi/units/angular_velocity.hpp>
#include <wpi/units/current.hpp>
#include <wpi/units/length.hpp>
#include <wpi/units/time.hpp>
#include <wpi/units/velocity.hpp>
#include <wpi/units/voltage.hpp>
 
#include "yams/gearing/MechanismGearing.hpp"
#include "yams/math/DerivativeTimeFilter.hpp"
#include "yams/motorcontrollers/SimSupplier.hpp"
 
namespace yams::motorcontrollers {
class SmartMotorController;
}  // namespace yams::motorcontrollers
 
namespace yams::motorcontrollers::simulation {
 
class ArmSimSupplier : public SimSupplier {
 public:
  ArmSimSupplier(wpi::sim::SingleJointedArmSim& sim, SmartMotorController& smc);
 
  void UpdateSim() override;
  bool GetUpdatedSim() override;
  void FeedUpdateSim() override;
  void StarveUpdateSim() override;
  bool IsInputFed() override;
  void FeedInput() override;
  void StarveInput() override;
  void SetMechanismStatorDutyCycle(double dutyCycle) override;
  wpi::units::volt_t GetMechanismSupplyVoltage() override;
  wpi::units::volt_t GetMechanismStatorVoltage() override;
  void SetMechanismStatorVoltage(wpi::units::volt_t volts) override;
 
  wpi::units::turn_t GetMechanismPosition() override;
  wpi::units::turns_per_second_t GetMechanismVelocity() override;
  wpi::units::turns_per_second_squared_t GetMechanismAcceleration() override;
  wpi::units::turn_t GetRotorPosition() override;
  wpi::units::turns_per_second_t GetRotorVelocity() override;
  wpi::units::turns_per_second_squared_t GetRotorAcceleration() override;
 
  void SetMechanismPosition(wpi::units::turn_t angle) override;
  void SetMechanismVelocity(wpi::units::turns_per_second_t velocity) override;
  void SetRotorPosition(wpi::units::turn_t angle) override;
  void SetRotorVelocity(wpi::units::turns_per_second_t velocity) override;
 
  wpi::units::ampere_t GetStatorCurrent() override;
  wpi::units::ampere_t GetSupplyCurrent() override;
 
 private:
  wpi::sim::SingleJointedArmSim& m_sim;
  std::function<double()> m_dutyCycleSupplier;
  gearing::MechanismGearing m_gearing;
  wpi::units::second_t m_period;
  const void* m_batteryKey;
  wpi::math::LinearFilter<double> m_supplyCurrentFilter;
  bool m_inputFed{false};
  bool m_simUpdated{false};
  wpi::units::volt_t m_lastInputVoltage{0};
  math::DerivativeTimeFilter m_rotorAccelFilter;
};
 
}  // namespace yams::motorcontrollers::simulation