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

Tuned test constants for VelocityPID.

Browse Source 
Also added a GetAvgError method to the PIDController
which averages the past n error values for use with
noisy sensor values (namely, for the velocity stuff).

Change-Id: I8a9cf40259dd56ef9093b36ed6891cc18b9131cf
This commit is contained in:
James Kuszmaul
2015-07-16 14:45:07 -04:00
parent d5922bb037
commit cd4ebbd8a0
5 changed files with 127 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

50
wpilibc/wpilibC++Devices/src/PIDController.cpp
View File

@@ -169,6 +169,15 @@ void PIDController::Calculate() {
result = m_result;
pidOutput->PIDWrite(result);
// Update the buffer.
m_buf.push(m_error);
m_bufTotal += m_error;
// Remove old elements when buffer is full.
if (m_buf.size() > m_bufLength) {
m_bufTotal -= m_buf.front();
m_buf.pop();
}
}
}
}
@@ -310,6 +319,7 @@ 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) {
@@ -325,6 +335,9 @@ void PIDController::SetSetpoint(float setpoint) {
} else {
m_setpoint = setpoint;
}
// Clear m_buf.
m_buf = std::queue<double>();
}
if (m_table != nullptr) {
@@ -368,6 +381,22 @@ PIDSourceType PIDController::GetPIDSourceType() const {
return m_pidInput->GetPIDSourceType();
}
/**
* Returns the current average of the error over the past few iterations.
* You can specify the number of iterations to average with SetToleranceBuffer()
* (defaults to 1). This is the same value that is used for OnTarget().
* @return the average error
*/
float PIDController::GetAvgError() const {
float avgError = 0;
{
std::unique_lock<priority_mutex> sync(m_mutex);
// Don't divide by zero.
if (m_buf.size()) avgError = m_bufTotal / m_buf.size();
}
return avgError;
}
/*
* Set the percentage error which is considered tolerable for use with
* OnTarget.
@@ -407,6 +436,25 @@ void PIDController::SetAbsoluteTolerance(float absTolerance) {
}
}
/*
* Set the number of previous error samples to average for tolerancing. When
* determining whether a mechanism is on target, the user may want to use a
* rolling average of previous measurements instead of a precise position or
* velocity. This is useful for noisy sensors which return a few erroneous
* measurements when the mechanism is on target. However, the mechanism will
* not register as on target for at least the specified bufLength cycles.
* @param bufLength Number of previous cycles to average. Defaults to 1.
*/
void PIDController::SetToleranceBuffer(unsigned bufLength) {
m_bufLength = bufLength;
// Cut the buffer down to size if needed.
while (m_buf.size() > bufLength) {
m_bufTotal -= m_buf.front();
m_buf.pop();
}
}
/*
* Return true if the error is within the percentage of the total input range,
* determined by SetTolerance. This asssumes that the maximum and minimum input
@@ -417,7 +465,7 @@ void PIDController::SetAbsoluteTolerance(float absTolerance) {
* time.
*/
bool PIDController::OnTarget() const {
double error = GetError();
double error = GetAvgError();
std::unique_lock<priority_mutex> sync(m_mutex);
switch (m_toleranceType) {