SmartMotorController.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 <memory>
#include <optional>
#include <string>
#include <vector>
#include <wpi/math/controller/ArmFeedforward.hpp>
#include <wpi/math/controller/ElevatorFeedforward.hpp>
#include <wpi/math/controller/PIDController.hpp>
#include <wpi/math/controller/SimpleMotorFeedforward.hpp>
#include <wpi/math/system/DCMotor.hpp>
#include <wpi/math/trajectory/ExponentialProfile.hpp>
#include <wpi/math/trajectory/TrapezoidProfile.hpp>
#include <wpi/nt/NetworkTable.hpp>
#include <wpi/system/Notifier.hpp>
#include <wpi/units/acceleration.hpp>
#include <wpi/units/angle.hpp>
#include <wpi/units/angular_acceleration.hpp>
#include <wpi/units/angular_jerk.hpp>
#include <wpi/units/angular_velocity.hpp>
#include <wpi/units/current.hpp>
#include <wpi/units/force.hpp>
#include <wpi/units/length.hpp>
#include <wpi/units/temperature.hpp>
#include <wpi/units/time.hpp>
#include <wpi/units/velocity.hpp>
#include <wpi/units/voltage.hpp>
 
#include "SimSupplier.hpp"
#include "SmartMotorControllerConfig.hpp"
#include "yams/math/LQRController.hpp"
#include "yams/telemetry/SmartMotorControllerTelemetry.hpp"
#include "yams/telemetry/SmartMotorControllerTelemetryConfig.hpp"
 
namespace yams::motorcontrollers {
 
class SmartMotorController {
 public:
  using ClosedLoopControllerSlot = SmartMotorControllerConfig::ClosedLoopControllerSlot;
  using MotorMode = SmartMotorControllerConfig::MotorMode;
  using ControlMode = SmartMotorControllerConfig::ControlMode;
 
  virtual ~SmartMotorController();
 
  // ---- Configuration ------------------------------------------------------
 
  virtual bool ApplyConfig(const SmartMotorControllerConfig& config) = 0;
 
  // ---- Simulation ---------------------------------------------------------
 
  virtual void SetupSimulation() = 0;
 
  virtual void SimIterate() = 0;
 
  void SetSimSupplier(std::shared_ptr<SimSupplier> supplier);
 
  SimSupplier* GetSimSupplier() const;
 
  // ---- Encoder sync -------------------------------------------------------
 
  virtual void SeedRelativeEncoder() = 0;
 
  virtual void SynchronizeRelativeEncoder() = 0;
 
  // ---- Open-loop outputs --------------------------------------------------
 
  virtual void SetDutyCycle(double dutyCycle) = 0;
 
  virtual double GetDutyCycle() = 0;
 
  virtual void SetVoltage(wpi::units::volt_t voltage) = 0;
 
  virtual wpi::units::volt_t GetVoltage() = 0;
 
  // ---- Closed-loop setpoints ----------------------------------------------
 
  virtual void SetPosition(wpi::units::turn_t angle) = 0;
 
  virtual void SetPosition(wpi::units::meter_t distance) = 0;
 
  virtual void SetVelocity(wpi::units::turns_per_second_t velocity) = 0;
 
  virtual void SetVelocity(wpi::units::meters_per_second_t velocity) = 0;
 
  virtual void SetVelocity(wpi::units::turns_per_second_t velocity,
                           wpi::units::newton_t feedforwardForce);
 
  void SetVelocity(wpi::units::meters_per_second_t velocity, wpi::units::newton_t feedforwardForce);
 
  // ---- Encoder writes -----------------------------------------------------
 
  virtual void SetEncoderPosition(wpi::units::turn_t angle) = 0;
 
  virtual void SetEncoderPosition(wpi::units::meter_t distance) = 0;
 
  virtual void SetEncoderVelocity(wpi::units::turns_per_second_t velocity) = 0;
 
  virtual void SetEncoderVelocity(wpi::units::meters_per_second_t velocity) = 0;
 
  // ---- Encoder reads ------------------------------------------------------
 
  virtual wpi::units::turn_t GetMechanismPosition() = 0;
 
  virtual wpi::units::turns_per_second_t GetMechanismVelocity() = 0;
 
  virtual wpi::units::turn_t GetRelativeMechanismPosition() = 0;
 
  virtual wpi::units::turns_per_second_t GetRelativeMechanismVelocity() = 0;
 
  virtual wpi::units::turns_per_second_squared_t GetMechanismAcceleration() = 0;
 
  virtual wpi::units::turn_t GetRotorPosition() = 0;
 
  virtual wpi::units::turns_per_second_t GetRotorVelocity() = 0;
 
  virtual wpi::units::meter_t GetMeasurementPosition() = 0;
 
  virtual wpi::units::meters_per_second_t GetMeasurementVelocity() = 0;
 
  virtual wpi::units::meters_per_second_squared_t GetMeasurementAcceleration() = 0;
 
  virtual std::optional<wpi::units::turn_t> GetExternalEncoderMechanismPosition() = 0;
 
  virtual std::optional<wpi::units::turns_per_second_t> GetExternalEncoderMechanismVelocity() = 0;
 
  // ---- Motor status -------------------------------------------------------
 
  virtual std::optional<wpi::units::ampere_t> GetSupplyCurrent() = 0;
 
  virtual wpi::units::ampere_t GetStatorCurrent() = 0;
 
  virtual wpi::units::celsius_t GetTemperature() = 0;
 
  virtual wpi::math::DCMotor GetDCMotor() = 0;
 
  // ---- Configuration setters (live tuning) --------------------------------
 
  virtual void SetZeroPower(MotorMode mode) = 0;
 
  virtual void SetMotorInverted(bool inverted) = 0;
 
  virtual void SetEncoderInverted(bool inverted) = 0;
 
  virtual void SetKp(double kP) = 0;
 
  virtual void SetKi(double kI) = 0;
 
  virtual void SetKd(double kD) = 0;
 
  virtual void SetFeedback(double kP, double kI, double kD) = 0;
 
  virtual void SetKs(double kS) = 0;
 
  virtual void SetKv(double kV) = 0;
 
  virtual void SetKa(double kA) = 0;
 
  virtual void SetKg(double kG) = 0;
 
  virtual void SetFeedforward(double kS, double kV, double kA, double kG) = 0;
 
  virtual void SetStatorCurrentLimit(wpi::units::ampere_t currentLimit) = 0;
 
  virtual void SetSupplyCurrentLimit(wpi::units::ampere_t currentLimit) = 0;
 
  virtual void SetClosedLoopRampRate(wpi::units::second_t rampRate) = 0;
 
  virtual void SetOpenLoopRampRate(wpi::units::second_t rampRate) = 0;
 
  virtual void SetMechanismUpperLimit(wpi::units::turn_t upperLimit) = 0;
 
  virtual void SetMechanismLowerLimit(wpi::units::turn_t lowerLimit) = 0;
 
  virtual void SetMechanismLimits(wpi::units::turn_t lower, wpi::units::turn_t upper) = 0;
 
  virtual void SetMechanismLimitsEnabled(bool enabled) = 0;
 
  virtual void SetMeasurementUpperLimit(wpi::units::meter_t upperLimit) = 0;
 
  virtual void SetMeasurementLowerLimit(wpi::units::meter_t lowerLimit) = 0;
 
  virtual void SetMotionProfileMaxVelocity(wpi::units::turns_per_second_t maxVelocity) = 0;
 
  virtual void SetMotionProfileMaxVelocity(wpi::units::meters_per_second_t maxVelocity) = 0;
 
  virtual void SetMotionProfileMaxAcceleration(wpi::units::turns_per_second_squared_t maxAcc) = 0;
 
  virtual void SetMotionProfileMaxAcceleration(wpi::units::meters_per_second_squared_t maxAcc) = 0;
 
  virtual void SetMotionProfileMaxJerk(
      wpi::units::angular_jerk::turns_per_second_cubed_t maxJerk) = 0;
 
  virtual void SetExponentialProfile(std::optional<double> kV, std::optional<double> kA,
                                     std::optional<wpi::units::volt_t> maxInput) = 0;
 
  virtual void SetClosedLoopSlot(ClosedLoopControllerSlot slot) = 0;
 
  virtual void SetMechanismGearing(const gearing::MechanismGearing& gearing);
 
  virtual void SetMechanismCircumference(wpi::units::meter_t circumference);
 
  // ---- Closed-loop controller thread --------------------------------------
 
  wpi::units::second_t GetClosedLoopControllerPeriod() const;
 
  virtual std::optional<wpi::units::second_t> GetVelocityMeasurementDelay() const;
 
  void StartClosedLoopController();
 
  void StopClosedLoopController();
 
  void IterateClosedLoopController();
 
  // ---- Telemetry ----------------------------------------------------------
 
  void SetupTelemetry(std::shared_ptr<wpi::nt::NetworkTable> dataTable,
                      std::shared_ptr<wpi::nt::NetworkTable> tuningTable);
 
  void SetupTelemetry();
 
  void SetupLiveTuning();
 
  void UpdateTelemetry();
 
  bool TuningEnabled() const;
 
  void ApplyTuningValues();
 
  void StopLiveTuning();
 
  const std::vector<SmartMotorController*>& GetLooselyCoupledFollowers() const {
    return m_looseFollowers;
  }
 
  void AddCloseHook(std::function<void()> hook);
 
  SmartMotorController& WithTelemetry(telemetry::SmartMotorControllerTelemetryConfig config);
 
  virtual telemetry::UnsupportedTelemetryFields GetUnsupportedTelemetryFields();
 
  ClosedLoopControllerSlot GetClosedLoopControllerSlot() const;
 
  // ---- Misc ---------------------------------------------------------------
 
  std::optional<wpi::units::turn_t> GetMechanismPositionSetpoint() const;
 
  std::optional<wpi::units::turns_per_second_t> GetMechanismSetpointVelocity() const;
 
  std::optional<wpi::units::meter_t> GetMeasurementPositionSetpoint() const;
 
  std::optional<wpi::units::meters_per_second_t> GetMeasurementSetpointVelocity() const;
 
  std::optional<wpi::units::newton_t> GetSetpointFeedforwardForce() const;
 
  virtual SmartMotorControllerConfig& GetConfig() = 0;
 
  virtual void* GetMotorController() = 0;
 
  virtual void* GetMotorControllerConfig() = 0;
 
  std::string GetName() const;
 
  bool IsMotor(const wpi::math::DCMotor& a, const wpi::math::DCMotor& b) const;
 
  void CheckConfigSafety();
 
  void Close();
 
 protected:
  SmartMotorControllerConfig* m_config{nullptr};
  ClosedLoopControllerSlot m_slot{ClosedLoopControllerSlot::SLOT_0};
  std::shared_ptr<SimSupplier> m_simSupplier;
 
  std::vector<SmartMotorController*> m_looseFollowers;
 
  void LoadLooselyCoupledFollowers();
 
  void ConfigureSoftwarePID(const SmartMotorControllerConfig& config);
 
  void ForwardPositionToFollowers(wpi::units::turn_t pos);
 
  void ForwardPositionToFollowers(wpi::units::meter_t dist);
 
  void ForwardVelocityToFollowers(wpi::units::turns_per_second_t vel);
 
  void ForwardVelocityToFollowers(wpi::units::meters_per_second_t vel);
 
  std::optional<wpi::math::PIDController> m_pid;
  std::optional<math::LQRController> m_lqr;
 
  // Angular motion profile state
  std::optional<wpi::math::TrapezoidProfile<wpi::units::turns>::State> m_trapState;
  std::optional<wpi::math::ExponentialProfile<wpi::units::turns, wpi::units::volts>::State>
      m_expoState;
 
  // Linear motion profile state
  std::optional<wpi::math::TrapezoidProfile<wpi::units::meters>::State> m_linearTrapState;
  std::optional<wpi::math::ExponentialProfile<wpi::units::meters, wpi::units::volts>::State>
      m_linearExpoState;
 
  std::optional<wpi::units::turn_t> m_lastClosedLoopMechanismPosition;
 
  std::optional<wpi::units::turn_t> m_setpointPosition;
  std::optional<wpi::units::turns_per_second_t> m_setpointVelocity;
  std::optional<wpi::units::newton_t> m_setpointFeedforwardForce;
 
  std::vector<std::function<void()>> m_closeHooks;
 
  const void* BatterySimKey() const { return this; }
 
  std::unique_ptr<wpi::Notifier> m_closedLoopControllerThread;
  bool m_closedLoopControllerRunning{false};
 
  std::shared_ptr<wpi::nt::NetworkTable> m_parentTable;
  std::shared_ptr<wpi::nt::NetworkTable> m_telemetryTable;
  std::shared_ptr<wpi::nt::NetworkTable> m_tuningTable;
 
  telemetry::SmartMotorControllerTelemetry m_telemetry;
  telemetry::SmartMotorControllerTelemetryConfig m_telemetryConfig;
  bool m_telemetryConfigExplicit{false};
  std::shared_ptr<telemetry::SmartMotorControllerTelemetryConfig> m_specifiedTelemetryConfig;
  bool m_liveTuningRegistered{false};
  bool m_closed{false};
 
 private:
  void ResetProfileStates();
};
 
}  // namespace yams::motorcontrollers