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

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

37
wpilibc/athena/src/PIDController.cpp
View File

@@ -110,16 +110,7 @@ void PIDController::Calculate() {
float result;
PIDOutput* pidOutput;
m_error = m_setpoint - input;
if (m_continuous) {
if (std::fabs(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() == PIDSourceType::kRate) {
if (m_P != 0) {
@@ -403,12 +394,11 @@ double PIDController::GetDeltaSetpoint() const {
* @return the current error
*/
float PIDController::GetError() const {
double pidInput;
double setpoint = GetSetpoint();
{
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
pidInput = m_pidInput->PIDGet();
return GetContinuousError(setpoint - m_pidInput->PIDGet());
}
return GetSetpoint() - pidInput;
}
/**
@@ -598,6 +588,27 @@ void PIDController::InitTable(std::shared_ptr<ITable> subtable) {
}
}
/**
* 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.
*/
double PIDController::GetContinuousError(double error) const {
if (m_continuous) {
if (std::fabs(error) > (m_maximumInput - m_minimumInput) / 2) {
if (error > 0) {
return error - (m_maximumInput - m_minimumInput);
} else {
return error + (m_maximumInput - m_minimumInput);
}
}
}
return error;
}
std::shared_ptr<ITable> PIDController::GetTable() const { return m_table; }
void PIDController::ValueChanged(ITable* source, llvm::StringRef key,

1
wpilibc/shared/include/PIDController.h
View File

@@ -88,6 +88,7 @@ class PIDController : public LiveWindowSendable,
std::shared_ptr<ITable> m_table;
virtual void Calculate();
virtual double CalculateFeedForward();
double GetContinuousError(double error) const;
private:
// factor for "proportional" control

32
wpilibc/sim/src/PIDController.cpp
View File

@@ -126,9 +126,9 @@ void PIDController::Calculate() {
if (m_continuous) {
if (std::fabs(m_error) > (m_maximumInput - m_minimumInput) / 2) {
if (m_error > 0) {
m_error = m_error - m_maximumInput + m_minimumInput;
m_error = m_error - (m_maximumInput - m_minimumInput);
} else {
m_error = m_error + m_maximumInput - m_minimumInput;
m_error = m_error + (m_maximumInput - m_minimumInput);
}
}
}
@@ -403,12 +403,11 @@ double PIDController::GetDeltaSetpoint() const {
* @return the current error
*/
float PIDController::GetError() const {
double pidInput;
double setpoint = GetSetpoint();
{
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
pidInput = m_pidInput->PIDGet();
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
return GetContinuousError(setpoint - m_pidInput->PIDGet());
}
return GetSetpoint() - pidInput;
}
/**
@@ -596,6 +595,27 @@ void PIDController::InitTable(std::shared_ptr<ITable> table) {
}
}
/**
* 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.
*/
double PIDController::GetContinuousError(double error) const {
if (m_continuous) {
if (std::fabs(error) > (m_maximumInput - m_minimumInput) / 2) {
if (error > 0) {
return error - (m_maximumInput - m_minimumInput);
} else {
return error + (m_maximumInput - m_minimumInput);
}
}
}
return error;
}
std::shared_ptr<ITable> PIDController::GetTable() const { return m_table; }
void PIDController::ValueChanged(ITable* source, llvm::StringRef key,

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() {