SwerveModuleConfig.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 <any>
#include <functional>
#include <optional>
#include <string>
#include <wpi/math/geometry/Rotation2d.hpp>
#include <wpi/math/geometry/Translation2d.hpp>
#include <wpi/math/kinematics/SwerveModuleVelocity.hpp>
#include <wpi/units/angle.hpp>
#include <wpi/units/length.hpp>
#include <wpi/units/velocity.hpp>
 
#include "yams/gearing/GearBox.hpp"
#include "yams/mechanisms/swerve/SwerveModuleVelocityWithAzimuth.hpp"
#include "yams/motorcontrollers/SmartMotorController.hpp"
#include "yams/motorcontrollers/SmartMotorControllerConfig.hpp"
#include "yams/telemetry/SwerveModuleTelemetryConfig.hpp"
 
namespace yams::mechanisms::config {
 
class SwerveModuleConfig {
 public:
  using TelemetryVerbosity = motorcontrollers::SmartMotorControllerConfig::TelemetryVerbosity;
 
  SwerveModuleConfig() = default;
 
  SwerveModuleConfig(motorcontrollers::SmartMotorController* drive,
                     motorcontrollers::SmartMotorController* azimuth);
 
  // ---- Builder methods -------------------------------------------------------
 
  SwerveModuleConfig& WithSmartMotorController(
      motorcontrollers::SmartMotorController* driveMotor,
      motorcontrollers::SmartMotorController* azimuthMotor);
 
  SwerveModuleConfig& WithCosineCompensation(bool compensate);
 
  SwerveModuleConfig& WithAbsoluteEncoder(std::function<wpi::units::degree_t()> supplier);
 
  SwerveModuleConfig& WithExternalAbsoluteEncoder(std::any absoluteEncoder);
 
  SwerveModuleConfig& WithCouplingRatio(std::optional<gearing::GearBox> couplingRatio);
 
  SwerveModuleConfig& WithCouplingRatio(double driveMotorRotationsPerAzimuthRotation);
 
  SwerveModuleConfig& WithDistanceFromCenterOfRotation(wpi::units::meter_t front,
                                                       wpi::units::meter_t left);
 
  SwerveModuleConfig& WithAbsoluteEncoderGearing(const gearing::GearBox& gearing);
 
  SwerveModuleConfig& WithAbsoluteEncoderOffset(wpi::units::degree_t offset);
 
  SwerveModuleConfig& WithLocation(wpi::math::Translation2d location);
 
  SwerveModuleConfig& WithLocation(wpi::units::meter_t front, wpi::units::meter_t left);
 
  SwerveModuleConfig& WithLocation(wpi::units::meter_t distance, wpi::units::degree_t angle);
 
  SwerveModuleConfig& WithWheelRadius(wpi::units::meter_t radius);
 
  SwerveModuleConfig& WithWheelDiameter(wpi::units::meter_t diameter);
 
  SwerveModuleConfig& WithVelocityDeadband(std::optional<wpi::units::meters_per_second_t> speed);
 
  SwerveModuleConfig& WithOptimization(bool enable);
 
  SwerveModuleConfig& WithTelemetry(const std::string& name, TelemetryVerbosity verbosity);
 
  SwerveModuleConfig& WithTelemetry(const std::string& name,
                                    telemetry::SwerveModuleTelemetryConfig telemetryConfig);
 
  // ---- Getters ---------------------------------------------------------------
 
  motorcontrollers::SmartMotorController* GetDriveMotor() const;
 
  motorcontrollers::SmartMotorController* GetAzimuthMotor() const;
 
  std::optional<std::string> GetTelemetryName() const;
 
  std::optional<TelemetryVerbosity> GetTelemetryVerbosity() const;
 
  std::optional<wpi::math::Translation2d> GetLocation() const;
 
  bool GetStateOptimization() const;
 
  wpi::units::degree_t GetAbsoluteEncoderAngle() const;
 
  std::function<wpi::units::degree_t()> GetRawAbsoluteEncoderAngle() const;
 
  std::optional<std::function<wpi::units::degree_t()>> GetAbsoluteEncoderSupplier() const;
 
  std::optional<telemetry::SwerveModuleTelemetryConfig> GetSwerveModuleTelemetryConfig();
 
  wpi::math::SwerveModuleVelocity GetOptimizedState(wpi::math::SwerveModuleVelocity state) const;
 
  wpi::math::SwerveModuleVelocity GetOptimizedState(
      wpi::math::SwerveModuleVelocity state,
      const swerve::SwerveModuleVelocityWithAzimuth& measured) const;
 
  double GetCosineCompensationScalar(wpi::math::Rotation2d desiredAngle) const;
 
  std::optional<gearing::GearBox> GetCouplingRatio() const;
 
  wpi::math::SwerveModuleVelocity GetLockedState(wpi::math::Rotation2d angle) const;
 
  wpi::math::SwerveModuleVelocity GetLockedState(
      wpi::math::Rotation2d angle, const swerve::SwerveModuleVelocityWithAzimuth& measured) const;
 
 private:
  motorcontrollers::SmartMotorController* m_driveMotor{nullptr};
  motorcontrollers::SmartMotorController* m_azimuthMotor{nullptr};
  std::optional<std::string> m_telemetryName;
  std::optional<TelemetryVerbosity> m_telemetryVerbosity;
  std::optional<telemetry::SwerveModuleTelemetryConfig> m_specifiedTelemetryConfig;
  std::optional<std::function<wpi::units::degree_t()>> m_absoluteEncoderSupplier;
  std::optional<wpi::units::degree_t> m_absoluteEncoderOffset;
  gearing::GearBox m_absoluteEncoderGearbox{1.0};
  bool m_stateOptimization{true};
  bool m_cosineCompensation{false};
  std::optional<gearing::GearBox> m_couplingRatio;
  std::any m_absoluteEncoder;
  std::optional<wpi::units::meters_per_second_t> m_velocityDeadband{
      wpi::units::meters_per_second_t{0.1}};
  mutable std::optional<wpi::math::Rotation2d> m_lastCommandedAngle;
  std::optional<wpi::math::Translation2d> m_location;
  std::optional<wpi::units::meter_t> m_wheelCircumference;
 
  double GetCosineCompensatedVelocity(const wpi::math::SwerveModuleVelocity& desiredState,
                                      const wpi::math::Rotation2d& currentAngle) const;
 
  wpi::math::Rotation2d GetAzimuthAngle() const;
 
  swerve::SwerveModuleVelocityWithAzimuth GetMeasuredAzimuth() const;
 
  static wpi::math::SwerveModuleVelocity Optimize(
      wpi::math::SwerveModuleVelocity desired, const wpi::math::Rotation2d& currentAngle,
      const std::optional<wpi::math::Rotation2d>& lastCommanded);
};
 
}  // namespace yams::mechanisms::config