Add constraints support to ProfiledPIDController Sendable implementation

rzblue · Feb 8, 2024 · 6d5b0bf · 6d5b0bf
1 parent 1db3936
commit 6d5b0bf
Show file tree

Hide file tree

Showing 6 changed files with 87 additions and 17 deletions.
diff --git a/glass/src/lib/native/cpp/other/ProfiledPIDController.cpp b/glass/src/lib/native/cpp/other/ProfiledPIDController.cpp
@@ -52,15 +52,25 @@ void glass::DisplayProfiledPIDController(ProfiledPIDControllerModel* m) {
       double value = d->GetValue();
       createTuningParameter("D", &value, [=](auto v) { m->SetD(v); });
     }
-    if (auto s = m->GetGoalData()) {
-      double value = s->GetValue();
-      createTuningParameter("Goal", &value, [=](auto v) { m->SetGoal(v); });
-    }
     if (auto s = m->GetIZoneData()) {
       double value = s->GetValue();
       createTuningParameterNoFilter("IZone", &value,
                                     [=](auto v) { m->SetIZone(v); });
     }
+    if (auto s = m->GetMaxVelocityData()) {
+      double value = s->GetValue();
+      createTuningParameter("Max Velocity", &value,
+                            [=](auto v) { m->SetMaxVelocity(v); });
+    }
+    if (auto s = m->GetMaxAccelerationData()) {
+      double value = s->GetValue();
+      createTuningParameter("Max Acceleration", &value,
+                            [=](auto v) { m->SetMaxAcceleration(v); });
+    }
+    if (auto s = m->GetGoalData()) {
+      double value = s->GetValue();
+      createTuningParameter("Goal", &value, [=](auto v) { m->SetGoal(v); });
+    }
   } else {
     ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(96, 96, 96, 255));
     ImGui::Text("Unknown PID Controller");

diff --git a/glass/src/lib/native/include/glass/other/ProfiledPIDController.h b/glass/src/lib/native/include/glass/other/ProfiledPIDController.h
@@ -15,14 +15,18 @@ class ProfiledPIDControllerModel : public Model {
   virtual DataSource* GetPData() = 0;
   virtual DataSource* GetIData() = 0;
   virtual DataSource* GetDData() = 0;
-  virtual DataSource* GetGoalData() = 0;
   virtual DataSource* GetIZoneData() = 0;
+  virtual DataSource* GetMaxVelocityData() = 0;
+  virtual DataSource* GetMaxAccelerationData() = 0;
+  virtual DataSource* GetGoalData() = 0;
 
   virtual void SetP(double value) = 0;
   virtual void SetI(double value) = 0;
   virtual void SetD(double value) = 0;
-  virtual void SetGoal(double value) = 0;
   virtual void SetIZone(double value) = 0;
+  virtual void SetMaxVelocity(double value) = 0;
+  virtual void SetMaxAcceleration(double value) = 0;
+  virtual void SetGoal(double value) = 0;
 };
 void DisplayProfiledPIDController(ProfiledPIDControllerModel* m);
 }  // namespace glass
diff --git a/glass/src/libnt/native/cpp/NTProfiledPIDController.cpp b/glass/src/libnt/native/cpp/NTProfiledPIDController.cpp
@@ -23,13 +23,20 @@ NTProfiledPIDControllerModel::NTProfiledPIDControllerModel(
       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_maxVelocity{
+          inst.GetDoubleTopic(fmt::format("{}/maxvelocity", path)).GetEntry(0)},
+      m_maxAcceleration{
+          inst.GetDoubleTopic(fmt::format("{}/maxacceleration", path))
+              .GetEntry(0)},
+      m_goal{inst.GetDoubleTopic(fmt::format("{}/goal", 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_maxVelocityData{fmt::format("NTPIDCtrlMaxVelo:{}", path)},
+      m_maxAccelerationData{fmt::format("NTPIDCtrlMaxAccel:{}", path)},
+      m_goalData{fmt::format("NTPIDCtrlGoal:{}", path)},
       m_nameValue{wpi::rsplit(path, '/').second} {}
 
 void NTProfiledPIDControllerModel::SetP(double value) {
@@ -44,13 +51,21 @@ void NTProfiledPIDControllerModel::SetD(double value) {
   m_d.Set(value);
 }
 
-void NTProfiledPIDControllerModel::SetGoal(double value) {
-  m_goal.Set(value);
+void NTProfiledPIDControllerModel::SetMaxVelocity(double value) {
+  m_maxVelocity.Set(value);
+}
+
+void NTProfiledPIDControllerModel::SetMaxAcceleration(double value) {
+  m_maxAcceleration.Set(value);
 }
+
 void NTProfiledPIDControllerModel::SetIZone(double value) {
   m_iZone.Set(value);
 }
 
+void NTProfiledPIDControllerModel::SetGoal(double value) {
+  m_goal.Set(value);
+}
 void NTProfiledPIDControllerModel::Update() {
   for (auto&& v : m_name.ReadQueue()) {
     m_nameValue = std::move(v.value);
@@ -64,12 +79,18 @@ void NTProfiledPIDControllerModel::Update() {
   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_maxVelocity.ReadQueue()) {
+    m_maxVelocityData.SetValue(v.value, v.time);
+  }
+  for (auto&& v : m_maxAcceleration.ReadQueue()) {
+    m_maxAccelerationData.SetValue(v.value, v.time);
+  }
+  for (auto&& v : m_goal.ReadQueue()) {
+    m_goalData.SetValue(v.value, v.time);
+  }
   for (auto&& v : m_controllable.ReadQueue()) {
     m_controllableValue = v.value;
   }

diff --git a/glass/src/libnt/native/include/glass/networktables/NTProfiledPIDController.h b/glass/src/libnt/native/include/glass/networktables/NTProfiledPIDController.h
@@ -29,14 +29,20 @@ class NTProfiledPIDControllerModel : public ProfiledPIDControllerModel {
   DataSource* GetPData() override { return &m_pData; }
   DataSource* GetIData() override { return &m_iData; }
   DataSource* GetDData() override { return &m_dData; }
-  DataSource* GetGoalData() override { return &m_goalData; }
   DataSource* GetIZoneData() override { return &m_iZoneData; }
+  DataSource* GetMaxVelocityData() override { return &m_maxVelocityData; }
+  DataSource* GetMaxAccelerationData() override {
+    return &m_maxAccelerationData;
+  }
+  DataSource* GetGoalData() override { return &m_goalData; }
 
   void SetP(double value) override;
   void SetI(double value) override;
   void SetD(double value) override;
-  void SetGoal(double value) override;
   void SetIZone(double value) override;
+  void SetMaxVelocity(double value) override;
+  void SetMaxAcceleration(double value) override;
+  void SetGoal(double value) override;
 
   void Update() override;
   bool Exists() override;
@@ -49,14 +55,18 @@ class NTProfiledPIDControllerModel : public ProfiledPIDControllerModel {
   nt::DoubleEntry m_p;
   nt::DoubleEntry m_i;
   nt::DoubleEntry m_d;
-  nt::DoubleEntry m_goal;
+  nt::DoubleEntry m_maxVelocity;
+  nt::DoubleEntry m_maxAcceleration;
   nt::DoubleEntry m_iZone;
+  nt::DoubleEntry m_goal;
 
   DataSource m_pData;
   DataSource m_iData;
   DataSource m_dData;
-  DataSource m_goalData;
+  DataSource m_maxVelocityData;
+  DataSource m_maxAccelerationData;
   DataSource m_iZoneData;
+  DataSource m_goalData;
 
   std::string m_nameValue;
   bool m_controllableValue = false;

diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java
@@ -444,6 +444,18 @@ public void initSendable(SendableBuilder builder) {
             MathSharedStore.reportError("IZone must be a non-negative number!", e.getStackTrace());
           }
         });
+    builder.addDoubleProperty(
+        "maxvelocity",
+        () -> getConstraints().maxVelocity,
+        maxVelocity ->
+            setConstraints(
+                new TrapezoidProfile.Constraints(maxVelocity, getConstraints().maxAcceleration)));
+    builder.addDoubleProperty(
+        "maxacceleration",
+        () -> getConstraints().maxAcceleration,
+        maxAcceleration ->
+            setConstraints(
+                new TrapezoidProfile.Constraints(getConstraints().maxVelocity, maxAcceleration)));
     builder.addDoubleProperty("goal", () -> getGoal().position, this::setGoal);
   }
 }
diff --git a/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h b/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
@@ -404,6 +404,19 @@ class ProfiledPIDController
     builder.AddDoubleProperty(
         "izone", [this] { return GetIZone(); },
         [this](double value) { SetIZone(value); });
+    builder.AddDoubleProperty(
+        "maxvelocity", [this] { return GetConstraints().maxVelocity.value(); },
+        [this](double value) {
+          SetConstraints(
+              Constraints{Velocity_t{value}, GetConstraints().maxAcceleration});
+        });
+    builder.AddDoubleProperty(
+        "maxacceleration",
+        [this] { return GetConstraints().maxAcceleration.value(); },
+        [this](double value) {
+          SetConstraints(
+              Constraints{GetConstraints().maxVelocity, Acceleration_t{value}});
+        });
     builder.AddDoubleProperty(
         "goal", [this] { return GetGoal().position.value(); },
         [this](double value) { SetGoal(Distance_t{value}); });