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

PIDController queue now stores inputs instead of errors (#138)

Browse Source 
Closes #29.
This commit is contained in:
Tyler Veness
2016-08-12 22:04:44 -07:00
committed by Peter Johnson
parent 63469c00c4
commit 7501ae65a1
2 changed files with 15 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -148,8 +148,8 @@ void PIDController::Calculate() {
pidOutput->PIDWrite(result);
// Update the buffer.
m_buf.push(m_error);
m_bufTotal += m_error;
m_buf.push(input);
m_bufTotal += input;
// Remove old elements when buffer is full.
if (m_buf.size() > m_bufLength) {
m_bufTotal -= m_buf.front();
@@ -332,8 +332,6 @@ void PIDController::SetOutputRange(float minimumOutput, float maximumOutput) {
/**
* Set the setpoint for the PIDController.
*
* Clears the queue for GetAvgError().
*
* @param setpoint the desired setpoint
*/
void PIDController::SetSetpoint(float setpoint) {
@@ -350,10 +348,6 @@ void PIDController::SetSetpoint(float setpoint) {
} else {
m_setpoint = setpoint;
}
// Clear m_buf.
m_buf = std::queue<double>();
m_bufTotal = 0;
}
if (m_table != nullptr) {
@@ -422,7 +416,7 @@ float PIDController::GetAvgError() const {
{
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
// Don't divide by zero.
if (m_buf.size()) avgError = m_bufTotal / m_buf.size();
if (m_buf.size()) avgError = m_setpoint - m_bufTotal / m_buf.size();
}
return avgError;
}
@@ -536,6 +530,10 @@ void PIDController::Disable() {
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
m_pidOutput->PIDWrite(0);
m_enabled = false;
// Clear buffer
m_buf = std::queue<double>();
m_bufTotal = 0;
}
if (m_table != nullptr) {