Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-01 02:41:48 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add constraints support to ProfiledPIDController Sendable implementation (#6354)

Browse Source
This commit is contained in:
Ryan Blue
2024-05-24 13:39:56 -04:00
committed by GitHub
parent badd090538
commit 65f4505e3c
6 changed files with 87 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
glass/src/lib/native/cpp/other/ProfiledPIDController.cpp
View File

@@ -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");

8
glass/src/lib/native/include/glass/other/ProfiledPIDController.h
View File

@@ -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

35
glass/src/libnt/native/cpp/NTProfiledPIDController.cpp
View File

@@ -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;
}

18
glass/src/libnt/native/include/glass/networktables/NTProfiledPIDController.h
View File

@@ -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;

12
wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java
View File

@@ -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);
}
}

13
wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
View File

@@ -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}); });