Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Simplified PIDController integration logic (#645)

Browse Source 
A clamp() helper function was written based on C++17's std::clamp().
This commit is contained in:
Tyler Veness
2017-11-13 22:28:55 -08:00
committed by Peter Johnson
parent 020ee227d2
commit 14fcf3f2f0
2 changed files with 21 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files

35
wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java
View File

@@ -261,32 +261,16 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
if (m_pidInput.getPIDSourceType().equals(PIDSourceType.kRate)) {
if (m_P != 0) {
double potentialPGain = (m_totalError + m_error) * m_P;
if (potentialPGain < m_maximumOutput) {
if (potentialPGain > m_minimumOutput) {
m_totalError += m_error;
} else {
m_totalError = m_minimumOutput / m_P;
}
} else {
m_totalError = m_maximumOutput / m_P;
}
m_totalError = clamp(m_totalError + m_error, m_minimumOutput / m_P,
m_maximumOutput / m_P);
}
m_result = m_P * m_totalError + m_D * m_error
+ calculateFeedForward();
} else {
if (m_I != 0) {
double potentialIGain = (m_totalError + m_error) * m_I;
if (potentialIGain < m_maximumOutput) {
if (potentialIGain > m_minimumOutput) {
m_totalError += m_error;
} else {
m_totalError = m_minimumOutput / m_I;
}
} else {
m_totalError = m_maximumOutput / m_I;
}
m_totalError = clamp(m_totalError + m_error, m_minimumOutput / m_I,
m_maximumOutput / m_I);
}
m_result = m_P * m_error + m_I * m_totalError
@@ -294,11 +278,8 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
}
m_prevError = m_error;
if (m_result > m_maximumOutput) {
m_result = m_maximumOutput;
} else if (m_result < m_minimumOutput) {
m_result = m_minimumOutput;
}
m_result = clamp(m_result, m_minimumOutput, m_maximumOutput);
pidOutput = m_pidOutput;
result = m_result;
@@ -838,4 +819,8 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
@Override
public void stopLiveWindowMode() {
}
private static double clamp(double value, double low, double high) {
return Math.max(low, Math.min(value, high));
}
}