Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-30 02:31:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

Fixed PIDController::GetError() for continuous inputs (#169)

Browse Source 
Closes #31
This commit is contained in:
Tyler Veness
2016-07-15 13:44:04 -07:00
committed by Peter Johnson
parent 2c911b0f7a
commit f9ebd3bde6
4 changed files with 73 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
wpilibj/src/shared/java/edu/wpi/first/wpilibj/PIDController.java
View File

@@ -265,16 +265,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
input = pidInput.pidGet();
}
synchronized (this) {
m_error = m_setpoint - input;
if (m_continuous) {
if (Math.abs(m_error) > (m_maximumInput - m_minimumInput) / 2) {
if (m_error > 0) {
m_error = m_error - m_maximumInput + m_minimumInput;
} else {
m_error = m_error + m_maximumInput - m_minimumInput;
}
}
}
m_error = getContinuousError(m_setpoint - input);
if (m_pidInput.getPIDSourceType().equals(PIDSourceType.kRate)) {
if (m_P != 0) {
@@ -546,8 +537,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
* @return the current error
*/
public synchronized double getError() {
// return m_error;
return getSetpoint() - m_pidInput.pidGet();
return getContinuousError(getSetpoint() - m_pidInput.pidGet());
}
/**
@@ -765,6 +755,26 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
}
}
/**
* Wraps error around for continuous inputs. The original error is returned if continuous mode is
* disabled. This is an unsynchronized function.
*
* @param error The current error of the PID controller.
* @return Error for continuous inputs.
*/
protected double getContinuousError(double error) {
if (m_continuous) {
if (Math.abs(error) > (m_maximumInput - m_minimumInput) / 2) {
if (error > 0) {
return error - (m_maximumInput - m_minimumInput);
} else {
return error + (m_maximumInput - m_minimumInput);
}
}
}
return error;
}
@Override
public ITable getTable() {