SwerveDriveConfig.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 <vector>
#include <wpi/commands2/SubsystemBase.hpp>
#include <wpi/math/controller/AntiTipping.hpp>
#include <wpi/math/controller/PIDController.hpp>
#include <wpi/math/geometry/Pose2d.hpp>
#include <wpi/math/geometry/Rotation2d.hpp>
#include <wpi/math/geometry/Rotation3d.hpp>
#include <wpi/math/geometry/Translation2d.hpp>
#include <wpi/math/kinematics/ChassisVelocities.hpp>
#include <wpi/units/angle.hpp>
#include <wpi/units/angular_velocity.hpp>
#include <wpi/units/length.hpp>
#include <wpi/units/time.hpp>
#include <wpi/units/velocity.hpp>
#include <wpi/util/Alert.hpp>
 
#include "yams/math/DerivativeTimeFilter.hpp"
#include "yams/mechanisms/config/GyroAxis.hpp"
#include "yams/mechanisms/swerve/SwerveModule.hpp"
#include "yams/motorcontrollers/SmartMotorControllerConfig.hpp"
#include "yams/telemetry/SwerveDriveTelemetryConfig.hpp"
 
namespace yams::mechanisms::swerve {
 
class SwerveDriveConfig {
 public:
  using TelemetryVerbosity = motorcontrollers::SmartMotorControllerConfig::TelemetryVerbosity;
 
  SwerveDriveConfig() = default;
 
  // ---- Builder methods -------------------------------------------------------
 
  SwerveDriveConfig& WithSubsystem(wpi::cmd::SubsystemBase* subsystem);
 
  SwerveDriveConfig& WithModules(std::vector<SwerveModule*> modules);
 
  SwerveDriveConfig& WithGyro(std::function<wpi::units::degree_t()> gyroSupplier);
 
  SwerveDriveConfig& WithGyro(std::function<wpi::math::Rotation3d()> gyroSupplier);
 
  SwerveDriveConfig& WithGyroHeadingAxis(config::GyroAxis axis);
 
  SwerveDriveConfig& WithAntiTipping(wpi::units::meters_per_second_t kP,
                                     wpi::units::degree_t tippingThreshold,
                                     wpi::units::meters_per_second_t maxCorrectionSpeed);
 
  SwerveDriveConfig& WithAntiTipping(std::optional<wpi::math::AntiTipping> antiTipping);
 
  SwerveDriveConfig& WithModuleStateOptimization(bool enabled);
 
  SwerveDriveConfig& WithGyroVelocity(
      std::function<wpi::units::degrees_per_second_t()> angularVelocitySupplier);
 
  SwerveDriveConfig& WithGyroOffset(wpi::units::degree_t offset);
 
  SwerveDriveConfig& WithGyroInverted(bool inverted);
 
  SwerveDriveConfig& WithStartingPose(wpi::math::Pose2d pose);
 
  SwerveDriveConfig& WithMaximumChassisSpeed(wpi::units::meters_per_second_t speed,
                                             wpi::units::degrees_per_second_t angularVelocity);
 
  SwerveDriveConfig& WithMaximumModuleSpeed(wpi::units::meters_per_second_t speed);
 
  SwerveDriveConfig& WithCenterOfRotation(wpi::math::Translation2d center);
 
  SwerveDriveConfig& WithCenterOfRotation(wpi::units::meter_t forward, wpi::units::meter_t left);
 
  SwerveDriveConfig& WithDiscretizationTime(wpi::units::second_t dt);
 
  SwerveDriveConfig& WithSimDiscretizationTime(wpi::units::second_t dt);
 
  SwerveDriveConfig& WithGyroAngularVelocityScaleFactor(double scaleFactor);
 
  SwerveDriveConfig& WithSimGyroAngularVelocityScaleFactor(double scaleFactor);
 
  SwerveDriveConfig& WithTranslationController(wpi::math::PIDController controller);
 
  SwerveDriveConfig& WithRotationController(wpi::math::PIDController controller);
 
  SwerveDriveConfig& WithSimTranslationController(wpi::math::PIDController controller);
 
  SwerveDriveConfig& WithSimRotationController(wpi::math::PIDController controller);
 
  SwerveDriveConfig& WithTelemetry(const std::string& name, TelemetryVerbosity verbosity);
 
  SwerveDriveConfig& WithTelemetry(const std::string& name,
                                   telemetry::SwerveDriveTelemetryConfig telemetryConfig);
 
  // ---- Getters ---------------------------------------------------------------
 
  const std::vector<SwerveModule*>& GetModules() const;
 
  wpi::cmd::SubsystemBase* GetSubsystem() const;
 
  wpi::math::Pose2d GetInitialPose() const;
 
  std::optional<wpi::units::meters_per_second_t> GetMaximumChassisLinearVelocity() const;
 
  std::optional<wpi::units::degrees_per_second_t> GetMaximumChassisAngularVelocity() const;
  std::optional<wpi::units::meters_per_second_t> GetMaximumModuleLinearVelocity() const;
  std::optional<wpi::math::Translation2d> GetCenterOfRotation() const;
  std::optional<TelemetryVerbosity> GetTelemetryVerbosity() const;
  const std::string& GetTelemetryName() const;
  std::optional<telemetry::SwerveDriveTelemetryConfig> GetSwerveDriveTelemetryConfig();
 
  wpi::units::degree_t GetGyroOffset() const;
 
  wpi::units::degree_t GetGyroAngle() const;
 
  wpi::math::Rotation3d GetGyroRotation3d() const;
 
  wpi::math::ChassisVelocities GetAntiTippingCorrection() const;
 
  std::optional<bool> GetModuleStateOptimization() const;
 
  std::optional<wpi::units::second_t> GetDiscretization() const;
 
  wpi::math::ChassisVelocities OptimizeRobotRelativeChassisSpeeds(
      wpi::math::ChassisVelocities speeds) const;
 
  std::optional<std::reference_wrapper<wpi::math::PIDController>> GetTranslationPID();
 
  std::optional<std::reference_wrapper<wpi::math::PIDController>> GetRotationPID();
 
  static wpi::math::Translation2d CubeTranslation(wpi::math::Translation2d translation);
 
  static wpi::math::Translation2d ScaleTranslation(wpi::math::Translation2d translation,
                                                   double scalar);
 
 private:
  std::vector<SwerveModule*> m_modules;
  wpi::cmd::SubsystemBase* m_subsystem{nullptr};
  std::string m_telemetryName{"swerve"};
  std::optional<TelemetryVerbosity> m_telemetryVerbosity;
  std::optional<telemetry::SwerveDriveTelemetryConfig> m_specifiedTelemetryConfig;
  std::optional<std::function<wpi::math::Rotation3d()>> m_gyroSupplier;
  config::GyroAxis m_gyroHeadingAxis{config::GyroAxis::YAW};
  // Heading unwrapped across the half rotation boundary, for deriving the angular velocity.
  mutable double m_continuousHeadingRadians{0};
  mutable bool m_continuousHeadingRead{false};
  mutable math::DerivativeTimeFilter m_gyroAngularVelocityFilter{wpi::units::millisecond_t{20}};
  mutable std::optional<wpi::util::Alert> m_noGyroAngularVelocitySupplierAlert;
  std::optional<std::function<wpi::units::degrees_per_second_t()>> m_gyroAngularVelocitySupplier;
  std::optional<wpi::units::degree_t> m_gyroOffset;
  bool m_gyroInverted{false};
  wpi::math::Pose2d m_initialPose;
  std::optional<wpi::units::meters_per_second_t> m_maximumChassisLinearVelocity;
  std::optional<wpi::units::degrees_per_second_t> m_maximumChassisAngularVelocity;
  std::optional<wpi::units::meters_per_second_t> m_maximumModuleLinearVelocity;
  std::optional<bool> m_moduleStateOptimization;
  std::optional<wpi::math::AntiTipping> m_antiTipping;
  std::optional<wpi::units::second_t> m_discretizationSeconds;
  std::optional<wpi::units::second_t> m_simDiscretizationSeconds;
  std::optional<double> m_angularVelocityScaleFactor;
  std::optional<double> m_simAngularVelocityScaleFactor;
  std::optional<wpi::math::Translation2d> m_centerOfRotation;
  std::optional<wpi::math::PIDController> m_translationController;
  std::optional<wpi::math::PIDController> m_rotationController;
  std::optional<wpi::math::PIDController> m_simTranslationController;
  std::optional<wpi::math::PIDController> m_simRotationController;
 
  wpi::math::ChassisVelocities AngularVelocitySkewCorrection(
      wpi::math::ChassisVelocities robotRelativeSpeeds) const;
};
 
}  // namespace yams::mechanisms::swerve