Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +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

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