#define WINDOW_SIZE 5 // Adjust the window size as needed
int32_t window_a[WINDOW_SIZE] = {0};
int32_t window_b[WINDOW_SIZE] = {0};
void addValueToWindow(int32_t window[], int32_t value, int& index, bool& filled) {
if (index >= WINDOW_SIZE) {
int32_t calculateMedian(int32_t window[], int index, bool filled) {
int32_t temp[WINDOW_SIZE];
int size = filled ? WINDOW_SIZE : index; // Use current index as size if not filled yet
for (int i = 0; i < size; i++) {
for (int i = 0; i < size - 1; i++) {
for (int j = i + 1; j < size; j++) {
return (temp[size / 2 - 1] + temp[size / 2]) / 2;
void applyMedianFilter(int32_t& a, int32_t& b) {
addValueToWindow(window_a, a, index_a, filled_a);
addValueToWindow(window_b, b, index_b, filled_b);
int32_t median_a = calculateMedian(window_a, index_a, filled_a);
int32_t median_b = calculateMedian(window_b, index_b, filled_b);
int32_t reconstructInt(uint8_t byte1, uint8_t byte2, uint8_t byte3, uint8_t byte4) {
return (int32_t(byte1) << 24) | (int32_t(byte2) << 16) | (int32_t(byte3) << 8) | int32_t(byte4);
// Initialize serial communication for debugging
uint8_t data[] = {0x00, 0x00, 0x00, 0x0A, 0x00, 0x00, 0x00, 0x14}; // Example data
// The first 4 bytes correspond to the first motor
int32_t a = reconstructInt(data[0], data[1], data[2], data[3]);
// The last 4 bytes correspond to the second motor
int32_t b = reconstructInt(data[4], data[5], data[6], data[7]);
// Print the filtered values
Serial.print("Filtered a: ");
Serial.print("Filtered b: ");
delay(1000); // Delay for readability in the serial monitor