From 4df0b7d9b6aba1438e610ef868a518154613e62f Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Fri, 24 Nov 2023 18:14:06 -0800 Subject: [PATCH] [wpimath] Add generic circular buffer class to Java The original is now called DoubleCircularBuffer. --- .../wpi/first/math/filter/LinearFilter.java | 10 +- .../wpi/first/math/filter/MedianFilter.java | 6 +- .../edu/wpi/first/util/CircularBuffer.java | 51 +++-- .../wpi/first/util/DoubleCircularBuffer.java | 191 ++++++++++++++++ .../main/native/include/wpi/circular_buffer.h | 10 +- .../wpi/first/util/CircularBufferTest.java | 16 +- .../first/util/DoubleCircularBufferTest.java | 213 ++++++++++++++++++ 7 files changed, 455 insertions(+), 42 deletions(-) create mode 100644 wpiutil/src/main/java/edu/wpi/first/util/DoubleCircularBuffer.java create mode 100644 wpiutil/src/test/java/edu/wpi/first/util/DoubleCircularBufferTest.java diff --git a/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java b/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java index 98dd4e1615c..94175a6bdd2 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java +++ b/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java @@ -6,7 +6,7 @@ import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.MathUsageId; -import edu.wpi.first.util.CircularBuffer; +import edu.wpi.first.util.DoubleCircularBuffer; import java.util.Arrays; import org.ejml.simple.SimpleMatrix; @@ -48,8 +48,8 @@ * to make sure calculate() gets called at the desired, constant frequency! */ public class LinearFilter { - private final CircularBuffer m_inputs; - private final CircularBuffer m_outputs; + private final DoubleCircularBuffer m_inputs; + private final DoubleCircularBuffer m_outputs; private final double[] m_inputGains; private final double[] m_outputGains; @@ -62,8 +62,8 @@ public class LinearFilter { * @param fbGains The "feedback" or IIR gains. */ public LinearFilter(double[] ffGains, double[] fbGains) { - m_inputs = new CircularBuffer(ffGains.length); - m_outputs = new CircularBuffer(fbGains.length); + m_inputs = new DoubleCircularBuffer(ffGains.length); + m_outputs = new DoubleCircularBuffer(fbGains.length); m_inputGains = Arrays.copyOf(ffGains, ffGains.length); m_outputGains = Arrays.copyOf(fbGains, fbGains.length); diff --git a/wpimath/src/main/java/edu/wpi/first/math/filter/MedianFilter.java b/wpimath/src/main/java/edu/wpi/first/math/filter/MedianFilter.java index c24f6e9139f..13c4e104373 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/filter/MedianFilter.java +++ b/wpimath/src/main/java/edu/wpi/first/math/filter/MedianFilter.java @@ -4,7 +4,7 @@ package edu.wpi.first.math.filter; -import edu.wpi.first.util.CircularBuffer; +import edu.wpi.first.util.DoubleCircularBuffer; import java.util.ArrayList; import java.util.Collections; import java.util.List; @@ -15,7 +15,7 @@ * processing, LIDAR, or ultrasonic sensors). */ public class MedianFilter { - private final CircularBuffer m_valueBuffer; + private final DoubleCircularBuffer m_valueBuffer; private final List m_orderedValues; private final int m_size; @@ -26,7 +26,7 @@ public class MedianFilter { */ public MedianFilter(int size) { // Circular buffer of values currently in the window, ordered by time - m_valueBuffer = new CircularBuffer(size); + m_valueBuffer = new DoubleCircularBuffer(size); // List of values currently in the window, ordered by value m_orderedValues = new ArrayList<>(size); // Size of rolling window diff --git a/wpiutil/src/main/java/edu/wpi/first/util/CircularBuffer.java b/wpiutil/src/main/java/edu/wpi/first/util/CircularBuffer.java index 729c8b19184..6041e44253e 100644 --- a/wpiutil/src/main/java/edu/wpi/first/util/CircularBuffer.java +++ b/wpiutil/src/main/java/edu/wpi/first/util/CircularBuffer.java @@ -4,11 +4,9 @@ package edu.wpi.first.util; -import java.util.Arrays; - /** This is a simple circular buffer so we don't need to "bucket brigade" copy old values. */ -public class CircularBuffer { - private double[] m_data; +public class CircularBuffer { + private T[] m_data; // Index of element at front of buffer private int m_front; @@ -21,9 +19,9 @@ public class CircularBuffer { * * @param size The size of the circular buffer. */ + @SuppressWarnings("unchecked") public CircularBuffer(int size) { - m_data = new double[size]; - Arrays.fill(m_data, 0.0); + m_data = (T[]) new Object[size]; } /** @@ -40,7 +38,7 @@ public int size() { * * @return value at front of buffer */ - public double getFirst() { + public T getFirst() { return m_data[m_front]; } @@ -48,11 +46,14 @@ public double getFirst() { * Get value at back of buffer. * * @return value at back of buffer + * @throws IndexOutOfBoundsException if the index is out of range (index < 0 || index >= + * size()) */ - public double getLast() { + @SuppressWarnings("unchecked") + public T getLast() { // If there are no elements in the buffer, do nothing if (m_length == 0) { - return 0.0; + throw new IndexOutOfBoundsException("getLast() called on an empty container"); } return m_data[(m_front + m_length - 1) % m_data.length]; @@ -64,7 +65,7 @@ public double getLast() { * * @param value The value to push. */ - public void addFirst(double value) { + public void addFirst(T value) { if (m_data.length == 0) { return; } @@ -84,7 +85,7 @@ public void addFirst(double value) { * * @param value The value to push. */ - public void addLast(double value) { + public void addLast(T value) { if (m_data.length == 0) { return; } @@ -103,14 +104,17 @@ public void addLast(double value) { * Pop value at front of buffer. * * @return value at front of buffer + * @throws IndexOutOfBoundsException if the index is out of range (index < 0 || index >= + * size()) */ - public double removeFirst() { + @SuppressWarnings("unchecked") + public T removeFirst() { // If there are no elements in the buffer, do nothing if (m_length == 0) { - return 0.0; + throw new IndexOutOfBoundsException("removeFirst() called on an empty container"); } - double temp = m_data[m_front]; + T temp = m_data[m_front]; m_front = moduloInc(m_front); m_length--; return temp; @@ -120,11 +124,14 @@ public double removeFirst() { * Pop value at back of buffer. * * @return value at back of buffer + * @throws IndexOutOfBoundsException if the index is out of range (index < 0 || index >= + * size()) */ - public double removeLast() { + @SuppressWarnings("unchecked") + public T removeLast() { // If there are no elements in the buffer, do nothing if (m_length == 0) { - return 0.0; + throw new IndexOutOfBoundsException("removeLast() called on an empty container"); } m_length--; @@ -138,8 +145,9 @@ public double removeLast() { * * @param size New buffer size. */ + @SuppressWarnings("unchecked") public void resize(int size) { - double[] newBuffer = new double[size]; + var newBuffer = (T[]) new Object[size]; m_length = Math.min(m_length, size); for (int i = 0; i < m_length; i++) { newBuffer[i] = m_data[(m_front + i) % m_data.length]; @@ -150,7 +158,6 @@ public void resize(int size) { /** Sets internal buffer contents to zero. */ public void clear() { - Arrays.fill(m_data, 0.0); m_front = 0; m_length = 0; } @@ -161,23 +168,25 @@ public void clear() { * @param index Index into the buffer. * @return Element at index starting from front of buffer. */ - public double get(int index) { + public T get(int index) { return m_data[(m_front + index) % m_data.length]; } /** - * Increment an index modulo the length of the m_data buffer. + * Increment an index modulo the length of the buffer. * * @param index Index into the buffer. + * @return The incremented index. */ private int moduloInc(int index) { return (index + 1) % m_data.length; } /** - * Decrement an index modulo the length of the m_data buffer. + * Decrement an index modulo the length of the buffer. * * @param index Index into the buffer. + * @return The decremented index. */ private int moduloDec(int index) { if (index == 0) { diff --git a/wpiutil/src/main/java/edu/wpi/first/util/DoubleCircularBuffer.java b/wpiutil/src/main/java/edu/wpi/first/util/DoubleCircularBuffer.java new file mode 100644 index 00000000000..548f14bf0a8 --- /dev/null +++ b/wpiutil/src/main/java/edu/wpi/first/util/DoubleCircularBuffer.java @@ -0,0 +1,191 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.util; + +import java.util.Arrays; + +/** This is a simple circular buffer so we don't need to "bucket brigade" copy old values. */ +public class DoubleCircularBuffer { + private double[] m_data; + + // Index of element at front of buffer + private int m_front; + + // Number of elements used in buffer + private int m_length; + + /** + * Create a CircularBuffer with the provided size. + * + * @param size The size of the circular buffer. + */ + public DoubleCircularBuffer(int size) { + m_data = new double[size]; + Arrays.fill(m_data, 0.0); + } + + /** + * Returns number of elements in buffer. + * + * @return number of elements in buffer + */ + public int size() { + return m_length; + } + + /** + * Get value at front of buffer. + * + * @return value at front of buffer + */ + public double getFirst() { + return m_data[m_front]; + } + + /** + * Get value at back of buffer. + * + * @return value at back of buffer + */ + public double getLast() { + // If there are no elements in the buffer, do nothing + if (m_length == 0) { + return 0.0; + } + + return m_data[(m_front + m_length - 1) % m_data.length]; + } + + /** + * Push new value onto front of the buffer. The value at the back is overwritten if the buffer is + * full. + * + * @param value The value to push. + */ + public void addFirst(double value) { + if (m_data.length == 0) { + return; + } + + m_front = moduloDec(m_front); + + m_data[m_front] = value; + + if (m_length < m_data.length) { + m_length++; + } + } + + /** + * Push new value onto back of the buffer. The value at the front is overwritten if the buffer is + * full. + * + * @param value The value to push. + */ + public void addLast(double value) { + if (m_data.length == 0) { + return; + } + + m_data[(m_front + m_length) % m_data.length] = value; + + if (m_length < m_data.length) { + m_length++; + } else { + // Increment front if buffer is full to maintain size + m_front = moduloInc(m_front); + } + } + + /** + * Pop value at front of buffer. + * + * @return value at front of buffer + */ + public double removeFirst() { + // If there are no elements in the buffer, do nothing + if (m_length == 0) { + return 0.0; + } + + double temp = m_data[m_front]; + m_front = moduloInc(m_front); + m_length--; + return temp; + } + + /** + * Pop value at back of buffer. + * + * @return value at back of buffer + */ + public double removeLast() { + // If there are no elements in the buffer, do nothing + if (m_length == 0) { + return 0.0; + } + + m_length--; + return m_data[(m_front + m_length) % m_data.length]; + } + + /** + * Resizes internal buffer to given size. + * + *

A new buffer is allocated because arrays are not resizable. + * + * @param size New buffer size. + */ + public void resize(int size) { + double[] newBuffer = new double[size]; + m_length = Math.min(m_length, size); + for (int i = 0; i < m_length; i++) { + newBuffer[i] = m_data[(m_front + i) % m_data.length]; + } + m_data = newBuffer; + m_front = 0; + } + + /** Sets internal buffer contents to zero. */ + public void clear() { + Arrays.fill(m_data, 0.0); + m_front = 0; + m_length = 0; + } + + /** + * Get the element at the provided index relative to the start of the buffer. + * + * @param index Index into the buffer. + * @return Element at index starting from front of buffer. + */ + public double get(int index) { + return m_data[(m_front + index) % m_data.length]; + } + + /** + * Increment an index modulo the length of the buffer. + * + * @param index Index into the buffer. + * @return The incremented index. + */ + private int moduloInc(int index) { + return (index + 1) % m_data.length; + } + + /** + * Decrement an index modulo the length of the buffer. + * + * @param index Index into the buffer. + * @return The decremented index. + */ + private int moduloDec(int index) { + if (index == 0) { + return m_data.length - 1; + } else { + return index - 1; + } + } +} diff --git a/wpiutil/src/main/native/include/wpi/circular_buffer.h b/wpiutil/src/main/native/include/wpi/circular_buffer.h index c54e2f55db3..93dabaf1370 100644 --- a/wpiutil/src/main/native/include/wpi/circular_buffer.h +++ b/wpiutil/src/main/native/include/wpi/circular_buffer.h @@ -268,16 +268,18 @@ class circular_buffer { size_t m_length = 0; /** - * Increment an index modulo the length of the buffer. + * Increment an index modulo the size of the buffer. * - * @return The result of the modulo operation. + * @param index Index into the buffer. + * @return The incremented index. */ size_t ModuloInc(size_t index) { return (index + 1) % m_data.size(); } /** - * Decrement an index modulo the length of the buffer. + * Decrement an index modulo the size of the buffer. * - * @return The result of the modulo operation. + * @param index Index into the buffer. + * @return The decremented index. */ size_t ModuloDec(size_t index) { if (index == 0) { diff --git a/wpiutil/src/test/java/edu/wpi/first/util/CircularBufferTest.java b/wpiutil/src/test/java/edu/wpi/first/util/CircularBufferTest.java index 79727543946..a040ba56422 100644 --- a/wpiutil/src/test/java/edu/wpi/first/util/CircularBufferTest.java +++ b/wpiutil/src/test/java/edu/wpi/first/util/CircularBufferTest.java @@ -21,7 +21,7 @@ class CircularBufferTest { @Test void addFirstTest() { - CircularBuffer queue = new CircularBuffer(8); + var queue = new CircularBuffer(8); for (double value : m_values) { queue.addFirst(value); @@ -34,7 +34,7 @@ void addFirstTest() { @Test void addLastTest() { - CircularBuffer queue = new CircularBuffer(8); + var queue = new CircularBuffer(8); for (double value : m_values) { queue.addLast(value); @@ -47,7 +47,7 @@ void addLastTest() { @Test void pushPopTest() { - CircularBuffer queue = new CircularBuffer(3); + var queue = new CircularBuffer(3); // Insert three elements into the buffer queue.addLast(1.0); @@ -91,22 +91,20 @@ void pushPopTest() { @Test void resetTest() { - CircularBuffer queue = new CircularBuffer(5); + var queue = new CircularBuffer(5); for (int i = 0; i < 6; i++) { - queue.addLast(i); + queue.addLast((double) i); } queue.clear(); - for (int i = 0; i < 5; i++) { - assertEquals(0.0, queue.get(i), 0.00005); - } + assertEquals(0, queue.size()); } @Test void resizeTest() { - CircularBuffer queue = new CircularBuffer(5); + var queue = new CircularBuffer(5); /* Buffer contains {1, 2, 3, _, _} * ^ front diff --git a/wpiutil/src/test/java/edu/wpi/first/util/DoubleCircularBufferTest.java b/wpiutil/src/test/java/edu/wpi/first/util/DoubleCircularBufferTest.java new file mode 100644 index 00000000000..58a39f4b0ae --- /dev/null +++ b/wpiutil/src/test/java/edu/wpi/first/util/DoubleCircularBufferTest.java @@ -0,0 +1,213 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.util; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import org.junit.jupiter.api.Test; + +class DoubleCircularBufferTest { + private final double[] m_values = { + 751.848, 766.366, 342.657, 234.252, 716.126, 132.344, 445.697, 22.727, 421.125, 799.913 + }; + private final double[] m_addFirstOut = { + 799.913, 421.125, 22.727, 445.697, 132.344, 716.126, 234.252, 342.657 + }; + private final double[] m_addLastOut = { + 342.657, 234.252, 716.126, 132.344, 445.697, 22.727, 421.125, 799.913 + }; + + @Test + void addFirstTest() { + var queue = new DoubleCircularBuffer(8); + + for (double value : m_values) { + queue.addFirst(value); + } + + for (int i = 0; i < m_addFirstOut.length; i++) { + assertEquals(m_addFirstOut[i], queue.get(i), 0.00005); + } + } + + @Test + void addLastTest() { + var queue = new DoubleCircularBuffer(8); + + for (double value : m_values) { + queue.addLast(value); + } + + for (int i = 0; i < m_addLastOut.length; i++) { + assertEquals(m_addLastOut[i], queue.get(i), 0.00005); + } + } + + @Test + void pushPopTest() { + var queue = new DoubleCircularBuffer(3); + + // Insert three elements into the buffer + queue.addLast(1.0); + queue.addLast(2.0); + queue.addLast(3.0); + + assertEquals(1.0, queue.get(0), 0.00005); + assertEquals(2.0, queue.get(1), 0.00005); + assertEquals(3.0, queue.get(2), 0.00005); + + /* + * The buffer is full now, so pushing subsequent elements will overwrite the + * front-most elements. + */ + + queue.addLast(4.0); // Overwrite 1 with 4 + + // The buffer now contains 2, 3, and 4 + assertEquals(2.0, queue.get(0), 0.00005); + assertEquals(3.0, queue.get(1), 0.00005); + assertEquals(4.0, queue.get(2), 0.00005); + + queue.addLast(5.0); // Overwrite 2 with 5 + + // The buffer now contains 3, 4, and 5 + assertEquals(3.0, queue.get(0), 0.00005); + assertEquals(4.0, queue.get(1), 0.00005); + assertEquals(5.0, queue.get(2), 0.00005); + + assertEquals(5.0, queue.removeLast(), 0.00005); // 5 is removed + + // The buffer now contains 3 and 4 + assertEquals(3.0, queue.get(0), 0.00005); + assertEquals(4.0, queue.get(1), 0.00005); + + assertEquals(3.0, queue.removeFirst(), 0.00005); // 3 is removed + + // Leaving only one element with value == 4 + assertEquals(4.0, queue.get(0), 0.00005); + } + + @Test + void resetTest() { + var queue = new DoubleCircularBuffer(5); + + for (int i = 0; i < 6; i++) { + queue.addLast(i); + } + + queue.clear(); + + for (int i = 0; i < 5; i++) { + assertEquals(0.0, queue.get(i), 0.00005); + } + } + + @Test + void resizeTest() { + var queue = new DoubleCircularBuffer(5); + + /* Buffer contains {1, 2, 3, _, _} + * ^ front + */ + queue.addLast(1.0); + queue.addLast(2.0); + queue.addLast(3.0); + + queue.resize(2); + assertEquals(1.0, queue.get(0), 0.00005); + assertEquals(2.0, queue.get(1), 0.00005); + + queue.resize(5); + assertEquals(1.0, queue.get(0), 0.00005); + assertEquals(2.0, queue.get(1), 0.00005); + + queue.clear(); + + /* Buffer contains {_, 1, 2, 3, _} + * ^ front + */ + queue.addLast(0.0); + queue.addLast(1.0); + queue.addLast(2.0); + queue.addLast(3.0); + queue.removeFirst(); + + queue.resize(2); + assertEquals(1.0, queue.get(0), 0.00005); + assertEquals(2.0, queue.get(1), 0.00005); + + queue.resize(5); + assertEquals(1.0, queue.get(0), 0.00005); + assertEquals(2.0, queue.get(1), 0.00005); + + queue.clear(); + + /* Buffer contains {_, _, 1, 2, 3} + * ^ front + */ + queue.addLast(0.0); + queue.addLast(0.0); + queue.addLast(1.0); + queue.addLast(2.0); + queue.addLast(3.0); + queue.removeFirst(); + queue.removeFirst(); + + queue.resize(2); + assertEquals(1.0, queue.get(0), 0.00005); + assertEquals(2.0, queue.get(1), 0.00005); + + queue.resize(5); + assertEquals(1.0, queue.get(0), 0.00005); + assertEquals(2.0, queue.get(1), 0.00005); + + queue.clear(); + + /* Buffer contains {3, _, _, 1, 2} + * ^ front + */ + queue.addLast(3.0); + queue.addFirst(2.0); + queue.addFirst(1.0); + + queue.resize(2); + assertEquals(1.0, queue.get(0), 0.00005); + assertEquals(2.0, queue.get(1), 0.00005); + + queue.resize(5); + assertEquals(1.0, queue.get(0), 0.00005); + assertEquals(2.0, queue.get(1), 0.00005); + + queue.clear(); + + /* Buffer contains {2, 3, _, _, 1} + * ^ front + */ + queue.addLast(2.0); + queue.addLast(3.0); + queue.addFirst(1.0); + + queue.resize(2); + assertEquals(1.0, queue.get(0), 0.00005); + assertEquals(2.0, queue.get(1), 0.00005); + + queue.resize(5); + assertEquals(1.0, queue.get(0), 0.00005); + assertEquals(2.0, queue.get(1), 0.00005); + + // Test addLast() after resize + queue.addLast(3.0); + assertEquals(1.0, queue.get(0), 0.00005); + assertEquals(2.0, queue.get(1), 0.00005); + assertEquals(3.0, queue.get(2), 0.00005); + + // Test addFirst() after resize + queue.addFirst(4.0); + assertEquals(4.0, queue.get(0), 0.00005); + assertEquals(1.0, queue.get(1), 0.00005); + assertEquals(2.0, queue.get(2), 0.00005); + assertEquals(3.0, queue.get(3), 0.00005); + } +}