[glass] Add Profiled PID controller support & IZone Support (#5959)

2026-06-19 00:41:43 +00:00 · 2023-12-22 14:29:25 -05:00
parent bcef6c5398
commit 43fb6e9f87
11 changed files with 303 additions and 2 deletions
--- a/glass/src/libnt/native/cpp/NTProfiledPIDController.cpp
+++ b/glass/src/libnt/native/cpp/NTProfiledPIDController.cpp
@@ -0,0 +1,80 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/networktables/NTProfiledPIDController.h"
+
+#include <fmt/format.h>
+#include <wpi/StringExtras.h>
+
+using namespace glass;
+
+NTProfiledPIDControllerModel::NTProfiledPIDControllerModel(
+    std::string_view path)
+    : NTProfiledPIDControllerModel(nt::NetworkTableInstance::GetDefault(),
+                                   path) {}
+
+NTProfiledPIDControllerModel::NTProfiledPIDControllerModel(
+    nt::NetworkTableInstance inst, std::string_view path)
+    : m_inst{inst},
+      m_name{inst.GetStringTopic(fmt::format("{}/.name", path)).Subscribe("")},
+      m_controllable{inst.GetBooleanTopic(fmt::format("{}/.controllable", path))
+                         .Subscribe(false)},
+      m_p{inst.GetDoubleTopic(fmt::format("{}/p", path)).GetEntry(0)},
+      m_i{inst.GetDoubleTopic(fmt::format("{}/i", path)).GetEntry(0)},
+      m_d{inst.GetDoubleTopic(fmt::format("{}/d", path)).GetEntry(0)},
+      m_goal{inst.GetDoubleTopic(fmt::format("{}/goal", path)).GetEntry(0)},
+      m_iZone{inst.GetDoubleTopic(fmt::format("{}/izone", path)).GetEntry(0)},
+      m_pData{fmt::format("NTPIDCtrlP:{}", path)},
+      m_iData{fmt::format("NTPIDCtrlI:{}", path)},
+      m_dData{fmt::format("NTPIDCtrlD:{}", path)},
+      m_goalData{fmt::format("NTPIDCtrlGoal:{}", path)},
+      m_iZoneData{fmt::format("NTPIDCtrlIZone:{}", path)},
+      m_nameValue{wpi::rsplit(path, '/').second} {}
+
+void NTProfiledPIDControllerModel::SetP(double value) {
+  m_p.Set(value);
+}
+
+void NTProfiledPIDControllerModel::SetI(double value) {
+  m_i.Set(value);
+}
+
+void NTProfiledPIDControllerModel::SetD(double value) {
+  m_d.Set(value);
+}
+
+void NTProfiledPIDControllerModel::SetGoal(double value) {
+  m_goal.Set(value);
+}
+void NTProfiledPIDControllerModel::SetIZone(double value) {
+  m_iZone.Set(value);
+}
+
+void NTProfiledPIDControllerModel::Update() {
+  for (auto&& v : m_name.ReadQueue()) {
+    m_nameValue = std::move(v.value);
+  }
+  for (auto&& v : m_p.ReadQueue()) {
+    m_pData.SetValue(v.value, v.time);
+  }
+  for (auto&& v : m_i.ReadQueue()) {
+    m_iData.SetValue(v.value, v.time);
+  }
+  for (auto&& v : m_d.ReadQueue()) {
+    m_dData.SetValue(v.value, v.time);
+  }
+  for (auto&& v : m_goal.ReadQueue()) {
+    m_goalData.SetValue(v.value, v.time);
+  }
+  for (auto&& v : m_iZone.ReadQueue()) {
+    m_iZoneData.SetValue(v.value, v.time);
+  }
+  for (auto&& v : m_controllable.ReadQueue()) {
+    m_controllableValue = v.value;
+  }
+}
+
+bool NTProfiledPIDControllerModel::Exists() {
+  return m_goal.Exists();
+}