SwerveModule.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/math/kinematics/SwerveModulePosition.hpp>
#include <wpi/math/kinematics/SwerveModuleVelocity.hpp>
#include <wpi/units/angle.hpp>
#include <wpi/units/force.hpp>
 
#include "yams/mechanisms/config/SwerveModuleConfig.hpp"
#include "yams/mechanisms/swerve/SwerveModuleVelocityWithAzimuth.hpp"
#include "yams/motorcontrollers/SmartMotorController.hpp"
#include "yams/telemetry/MechanismTelemetry.hpp"
#include "yams/telemetry/SwerveModuleTelemetry.hpp"
 
namespace yams::mechanisms::swerve {
 
class SwerveModule {
 public:
  explicit SwerveModule(config::SwerveModuleConfig* config);
 
  void SetupTelemetry(const std::string& mechName);
 
  void SeedAzimuthEncoder();
 
  std::string GetName() const;
 
  wpi::units::degree_t GetRawAbsoluteEncoderAngle() const;
 
  const config::SwerveModuleConfig& GetConfig() const;
 
  wpi::math::SwerveModuleVelocity SetSwerveModuleState(wpi::math::SwerveModuleVelocity state);
 
  wpi::math::SwerveModuleVelocity SetSwerveModuleState(wpi::math::SwerveModuleVelocity state,
                                                       wpi::units::newton_t feedforwardForce);
 
  config::SwerveModuleConfig& GetMutableConfig();
 
  wpi::math::SwerveModuleVelocity SetAzimuthAngle(wpi::math::Rotation2d angle);
 
 
  SwerveModuleVelocityWithAzimuth GetState() const;
 
  wpi::math::SwerveModulePosition GetPosition() const;
 
  void UpdateTelemetry();
 
  void SimIterate();
 
  motorcontrollers::SmartMotorController* GetDriveMotorController() const;
 
  motorcontrollers::SmartMotorController* GetAzimuthMotorController() const;
 
 private:
  config::SwerveModuleConfig* m_config{nullptr};
  telemetry::MechanismTelemetry m_telemetry;
  std::optional<telemetry::SwerveModuleTelemetry> m_swerveModuleTelemetry;
  std::function<wpi::units::degree_t()> m_azimuthEncoderWithoutOffsets;
  // Azimuth travel, unwrapped, for taking the coupled wheel travel out of the drive position.
  mutable std::optional<wpi::math::Rotation2d> m_lastAzimuthAngle;
  mutable wpi::units::turn_t m_azimuthTravel{0};
 
  wpi::units::meter_t GetCouplingDistance(wpi::units::turn_t azimuthAngle) const;
  double GetDriveRotorToMechanismRatio() const;
 public:
  wpi::units::meters_per_second_t GetCouplingVelocity() const;
 
  // Public so SwerveDrive can access them directly (mirrors Java protected fields).
  motorcontrollers::SmartMotorController* m_driveMotorController;
  motorcontrollers::SmartMotorController* m_azimuthMotorController;
};
 
}  // namespace yams::mechanisms::swerve