diff --git a/glass/src/lib/native/cpp/other/PIDController.cpp b/glass/src/lib/native/cpp/other/PIDController.cpp index 3c83b11530..8aa5e1f99d 100644 --- a/glass/src/lib/native/cpp/other/PIDController.cpp +++ b/glass/src/lib/native/cpp/other/PIDController.cpp @@ -29,6 +29,16 @@ void glass::DisplayPIDController(PIDControllerModel* m) { callback(*v); } }; + // Workaround to allow for the input of inf, -inf, and nan + auto createTuningParameterNoFilter = + [flag](const char* name, double* v, + std::function callback) { + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4); + if (ImGui::InputScalar(name, ImGuiDataType_Double, v, NULL, NULL, + "%.3f", flag)) { + callback(*v); + } + }; if (auto p = m->GetPData()) { double value = p->GetValue(); @@ -47,6 +57,11 @@ void glass::DisplayPIDController(PIDControllerModel* m) { createTuningParameter("Setpoint", &value, [=](auto v) { m->SetSetpoint(v); }); } + if (auto s = m->GetIZoneData()) { + double value = s->GetValue(); + createTuningParameterNoFilter("IZone", &value, + [=](auto v) { m->SetIZone(v); }); + } } else { ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(96, 96, 96, 255)); ImGui::Text("Unknown PID Controller"); diff --git a/glass/src/lib/native/cpp/other/ProfiledPIDController.cpp b/glass/src/lib/native/cpp/other/ProfiledPIDController.cpp new file mode 100644 index 0000000000..8113c8cf1f --- /dev/null +++ b/glass/src/lib/native/cpp/other/ProfiledPIDController.cpp @@ -0,0 +1,69 @@ +// 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/other/ProfiledPIDController.h" + +#include + +#include + +#include "glass/Context.h" +#include "glass/DataSource.h" + +using namespace glass; + +void glass::DisplayProfiledPIDController(ProfiledPIDControllerModel* m) { + if (auto name = m->GetName()) { + ImGui::Text("%s", name); + ImGui::Separator(); + } + + if (m->Exists()) { + auto flag = m->IsReadOnly() ? ImGuiInputTextFlags_ReadOnly + : ImGuiInputTextFlags_None; + auto createTuningParameter = [flag](const char* name, double* v, + std::function callback) { + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4); + if (ImGui::InputDouble(name, v, 0.0, 0.0, "%.3f", flag)) { + callback(*v); + } + }; + // Workaround to allow for the input of inf, -inf, and nan + auto createTuningParameterNoFilter = + [flag](const char* name, double* v, + std::function callback) { + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4); + if (ImGui::InputScalar(name, ImGuiDataType_Double, v, NULL, NULL, + "%.3f", flag)) { + callback(*v); + } + }; + + if (auto p = m->GetPData()) { + double value = p->GetValue(); + createTuningParameter("P", &value, [=](auto v) { m->SetP(v); }); + } + if (auto i = m->GetIData()) { + double value = i->GetValue(); + createTuningParameter("I", &value, [=](auto v) { m->SetI(v); }); + } + if (auto d = m->GetDData()) { + 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); }); + } + } else { + ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(96, 96, 96, 255)); + ImGui::Text("Unknown PID Controller"); + ImGui::PopStyleColor(); + } +} diff --git a/glass/src/lib/native/include/glass/other/PIDController.h b/glass/src/lib/native/include/glass/other/PIDController.h index ab0dcb3ff6..8c11c5914d 100644 --- a/glass/src/lib/native/include/glass/other/PIDController.h +++ b/glass/src/lib/native/include/glass/other/PIDController.h @@ -16,11 +16,13 @@ class PIDControllerModel : public Model { virtual DataSource* GetIData() = 0; virtual DataSource* GetDData() = 0; virtual DataSource* GetSetpointData() = 0; + virtual DataSource* GetIZoneData() = 0; virtual void SetP(double value) = 0; virtual void SetI(double value) = 0; virtual void SetD(double value) = 0; virtual void SetSetpoint(double value) = 0; + virtual void SetIZone(double value) = 0; }; void DisplayPIDController(PIDControllerModel* m); } // namespace glass diff --git a/glass/src/lib/native/include/glass/other/ProfiledPIDController.h b/glass/src/lib/native/include/glass/other/ProfiledPIDController.h new file mode 100644 index 0000000000..bebf3abb06 --- /dev/null +++ b/glass/src/lib/native/include/glass/other/ProfiledPIDController.h @@ -0,0 +1,28 @@ +// 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. + +#pragma once + +#include "glass/Model.h" + +namespace glass { +class DataSource; +class ProfiledPIDControllerModel : public Model { + public: + virtual const char* GetName() const = 0; + + virtual DataSource* GetPData() = 0; + virtual DataSource* GetIData() = 0; + virtual DataSource* GetDData() = 0; + virtual DataSource* GetGoalData() = 0; + virtual DataSource* GetIZoneData() = 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; +}; +void DisplayProfiledPIDController(ProfiledPIDControllerModel* m); +} // namespace glass diff --git a/glass/src/libnt/native/cpp/NTPIDController.cpp b/glass/src/libnt/native/cpp/NTPIDController.cpp index 1dde27d6fd..9796f6d447 100644 --- a/glass/src/libnt/native/cpp/NTPIDController.cpp +++ b/glass/src/libnt/native/cpp/NTPIDController.cpp @@ -23,10 +23,12 @@ NTPIDControllerModel::NTPIDControllerModel(nt::NetworkTableInstance inst, m_d{inst.GetDoubleTopic(fmt::format("{}/d", path)).GetEntry(0)}, m_setpoint{ inst.GetDoubleTopic(fmt::format("{}/setpoint", 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_setpointData{fmt::format("NTPIDCtrlStpt:{}", path)}, + m_iZoneData{fmt::format("NTPIDCtrlIZone:{}", path)}, m_nameValue{wpi::rsplit(path, '/').second} {} void NTPIDControllerModel::SetP(double value) { @@ -44,6 +46,9 @@ void NTPIDControllerModel::SetD(double value) { void NTPIDControllerModel::SetSetpoint(double value) { m_setpoint.Set(value); } +void NTPIDControllerModel::SetIZone(double value) { + m_iZone.Set(value); +} void NTPIDControllerModel::Update() { for (auto&& v : m_name.ReadQueue()) { @@ -61,6 +66,9 @@ void NTPIDControllerModel::Update() { for (auto&& v : m_setpoint.ReadQueue()) { m_setpointData.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; } diff --git a/glass/src/libnt/native/cpp/NTProfiledPIDController.cpp b/glass/src/libnt/native/cpp/NTProfiledPIDController.cpp new file mode 100644 index 0000000000..bae069b74f --- /dev/null +++ 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 +#include + +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(); +} diff --git a/glass/src/libnt/native/cpp/StandardNetworkTables.cpp b/glass/src/libnt/native/cpp/StandardNetworkTables.cpp index ef610253e4..9031873ad8 100644 --- a/glass/src/libnt/native/cpp/StandardNetworkTables.cpp +++ b/glass/src/libnt/native/cpp/StandardNetworkTables.cpp @@ -14,6 +14,7 @@ #include "glass/networktables/NTMechanism2D.h" #include "glass/networktables/NTMotorController.h" #include "glass/networktables/NTPIDController.h" +#include "glass/networktables/NTProfiledPIDController.h" #include "glass/networktables/NTStringChooser.h" #include "glass/networktables/NTSubsystem.h" #include "glass/networktables/NetworkTablesProvider.h" @@ -141,6 +142,18 @@ void glass::AddStandardNetworkTablesViews(NetworkTablesProvider& provider) { DisplayPIDController(static_cast(model)); }); }); + provider.Register( + NTProfiledPIDControllerModel::kType, + [](nt::NetworkTableInstance inst, const char* path) { + return std::make_unique(inst, path); + }, + [](Window* win, Model* model, const char* path) { + win->SetFlags(ImGuiWindowFlags_AlwaysAutoResize); + return MakeFunctionView([=] { + DisplayProfiledPIDController( + static_cast(model)); + }); + }); provider.Register( NTMotorControllerModel::kType, [](nt::NetworkTableInstance inst, const char* path) { diff --git a/glass/src/libnt/native/include/glass/networktables/NTPIDController.h b/glass/src/libnt/native/include/glass/networktables/NTPIDController.h index f901f72caa..d19dcea2de 100644 --- a/glass/src/libnt/native/include/glass/networktables/NTPIDController.h +++ b/glass/src/libnt/native/include/glass/networktables/NTPIDController.h @@ -29,11 +29,13 @@ class NTPIDControllerModel : public PIDControllerModel { DataSource* GetIData() override { return &m_iData; } DataSource* GetDData() override { return &m_dData; } DataSource* GetSetpointData() override { return &m_setpointData; } + DataSource* GetIZoneData() override { return &m_iZoneData; } void SetP(double value) override; void SetI(double value) override; void SetD(double value) override; void SetSetpoint(double value) override; + void SetIZone(double value) override; void Update() override; bool Exists() override; @@ -47,11 +49,13 @@ class NTPIDControllerModel : public PIDControllerModel { nt::DoubleEntry m_i; nt::DoubleEntry m_d; nt::DoubleEntry m_setpoint; + nt::DoubleEntry m_iZone; DataSource m_pData; DataSource m_iData; DataSource m_dData; DataSource m_setpointData; + DataSource m_iZoneData; std::string m_nameValue; bool m_controllableValue = false; diff --git a/glass/src/libnt/native/include/glass/networktables/NTProfiledPIDController.h b/glass/src/libnt/native/include/glass/networktables/NTProfiledPIDController.h new file mode 100644 index 0000000000..e968f79eb5 --- /dev/null +++ b/glass/src/libnt/native/include/glass/networktables/NTProfiledPIDController.h @@ -0,0 +1,64 @@ +// 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. + +#pragma once + +#include +#include + +#include +#include +#include +#include + +#include "glass/DataSource.h" +#include "glass/other/ProfiledPIDController.h" + +namespace glass { +class NTProfiledPIDControllerModel : public ProfiledPIDControllerModel { + public: + static constexpr const char* kType = "ProfiledPIDController"; + + explicit NTProfiledPIDControllerModel(std::string_view path); + NTProfiledPIDControllerModel(nt::NetworkTableInstance inst, + std::string_view path); + + const char* GetName() const override { return m_nameValue.c_str(); } + + 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; } + + 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 Update() override; + bool Exists() override; + bool IsReadOnly() override { return !m_controllableValue; } + + private: + nt::NetworkTableInstance m_inst; + nt::StringSubscriber m_name; + nt::BooleanSubscriber m_controllable; + nt::DoubleEntry m_p; + nt::DoubleEntry m_i; + nt::DoubleEntry m_d; + nt::DoubleEntry m_goal; + nt::DoubleEntry m_iZone; + + DataSource m_pData; + DataSource m_iData; + DataSource m_dData; + DataSource m_goalData; + DataSource m_iZoneData; + + std::string m_nameValue; + bool m_controllableValue = false; +}; +} // namespace glass diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java index fce8bd09f5..97c9551668 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java @@ -409,7 +409,16 @@ public class PIDController implements Sendable, AutoCloseable { builder.addDoubleProperty("p", this::getP, this::setP); builder.addDoubleProperty("i", this::getI, this::setI); builder.addDoubleProperty("d", this::getD, this::setD); - builder.addDoubleProperty("izone", this::getIZone, this::setIZone); + builder.addDoubleProperty( + "izone", + this::getIZone, + (double toSet) -> { + try { + setIZone(toSet); + } catch (IllegalArgumentException e) { + MathSharedStore.reportError("IZone must be a non-negative number!", e.getStackTrace()); + } + }); builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint); } } 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 index 7ecff5ab69..e0557b1bad 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java @@ -426,7 +426,16 @@ public class ProfiledPIDController implements Sendable { builder.addDoubleProperty("p", this::getP, this::setP); builder.addDoubleProperty("i", this::getI, this::setI); builder.addDoubleProperty("d", this::getD, this::setD); - builder.addDoubleProperty("izone", this::getIZone, this::setIZone); + builder.addDoubleProperty( + "izone", + this::getIZone, + (double toSet) -> { + try { + setIZone(toSet); + } catch (IllegalArgumentException e) { + MathSharedStore.reportError("IZone must be a non-negative number!", e.getStackTrace()); + } + }); builder.addDoubleProperty("goal", () -> getGoal().position, this::setGoal); } }