Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51: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,