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

Template C++ LinearFilter to work with unit types (#2142)

Browse Source
This commit is contained in:
Oblarg
2019-12-01 02:12:02 -05:00
committed by Peter Johnson
parent 5b73c17f25
commit 9a515c80f8
7 changed files with 76 additions and 100 deletions
Download Patch File Download Diff File Expand all files Collapse all files

70
wpilibc/src/main/native/cpp/LinearFilter.cpp
View File

@@ -1,70 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2015-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "frc/LinearFilter.h"
#include <cassert>
#include <cmath>
#include <hal/FRCUsageReporting.h>
using namespace frc;
LinearFilter::LinearFilter(wpi::ArrayRef<double> ffGains,
wpi::ArrayRef<double> fbGains)
: m_inputs(ffGains.size()),
m_outputs(fbGains.size()),
m_inputGains(ffGains),
m_outputGains(fbGains) {
static int instances = 0;
instances++;
HAL_Report(HALUsageReporting::kResourceType_LinearFilter, instances);
}
LinearFilter LinearFilter::SinglePoleIIR(double timeConstant,
units::second_t period) {
double gain = std::exp(-period.to<double>() / timeConstant);
return LinearFilter(1.0 - gain, -gain);
}
LinearFilter LinearFilter::HighPass(double timeConstant,
units::second_t period) {
double gain = std::exp(-period.to<double>() / timeConstant);
return LinearFilter({gain, -gain}, {-gain});
}
LinearFilter LinearFilter::MovingAverage(int taps) {
assert(taps > 0);
std::vector<double> gains(taps, 1.0 / taps);
return LinearFilter(gains, {});
}
void LinearFilter::Reset() {
m_inputs.reset();
m_outputs.reset();
}
double LinearFilter::Calculate(double input) {
double retVal = 0.0;
// Rotate the inputs
m_inputs.push_front(input);
// Calculate the new value
for (size_t i = 0; i < m_inputGains.size(); i++) {
retVal += m_inputs[i] * m_inputGains[i];
}
for (size_t i = 0; i < m_outputGains.size(); i++) {
retVal -= m_outputs[i] * m_outputGains[i];
}
// Rotate the outputs
m_outputs.push_front(retVal);
return retVal;
}

4
wpilibc/src/main/native/cpp/PIDBase.cpp
View File

@@ -35,7 +35,7 @@ PIDBase::PIDBase(double Kp, double Ki, double Kd, double Kf, PIDSource& source,
m_F = Kf;
m_pidInput = &source;
m_filter = LinearFilter::MovingAverage(1);
m_filter = LinearFilter<double>::MovingAverage(1);
m_pidOutput = &output;
@@ -196,7 +196,7 @@ void PIDBase::SetPercentTolerance(double percent) {
void PIDBase::SetToleranceBuffer(int bufLength) {
std::scoped_lock lock(m_thisMutex);
m_filter = LinearFilter::MovingAverage(bufLength);
m_filter = LinearFilter<double>::MovingAverage(bufLength);
}
bool PIDBase::OnTarget() const {