SwerveDriveTelemetryConfig.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 <optional>
#include <string>
#include <unordered_map>
#include <vector>
#include <wpi/math/geometry/Pose2d.hpp>
#include <wpi/math/geometry/struct/Pose2dStruct.hpp>
#include <wpi/math/kinematics/ChassisVelocities.hpp>
#include <wpi/math/kinematics/SwerveModuleVelocity.hpp>
#include <wpi/math/kinematics/struct/ChassisVelocitiesStruct.hpp>
#include <wpi/math/kinematics/struct/SwerveModuleVelocityStruct.hpp>
 
#include "yams/motorcontrollers/SmartMotorControllerConfig.hpp"
#include "yams/telemetry/SmartMotorControllerTelemetry.hpp"
#include "yams/telemetry/StructArrayTelemetry.hpp"
#include "yams/telemetry/StructTelemetry.hpp"
 
namespace yams::telemetry {
 
class SwerveDriveTelemetryConfig {
 public:
  using TelemetryVerbosity = motorcontrollers::SmartMotorControllerConfig::TelemetryVerbosity;
 
  enum class StructTelemetryField {
    Pose,
    DesiredRobotRelativeChassisSpeeds,
    CurrentRobotRelativeChassisSpeeds,
    FieldRelativeChassisSpeeds,
  };
 
  enum class StructArrayTelemetryField {
    DesiredModuleStates,
    CurrentModuleStates,
  };
 
  enum class DoubleTelemetryField {
    TranslationP,
    TranslationI,
    TranslationD,
    RotationP,
    RotationI,
    RotationD,
    AutoAlignPoseX,
    AutoAlignPoseY,
    AutoAlignPoseRotation,
    ModulesDriveP,
    ModulesDriveI,
    ModulesDriveD,
    ModulesDriveKs,
    ModulesDriveKv,
    ModulesDriveKa,
    ModulesDriveVelocity,
    ModulesDriveVoltage,
    ModulesAzimuthP,
    ModulesAzimuthI,
    ModulesAzimuthD,
    ModulesAzimuthKs,
    ModulesAzimuthKv,
    ModulesAzimuthKa,
    ModulesAzimuthAngle,
    ModulesAzimuthVoltage,
    Gyro,
  };
 
  enum class BooleanTelemetryField {
    AutoAlignEnabled,
    ModulesDriveTuningEnabled,
    ModulesDriveInPlace,
    ModulesDriveVoltageEnabled,
    ModulesAzimuthTuningEnabled,
    ModulesAzimuthVoltageEnabled,
  };
 
  SwerveDriveTelemetryConfig();
 
  explicit SwerveDriveTelemetryConfig(TelemetryVerbosity verbosity);
 
  SwerveDriveTelemetryConfig& WithDataLogName(const std::string& dataLogName);
 
  SwerveDriveTelemetryConfig& WithNetworkTables(bool enabled);
 
  SwerveDriveTelemetryConfig& WithoutNetworkTables();
 
  SwerveDriveTelemetryConfig& WithTelemetryVerbosity(TelemetryVerbosity verbosity);
 
  SwerveDriveTelemetryConfig& WithPose();
  SwerveDriveTelemetryConfig& WithGyro();
  SwerveDriveTelemetryConfig& WithDesiredRobotRelativeChassisSpeeds();
  SwerveDriveTelemetryConfig& WithCurrentRobotRelativeChassisSpeeds();
  SwerveDriveTelemetryConfig& WithFieldRelativeChassisSpeeds();
  SwerveDriveTelemetryConfig& WithDesiredModuleStates();
  SwerveDriveTelemetryConfig& WithCurrentModuleStates();
 
  std::optional<std::string> GetDataLogName() const;
  bool GetNT4Enabled() const;
 
  std::unordered_map<DoubleTelemetryField, DoubleTelemetry<DoubleTelemetryField>>&
  GetDoubleFields();
 
  std::unordered_map<BooleanTelemetryField, BooleanTelemetry<BooleanTelemetryField>>&
  GetBoolFields();
 
  std::unordered_map<StructTelemetryField,
                     StructTelemetry<wpi::math::Pose2d, StructTelemetryField>>&
  GetPoseFields();
 
  std::unordered_map<StructTelemetryField,
                     StructTelemetry<wpi::math::ChassisVelocities, StructTelemetryField>>&
  GetChassisSpeedsFields();
 
  std::unordered_map<
      StructArrayTelemetryField,
      StructArrayTelemetry<wpi::math::SwerveModuleVelocity, StructArrayTelemetryField>>&
  GetModuleStatesFields();
 
  SwerveDriveTelemetryConfig& WithCustom(DoubleTelemetryField field, bool value);
  SwerveDriveTelemetryConfig& WithCustom(StructTelemetryField field, bool value);
  SwerveDriveTelemetryConfig& WithCustom(StructArrayTelemetryField field, bool value);
  SwerveDriveTelemetryConfig& WithCustom(BooleanTelemetryField field, bool value);
  SwerveDriveTelemetryConfig& WithCustom(const std::vector<DoubleTelemetryField>& fields,
                                         bool value);
  SwerveDriveTelemetryConfig& WithCustom(const std::vector<StructTelemetryField>& fields,
                                         bool value);
  SwerveDriveTelemetryConfig& WithCustom(const std::vector<StructArrayTelemetryField>& fields,
                                         bool value);
  SwerveDriveTelemetryConfig& WithCustom(const std::vector<BooleanTelemetryField>& fields,
                                         bool value);
 
 private:
  std::optional<std::string> m_dataLogName;
  bool m_nt4Telemetry{true};
 
  std::unordered_map<DoubleTelemetryField, DoubleTelemetry<DoubleTelemetryField>> m_doubleFields;
  std::unordered_map<BooleanTelemetryField, BooleanTelemetry<BooleanTelemetryField>> m_boolFields;
  std::unordered_map<StructTelemetryField, StructTelemetry<wpi::math::Pose2d, StructTelemetryField>>
      m_poseFields;
  std::unordered_map<StructTelemetryField,
                     StructTelemetry<wpi::math::ChassisVelocities, StructTelemetryField>>
      m_chassisSpeedsFields;
  std::unordered_map<
      StructArrayTelemetryField,
      StructArrayTelemetry<wpi::math::SwerveModuleVelocity, StructArrayTelemetryField>>
      m_moduleStatesFields;
};
 
}  // namespace yams::telemetry