SmartMotorControllerTelemetry.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 <memory>
#include <optional>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>
#include <wpi/datalog/DataLog.hpp>
#include <wpi/nt/BooleanTopic.hpp>
#include <wpi/nt/DoubleTopic.hpp>
#include <wpi/nt/NetworkTable.hpp>
#include <wpi/system/DataLogManager.hpp>
#include <wpi/util/json.hpp>
 
// DoubleTelemetry<F>::TransformUnit() calls SmartMotorControllerConfig methods directly in its
// (header-only, template) body; since SmartMotorControllerConfig is not a dependent type there,
// two-phase lookup requires it to be complete at this point, not just forward-declared.
#include "yams/motorcontrollers/SmartMotorControllerConfig.hpp"
 
namespace yams::motorcontrollers {
class SmartMotorController;
}  // namespace yams::motorcontrollers
 
namespace yams::telemetry {
 
// ---- Field enums ------------------------------------------------------------
 
enum class DoubleTelemetryField {
  ExponentialProfileKV,               
  ExponentialProfileKA,               
  ExponentialProfileMaxInput,         
  TrapezoidalProfileMaxVelocity,      
  TrapezoidalProfileMaxAcceleration,  
  TrapezoidalProfileMaxJerk,          
  TunableClosedLoopControllerSlot,    
  ActiveClosedLoopControllerSlot,     
  kS,                                 
  kV,                                 
  kA,                                 
  kG,                                 
  kP,                                 
  kI,                                 
  kD,                                 
  TunableSetpointPosition,            
  SetpointPosition,                   
  TunableSetpointVelocity,            
  SetpointVelocity,                   
  SetpointForce,                      
  ClosedLoopPeriod,                   
  VelocityMeasurementDelay,           
  TunableVoltage,  
  OutputVoltage,                      
  StatorCurrent,                      
  StatorCurrentLimit,                 
  SupplyCurrent,                      
  SupplyCurrentLimit,                 
  MotorTemperature,                   
  MeasurementPosition,                
  MeasurementVelocity,                
  MeasurementAcceleration,            
  MeasurementLowerLimit,              
  MeasurementUpperLimit,              
  MechanismPosition,                  
  MechanismVelocity,                  
  MechanismAcceleration,              
  MechanismLowerLimit,                
  MechanismUpperLimit,                
  RotorPosition,                      
  RotorVelocity,                      
  ExternalEncoderPosition,  
  ExternalEncoderVelocity,  
  ClosedloopRampRate,       
  OpenloopRampRate,         
};
 
enum class BooleanTelemetryField {
  MechanismUpperLimit,     
  MechanismLowerLimit,     
  TemperatureLimit,        
  VelocityControl,         
  ElevatorFeedForward,     
  ArmFeedForward,          
  SimpleMotorFeedForward,  
  MotionProfile,           
  MotorInversion,          
  EncoderInversion,        
  TunableVoltageEnabled,   
};
 
// ---- UnsupportedTelemetryFields ---------------------------------------------
 
struct UnsupportedTelemetryFields {
  std::optional<std::vector<BooleanTelemetryField>> boolFields;   
  std::optional<std::vector<DoubleTelemetryField>> doubleFields;  
};
 
// ---- DoubleTelemetry --------------------------------------------------------
 
template <typename F>
class DoubleTelemetry {
 public:
  DoubleTelemetry(std::string key, double defaultVal, F field, bool tunable, std::string unit)
      : m_field{field},
        m_key{std::move(key)},
        m_tunable{tunable},
        m_unit{std::move(unit)},
        m_defaultValue{defaultVal},
        m_cachedValue{defaultVal} {}
 
  void SetDefaultValue(double value) { m_cachedValue = m_defaultValue = value; }
 
  void SetupNetworkTables(std::shared_ptr<wpi::nt::NetworkTable> dataTable,
                          std::shared_ptr<wpi::nt::NetworkTable> tuningTable) {
    m_dataTable = dataTable;
    m_tuningTable = tuningTable;
    if (!m_enabled) return;
 
    if (tuningTable && m_tunable) {
      auto topic = tuningTable->GetDoubleTopic(m_key);
      if (m_unit != "none") {
        topic.SetProperties(wpi::util::json{{"units", m_unit}});
      }
      m_subPublisher = topic.Publish();
      m_subPublisher->SetDefault(m_defaultValue);
      m_subscriber = topic.Subscribe(m_defaultValue);
    } else if (dataTable) {
      auto topic = dataTable->GetDoubleTopic(m_key);
      if (m_unit != "none") {
        topic.SetProperties(wpi::util::json{{"units", m_unit}});
      }
      m_publisher = topic.Publish();
      m_publisher->SetDefault(m_defaultValue);
    }
  }
 
  void SetupNetworkTable(std::shared_ptr<wpi::nt::NetworkTable> dataTable) {
    SetupNetworkTables(dataTable, nullptr);
  }
 
  void SetupDataLog(const std::string& prefix) {
    if (m_tunable) return;
    std::string path = prefix;
    if (!path.empty() && path.back() != '/') path += '/';
    path += m_unit + '/' + m_key;
    m_dataLogEntry = wpi::log::DoubleLogEntry{wpi::DataLogManager::GetLog(), path};
  }
 
  void TransformUnit(const motorcontrollers::SmartMotorControllerConfig& cfg) {
    bool linear = cfg.GetLinearClosedLoopControllerUse();
    if (m_unit == "tunable_position") {
      m_unit = linear ? "meter" : "degrees";
    } else if (m_unit == "position") {
      m_unit = linear ? "meter" : "rotations";
    } else if (m_unit == "tunable_velocity") {
      m_unit = linear ? "meter_per_second" : "rotations_per_minute";
    } else if (m_unit == "velocity") {
      m_unit = linear ? "meter_per_second" : "rotation_per_second";
    } else if (m_unit == "tunable_acceleration") {
      m_unit = linear ? "meter_per_second_per_second" : "rotations_per_minute_per_second";
    } else if (m_unit == "acceleration") {
      m_unit = linear ? "meter_per_second_per_second" : "rotation_per_second_per_second";
    }
  }
 
  void ForceSet(double value) {
    if (m_subPublisher) {
      m_subPublisher->Set(value);
    } else if (m_publisher) {
      m_publisher->Set(value);
    }
  }
 
  bool Set(double value) {
    if (!m_enabled) return false;
    if (m_dataLogEntry) {
      m_dataLogEntry->Append(value);
    }
    if (m_subscriber) {
      double tuningValue = m_subscriber->Get(m_defaultValue);
      if (tuningValue != value) return false;
    }
    if (m_publisher) {
      m_publisher->Set(value);
    }
    return true;
  }
 
  double Get() const {
    if (!m_enabled) return m_defaultValue;
    if (m_subscriber) {
      return m_subscriber->Get(m_defaultValue);
    }
    throw std::runtime_error("Tuning table not configured for " + m_key + "!");
  }
 
  bool IsTunable() {
    if (m_subscriber && m_tunable && m_enabled) {
      double current = m_subscriber->Get(m_defaultValue);
      if (current != m_cachedValue) {
        m_cachedValue = current;
        return true;
      }
    }
    return false;
  }
 
  void Enable() { m_enabled = true; }
  void Disable() { m_enabled = false; }
  void Display(bool state) { m_enabled = state; }
  F GetField() const { return m_field; }
  bool IsEnabled() const { return m_enabled; }
 
  void Close() {
    m_subscriber.reset();
    m_subPublisher.reset();
    m_publisher.reset();
    if (m_dataTable) {
      m_dataTable->GetEntry(m_key).Unpublish();
    }
    if (m_tuningTable) {
      m_tuningTable->GetEntry(m_key).Unpublish();
    }
  }
 
 private:
  F m_field;
  std::string m_key;
  bool m_tunable;
  bool m_enabled{false};
  std::string m_unit;
  double m_defaultValue;
  double m_cachedValue;
 
  std::optional<wpi::nt::DoublePublisher> m_publisher;
  std::optional<wpi::nt::DoubleSubscriber> m_subscriber;
  std::optional<wpi::nt::DoublePublisher> m_subPublisher;  // tunable: publish + subscribe
  std::optional<wpi::log::DoubleLogEntry> m_dataLogEntry;
 
  std::shared_ptr<wpi::nt::NetworkTable> m_tuningTable;
  std::shared_ptr<wpi::nt::NetworkTable> m_dataTable;
};
 
// ---- BooleanTelemetry -------------------------------------------------------
 
template <typename F>
class BooleanTelemetry {
 public:
  BooleanTelemetry(std::string key, bool defaultVal, F field, bool tunable)
      : m_field{field},
        m_key{std::move(key)},
        m_tunable{tunable},
        m_defaultValue{defaultVal},
        m_cachedValue{defaultVal} {}
 
  void SetDefaultValue(bool value) { m_cachedValue = m_defaultValue = value; }
 
  void SetupNetworkTables(std::shared_ptr<wpi::nt::NetworkTable> dataTable,
                          std::shared_ptr<wpi::nt::NetworkTable> tuningTable) {
    m_dataTable = dataTable;
    m_tuningTable = tuningTable;
    if (!m_enabled) return;
 
    if (tuningTable && m_tunable) {
      auto topic = tuningTable->GetBooleanTopic(m_key);
      m_pubSub = topic.Publish();
      m_pubSub->SetDefault(m_defaultValue);
      m_subscriber = topic.Subscribe(m_defaultValue);
    } else if (dataTable) {
      auto topic = dataTable->GetBooleanTopic(m_key);
      m_publisher = topic.Publish();
      m_publisher->SetDefault(m_defaultValue);
    }
  }
 
  void SetupNetworkTable(std::shared_ptr<wpi::nt::NetworkTable> dataTable) {
    SetupNetworkTables(dataTable, nullptr);
  }
 
  void SetupDataLog(const std::string& prefix) {
    if (m_tunable) return;
    std::string path = prefix;
    if (!path.empty() && path.back() != '/') path += '/';
    path += m_key;
    m_dataLogEntry = wpi::log::BooleanLogEntry{wpi::DataLogManager::GetLog(), path};
  }
 
  bool Set(bool value) {
    if (!m_enabled) return false;
    if (m_dataLogEntry) {
      m_dataLogEntry->Append(value);
    }
    if (m_subscriber) {
      bool tuningValue = m_subscriber->Get(m_defaultValue);
      if (tuningValue != value) return false;
    }
    if (m_publisher) {
      m_publisher->Set(value);
    }
    return true;
  }
 
  bool Get() const {
    if (!m_enabled) return m_defaultValue;
    if (m_subscriber) {
      return m_subscriber->Get(m_defaultValue);
    }
    throw std::runtime_error("Tuning table not configured for " + m_key + "!");
  }
 
  void ForceSet(bool value) {
    if (m_pubSub) {
      m_pubSub->Set(value);
    } else if (m_publisher) {
      m_publisher->Set(value);
    }
  }
 
  bool IsTunable() {
    if (m_subscriber && m_tunable && m_enabled) {
      bool current = m_subscriber->Get(m_defaultValue);
      if (current != m_cachedValue) {
        m_cachedValue = current;
        return true;
      }
    }
    return false;
  }
 
  void Enable() { m_enabled = true; }
  void Disable() { m_enabled = false; }
  void Display(bool state) { m_enabled = state; }
  F GetField() const { return m_field; }
  bool IsEnabled() const { return m_enabled; }
 
  void Close() {
    m_subscriber.reset();
    m_pubSub.reset();
    m_publisher.reset();
    if (m_dataTable) {
      m_dataTable->GetEntry(m_key).Unpublish();
    }
    if (m_tuningTable) {
      m_tuningTable->GetEntry(m_key).Unpublish();
    }
  }
 
 private:
  F m_field;
  std::string m_key;
  bool m_tunable;
  bool m_enabled{false};
  bool m_defaultValue;
  bool m_cachedValue;
 
  std::optional<wpi::nt::BooleanPublisher> m_publisher;
  std::optional<wpi::nt::BooleanSubscriber> m_subscriber;
  std::optional<wpi::nt::BooleanPublisher> m_pubSub;  // tunable: publish + subscribe
  std::optional<wpi::log::BooleanLogEntry> m_dataLogEntry;
 
  std::shared_ptr<wpi::nt::NetworkTable> m_tuningTable;
  std::shared_ptr<wpi::nt::NetworkTable> m_dataTable;
};
 
// ---- SmartMotorControllerTelemetry ------------------------------------------
 
class SmartMotorControllerTelemetry {
 public:
  void SetupTelemetry(
      motorcontrollers::SmartMotorController& smc,
      std::shared_ptr<wpi::nt::NetworkTable> publishTable,
      std::shared_ptr<wpi::nt::NetworkTable> tuningTable,
      std::unordered_map<DoubleTelemetryField, DoubleTelemetry<DoubleTelemetryField>>& doubleFields,
      std::unordered_map<BooleanTelemetryField, BooleanTelemetry<BooleanTelemetryField>>&
          boolFields,
      bool nt4Enabled, std::optional<std::string> dataLogName);
 
  void Publish(motorcontrollers::SmartMotorController& smc);
 
  void ApplyTuningValues(motorcontrollers::SmartMotorController& smc);
 
  void StopTuning(motorcontrollers::SmartMotorController& smc);
 
  bool TuningEnabled() const;
 
  void Close();
 
 private:
  std::shared_ptr<wpi::nt::NetworkTable> m_dataTable;
  std::shared_ptr<wpi::nt::NetworkTable> m_tuningTable;
 
  std::unordered_map<DoubleTelemetryField, DoubleTelemetry<DoubleTelemetryField>>* m_doubleFields{
      nullptr};
  std::unordered_map<BooleanTelemetryField, BooleanTelemetry<BooleanTelemetryField>>* m_boolFields{
      nullptr};
};
 
}  // namespace yams::telemetry