#include <Arduino.h>
#include <Wire.h>
#include <Arduino_LSM9DS1.h>

int16_t accX, accY, accZ;
float filteredAccX;

// Define filter cutoff frequency
const float cutoffFrequency = 5.0;

// Create a low-pass filter object
DSP::LowPassFilter lowPassFilter(cutoffFrequency);

void setup() {
  Wire.begin();
  if (!IMU.begin()) {
    while (1) {
      Serial.println("Failed to find MPU6050 chip");
      delay(1000);
    }
  }
  Serial.begin(9600);
}

void loop() {
  IMU.readSensor();
  accX = IMU.getAccelX();
  accY = IMU.getAccelY();
  accZ = IMU.getAccelZ();

  // Apply the low-pass filter to the accelerometer X-axis data
  filteredAccX = lowPassFilter.process(accX);

  // Print the filtered and unfiltered data to the serial monitor
  Serial.print("Unfiltered X-axis: ");
  Serial.print(accX);
  Serial.print(" Filtered X-axis: ");
  Serial.println(filteredAccX);

  delay(100);
}