diff --git a/glass/src/lib/native/cpp/other/ProfiledPIDController.cpp b/glass/src/lib/native/cpp/other/ProfiledPIDController.cpp index 8113c8cf1f..f9d37b9001 100644 --- 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 index bebf3abb06..a087757da6 100644 --- 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 index bae069b74f..798864c313 100644 --- 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 index e968f79eb5..2197354157 100644 --- 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_iZone; + nt::DoubleEntry m_maxVelocity; + nt::DoubleEntry m_maxAcceleration; + nt::DoubleEntry m_goal; DataSource m_pData; DataSource m_iData; DataSource m_dData; - DataSource m_goalData; DataSource m_iZoneData; + DataSource m_maxVelocityData; + DataSource m_maxAccelerationData; + 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 index e69c73f12c..e96172dcb8 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 @@ -444,6 +444,18 @@ public class ProfiledPIDController implements Sendable { 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 index 72278affca..18f371d252 100644 --- 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}); });