/*
  Forum: https://forum.arduino.cc/t/smoothing-spurious-mpu6050-results/1337284
  Wokwi: https://wokwi.com/projects/418886116138416129

  2025/01/01
  ec2021

*/

#include <MPU6050_tockn.h>
#include <Wire.h>

MPU6050 mpu(Wire);

const int minAngleX {-20};
const int maxAngleX { 20};
const int minAngleY {-20};
const int maxAngleY { 20};
const int maxCount {50};
const unsigned long interval {5};

int angleX;
int angleY;
int lastAngleX = minAngleX - 1;
int lastAngleY = minAngleY - 1;
long angleXsum;
long angleYsum;
int count = 0;
unsigned long lastMeasurement = 0;


void setup(void) {
  Serial.begin(115200);
  mpu.begin();
  mpu.calcGyroOffsets();
}

void loop() {
  getGYRO();
}

void getGYRO() {
  if (count < maxCount && millis() - lastMeasurement >= interval) {
    lastMeasurement = millis();
    mpu.update();
    angleXsum = angleXsum + mpu.getAngleX();
    angleYsum = angleYsum + mpu.getAngleY();
    count++;
    if (count >= maxCount) {
      angleX = angleXsum / maxCount;
      angleY = angleYsum / maxCount;
      angleX = constrain(angleX, minAngleX, maxAngleX);
      angleY = constrain(angleY, minAngleY, maxAngleY);
      angleXsum = 0;
      angleYsum = 0;
      count     = 0;
      printData();
    }
  }
}

void printData() {
  if (angleX != lastAngleX || angleY != lastAngleY) {
    Serial.print("X Sum: \t"); Serial.print(angleX);
    Serial.print("\tY Sum: "); Serial.println(angleY);
    lastAngleX = angleX;
    lastAngleY = angleY;
  }
}