mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-07-02 02:51:42 +00:00
Move circular buffer class from wpilib to wpiutil (#1840)
This commit is contained in:
Tyler Veness
committed by
Peter Johnson
Peter Johnson
parent
4cd8a56672
commit
e3d86fee46
@@ -12,8 +12,7 @@
|
||||
|
||||
#include <units/units.h>
|
||||
#include <wpi/ArrayRef.h>
|
||||
|
||||
#include "frc/circular_buffer.h"
|
||||
#include <wpi/circular_buffer.h>
|
||||
|
||||
namespace frc {
|
||||
|
||||
@@ -142,8 +141,8 @@ class LinearFilter {
|
||||
double Calculate(double input);
|
||||
|
||||
private:
|
||||
circular_buffer<double> m_inputs{0};
|
||||
circular_buffer<double> m_outputs{0};
|
||||
wpi::circular_buffer<double> m_inputs{0};
|
||||
wpi::circular_buffer<double> m_outputs{0};
|
||||
std::vector<double> m_inputGains;
|
||||
std::vector<double> m_outputGains;
|
||||
};
|
||||
|
||||
@@ -1,62 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2015-2018 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cstddef>
|
||||
#include <vector>
|
||||
|
||||
namespace frc {
|
||||
|
||||
/**
|
||||
* This is a simple circular buffer so we don't need to "bucket brigade" copy
|
||||
* old values.
|
||||
*/
|
||||
template <class T>
|
||||
class circular_buffer {
|
||||
public:
|
||||
explicit circular_buffer(size_t size);
|
||||
|
||||
using value_type = T;
|
||||
using reference = value_type&;
|
||||
using const_reference = const value_type&;
|
||||
using pointer = value_type*;
|
||||
using size_type = size_t;
|
||||
using iterator_category = std::forward_iterator_tag;
|
||||
using difference_type = std::ptrdiff_t;
|
||||
|
||||
size_type size() const;
|
||||
T& front();
|
||||
const T& front() const;
|
||||
T& back();
|
||||
const T& back() const;
|
||||
void push_front(T value);
|
||||
void push_back(T value);
|
||||
T pop_front();
|
||||
T pop_back();
|
||||
void resize(size_t size);
|
||||
void reset();
|
||||
|
||||
T& operator[](size_t index);
|
||||
const T& operator[](size_t index) const;
|
||||
|
||||
private:
|
||||
std::vector<T> m_data;
|
||||
|
||||
// Index of element at front of buffer
|
||||
size_t m_front = 0;
|
||||
|
||||
// Number of elements used in buffer
|
||||
size_t m_length = 0;
|
||||
|
||||
size_t ModuloInc(size_t index);
|
||||
size_t ModuloDec(size_t index);
|
||||
};
|
||||
|
||||
} // namespace frc
|
||||
|
||||
#include "frc/circular_buffer.inc"
|
||||
@@ -1,239 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2015-2018 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <algorithm>
|
||||
|
||||
namespace frc {
|
||||
|
||||
template <class T>
|
||||
circular_buffer<T>::circular_buffer(size_t size) : m_data(size, 0) {}
|
||||
|
||||
/**
|
||||
* Returns number of elements in buffer
|
||||
*/
|
||||
template <class T>
|
||||
typename circular_buffer<T>::size_type circular_buffer<T>::size() const {
|
||||
return m_length;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns value at front of buffer
|
||||
*/
|
||||
template <class T>
|
||||
T& circular_buffer<T>::front() {
|
||||
return (*this)[0];
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns value at front of buffer
|
||||
*/
|
||||
template <class T>
|
||||
const T& circular_buffer<T>::front() const {
|
||||
return (*this)[0];
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns value at back of buffer
|
||||
*/
|
||||
template <class T>
|
||||
T& circular_buffer<T>::back() {
|
||||
// If there are no elements in the buffer, do nothing
|
||||
if (m_length == 0) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
return m_data[(m_front + m_length - 1) % m_data.size()];
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns value at back of buffer
|
||||
*/
|
||||
template <class T>
|
||||
const T& circular_buffer<T>::back() const {
|
||||
// If there are no elements in the buffer, do nothing
|
||||
if (m_length == 0) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
return m_data[(m_front + m_length - 1) % m_data.size()];
|
||||
}
|
||||
|
||||
/**
|
||||
* Push new value onto front of the buffer. The value at the back is overwritten
|
||||
* if the buffer is full.
|
||||
*/
|
||||
template <class T>
|
||||
void circular_buffer<T>::push_front(T value) {
|
||||
if (m_data.size() == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
m_front = ModuloDec(m_front);
|
||||
|
||||
m_data[m_front] = value;
|
||||
|
||||
if (m_length < m_data.size()) {
|
||||
m_length++;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Push new value onto back of the buffer. The value at the front is overwritten
|
||||
* if the buffer is full.
|
||||
*/
|
||||
template <class T>
|
||||
void circular_buffer<T>::push_back(T value) {
|
||||
if (m_data.size() == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
m_data[(m_front + m_length) % m_data.size()] = value;
|
||||
|
||||
if (m_length < m_data.size()) {
|
||||
m_length++;
|
||||
} else {
|
||||
// Increment front if buffer is full to maintain size
|
||||
m_front = ModuloInc(m_front);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Pop value at front of buffer.
|
||||
*/
|
||||
template <class T>
|
||||
T circular_buffer<T>::pop_front() {
|
||||
// If there are no elements in the buffer, do nothing
|
||||
if (m_length == 0) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
T& temp = m_data[m_front];
|
||||
m_front = ModuloInc(m_front);
|
||||
m_length--;
|
||||
return temp;
|
||||
}
|
||||
|
||||
/**
|
||||
* Pop value at back of buffer.
|
||||
*/
|
||||
template <class T>
|
||||
T circular_buffer<T>::pop_back() {
|
||||
// If there are no elements in the buffer, do nothing
|
||||
if (m_length == 0) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
m_length--;
|
||||
return m_data[(m_front + m_length) % m_data.size()];
|
||||
}
|
||||
|
||||
/**
|
||||
* Resizes internal buffer to given size.
|
||||
*/
|
||||
template <class T>
|
||||
void circular_buffer<T>::resize(size_t size) {
|
||||
if (size > m_data.size()) {
|
||||
// Find end of buffer
|
||||
size_t insertLocation = (m_front + m_length) % m_data.size();
|
||||
|
||||
// If insertion location precedes front of buffer, push front index back
|
||||
if (insertLocation <= m_front) {
|
||||
m_front += size - m_data.size();
|
||||
}
|
||||
|
||||
// Add elements to end of buffer
|
||||
m_data.insert(m_data.begin() + insertLocation, size - m_data.size(), 0);
|
||||
} else if (size < m_data.size()) {
|
||||
/* 1) Shift element block start at "front" left as many blocks as were
|
||||
* removed up to but not exceeding buffer[0]
|
||||
* 2) Shrink buffer, which will remove even more elements automatically if
|
||||
* necessary
|
||||
*/
|
||||
size_t elemsToRemove = m_data.size() - size;
|
||||
auto frontIter = m_data.begin() + m_front;
|
||||
if (m_front < elemsToRemove) {
|
||||
/* Remove elements from end of buffer before shifting start of element
|
||||
* block. Doing so saves a few copies.
|
||||
*/
|
||||
m_data.erase(frontIter + size, m_data.end());
|
||||
|
||||
// Shift start of element block to left
|
||||
m_data.erase(m_data.begin(), frontIter);
|
||||
|
||||
// Update metadata
|
||||
m_front = 0;
|
||||
} else {
|
||||
// Shift start of element block to left
|
||||
m_data.erase(frontIter - elemsToRemove, frontIter);
|
||||
|
||||
// Update metadata
|
||||
m_front -= elemsToRemove;
|
||||
}
|
||||
|
||||
/* Length only changes during a shrink if all unused spaces have been
|
||||
* removed. Length decreases as used spaces are removed to meet the
|
||||
* required size.
|
||||
*/
|
||||
if (m_length > size) {
|
||||
m_length = size;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets internal buffer contents to zero.
|
||||
*/
|
||||
template <class T>
|
||||
void circular_buffer<T>::reset() {
|
||||
std::fill(m_data.begin(), m_data.end(), 0);
|
||||
m_front = 0;
|
||||
m_length = 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @return Element at index starting from front of buffer.
|
||||
*/
|
||||
template <class T>
|
||||
T& circular_buffer<T>::operator[](size_t index) {
|
||||
return m_data[(m_front + index) % m_data.size()];
|
||||
}
|
||||
|
||||
/**
|
||||
* @return Element at index starting from front of buffer.
|
||||
*/
|
||||
template <class T>
|
||||
const T& circular_buffer<T>::operator[](size_t index) const {
|
||||
return m_data[(m_front + index) % m_data.size()];
|
||||
}
|
||||
|
||||
/**
|
||||
* Increment an index modulo the length of the buffer.
|
||||
*
|
||||
* @return The result of the modulo operation.
|
||||
*/
|
||||
template <class T>
|
||||
size_t circular_buffer<T>::ModuloInc(size_t index) {
|
||||
return (index + 1) % m_data.size();
|
||||
}
|
||||
|
||||
/**
|
||||
* Decrement an index modulo the length of the buffer.
|
||||
*
|
||||
* @return The result of the modulo operation.
|
||||
*/
|
||||
template <class T>
|
||||
size_t circular_buffer<T>::ModuloDec(size_t index) {
|
||||
if (index == 0) {
|
||||
return m_data.size() - 1;
|
||||
} else {
|
||||
return index - 1;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace frc
|
||||
@@ -12,9 +12,9 @@
|
||||
#include <vector>
|
||||
|
||||
#include <wpi/ArrayRef.h>
|
||||
#include <wpi/circular_buffer.h>
|
||||
#include <wpi/deprecated.h>
|
||||
|
||||
#include "frc/circular_buffer.h"
|
||||
#include "frc/filters/Filter.h"
|
||||
|
||||
namespace frc {
|
||||
@@ -215,8 +215,8 @@ class LinearDigitalFilter : public Filter {
|
||||
double PIDGet() override;
|
||||
|
||||
private:
|
||||
circular_buffer<double> m_inputs;
|
||||
circular_buffer<double> m_outputs;
|
||||
wpi::circular_buffer<double> m_inputs;
|
||||
wpi::circular_buffer<double> m_outputs;
|
||||
std::vector<double> m_inputGains;
|
||||
std::vector<double> m_outputGains;
|
||||
};
|
||||
|
||||
@@ -1,211 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2015-2018 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/circular_buffer.h" // NOLINT(build/include_order)
|
||||
|
||||
#include <array>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
static const std::array<double, 10> values = {
|
||||
{751.848, 766.366, 342.657, 234.252, 716.126, 132.344, 445.697, 22.727,
|
||||
421.125, 799.913}};
|
||||
|
||||
static const std::array<double, 8> pushFrontOut = {
|
||||
{799.913, 421.125, 22.727, 445.697, 132.344, 716.126, 234.252, 342.657}};
|
||||
|
||||
static const std::array<double, 8> pushBackOut = {
|
||||
{342.657, 234.252, 716.126, 132.344, 445.697, 22.727, 421.125, 799.913}};
|
||||
|
||||
TEST(CircularBufferTest, PushFrontTest) {
|
||||
circular_buffer<double> queue(8);
|
||||
|
||||
for (auto& value : values) {
|
||||
queue.push_front(value);
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < pushFrontOut.size(); i++) {
|
||||
EXPECT_EQ(pushFrontOut[i], queue[i]);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(CircularBufferTest, PushBackTest) {
|
||||
circular_buffer<double> queue(8);
|
||||
|
||||
for (auto& value : values) {
|
||||
queue.push_back(value);
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < pushBackOut.size(); i++) {
|
||||
EXPECT_EQ(pushBackOut[i], queue[i]);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(CircularBufferTest, PushPopTest) {
|
||||
circular_buffer<double> queue(3);
|
||||
|
||||
// Insert three elements into the buffer
|
||||
queue.push_back(1.0);
|
||||
queue.push_back(2.0);
|
||||
queue.push_back(3.0);
|
||||
|
||||
EXPECT_EQ(1.0, queue[0]);
|
||||
EXPECT_EQ(2.0, queue[1]);
|
||||
EXPECT_EQ(3.0, queue[2]);
|
||||
|
||||
/*
|
||||
* The buffer is full now, so pushing subsequent elements will overwrite the
|
||||
* front-most elements.
|
||||
*/
|
||||
|
||||
queue.push_back(4.0); // Overwrite 1 with 4
|
||||
|
||||
// The buffer now contains 2, 3 and 4
|
||||
EXPECT_EQ(2.0, queue[0]);
|
||||
EXPECT_EQ(3.0, queue[1]);
|
||||
EXPECT_EQ(4.0, queue[2]);
|
||||
|
||||
queue.push_back(5.0); // Overwrite 2 with 5
|
||||
|
||||
// The buffer now contains 3, 4 and 5
|
||||
EXPECT_EQ(3.0, queue[0]);
|
||||
EXPECT_EQ(4.0, queue[1]);
|
||||
EXPECT_EQ(5.0, queue[2]);
|
||||
|
||||
EXPECT_EQ(5.0, queue.pop_back()); // 5 is removed
|
||||
|
||||
// The buffer now contains 3 and 4
|
||||
EXPECT_EQ(3.0, queue[0]);
|
||||
EXPECT_EQ(4.0, queue[1]);
|
||||
|
||||
EXPECT_EQ(3.0, queue.pop_front()); // 3 is removed
|
||||
|
||||
// Leaving only one element with value == 4
|
||||
EXPECT_EQ(4.0, queue[0]);
|
||||
}
|
||||
|
||||
TEST(CircularBufferTest, ResetTest) {
|
||||
circular_buffer<double> queue(5);
|
||||
|
||||
for (size_t i = 1; i < 6; i++) {
|
||||
queue.push_back(i);
|
||||
}
|
||||
|
||||
queue.reset();
|
||||
|
||||
for (size_t i = 0; i < 5; i++) {
|
||||
EXPECT_EQ(0.0, queue[i]);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(CircularBufferTest, ResizeTest) {
|
||||
circular_buffer<double> queue(5);
|
||||
|
||||
/* Buffer contains {1, 2, 3, _, _}
|
||||
* ^ front
|
||||
*/
|
||||
queue.push_back(1.0);
|
||||
queue.push_back(2.0);
|
||||
queue.push_back(3.0);
|
||||
|
||||
queue.resize(2);
|
||||
EXPECT_EQ(1.0, queue[0]);
|
||||
EXPECT_EQ(2.0, queue[1]);
|
||||
|
||||
queue.resize(5);
|
||||
EXPECT_EQ(1.0, queue[0]);
|
||||
EXPECT_EQ(2.0, queue[1]);
|
||||
|
||||
queue.reset();
|
||||
|
||||
/* Buffer contains {_, 1, 2, 3, _}
|
||||
* ^ front
|
||||
*/
|
||||
queue.push_back(0.0);
|
||||
queue.push_back(1.0);
|
||||
queue.push_back(2.0);
|
||||
queue.push_back(3.0);
|
||||
queue.pop_front();
|
||||
|
||||
queue.resize(2);
|
||||
EXPECT_EQ(1.0, queue[0]);
|
||||
EXPECT_EQ(2.0, queue[1]);
|
||||
|
||||
queue.resize(5);
|
||||
EXPECT_EQ(1.0, queue[0]);
|
||||
EXPECT_EQ(2.0, queue[1]);
|
||||
|
||||
queue.reset();
|
||||
|
||||
/* Buffer contains {_, _, 1, 2, 3}
|
||||
* ^ front
|
||||
*/
|
||||
queue.push_back(0.0);
|
||||
queue.push_back(0.0);
|
||||
queue.push_back(1.0);
|
||||
queue.push_back(2.0);
|
||||
queue.push_back(3.0);
|
||||
queue.pop_front();
|
||||
queue.pop_front();
|
||||
|
||||
queue.resize(2);
|
||||
EXPECT_EQ(1.0, queue[0]);
|
||||
EXPECT_EQ(2.0, queue[1]);
|
||||
|
||||
queue.resize(5);
|
||||
EXPECT_EQ(1.0, queue[0]);
|
||||
EXPECT_EQ(2.0, queue[1]);
|
||||
|
||||
queue.reset();
|
||||
|
||||
/* Buffer contains {3, _, _, 1, 2}
|
||||
* ^ front
|
||||
*/
|
||||
queue.push_back(3.0);
|
||||
queue.push_front(2.0);
|
||||
queue.push_front(1.0);
|
||||
|
||||
queue.resize(2);
|
||||
EXPECT_EQ(1.0, queue[0]);
|
||||
EXPECT_EQ(2.0, queue[1]);
|
||||
|
||||
queue.resize(5);
|
||||
EXPECT_EQ(1.0, queue[0]);
|
||||
EXPECT_EQ(2.0, queue[1]);
|
||||
|
||||
queue.reset();
|
||||
|
||||
/* Buffer contains {2, 3, _, _, 1}
|
||||
* ^ front
|
||||
*/
|
||||
queue.push_back(2.0);
|
||||
queue.push_back(3.0);
|
||||
queue.push_front(1.0);
|
||||
|
||||
queue.resize(2);
|
||||
EXPECT_EQ(1.0, queue[0]);
|
||||
EXPECT_EQ(2.0, queue[1]);
|
||||
|
||||
queue.resize(5);
|
||||
EXPECT_EQ(1.0, queue[0]);
|
||||
EXPECT_EQ(2.0, queue[1]);
|
||||
|
||||
// Test push_back() after resize
|
||||
queue.push_back(3.0);
|
||||
EXPECT_EQ(1.0, queue[0]);
|
||||
EXPECT_EQ(2.0, queue[1]);
|
||||
EXPECT_EQ(3.0, queue[2]);
|
||||
|
||||
// Test push_front() after resize
|
||||
queue.push_front(4.0);
|
||||
EXPECT_EQ(4.0, queue[0]);
|
||||
EXPECT_EQ(1.0, queue[1]);
|
||||
EXPECT_EQ(2.0, queue[2]);
|
||||
EXPECT_EQ(3.0, queue[3]);
|
||||
}
|
||||
Reference in New Issue
Block a user