Inputs Devices

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

#define SDA_PIN 6
#define SCL_PIN 7

MPU6050 MPU;

const int Num_Reading = 10;

float Readings_Gyr_X[Num_Reading];
float Readings_Gyr_Y[Num_Reading];
float Readings_Gyr_Z[Num_Reading];

int Index = 0; 
float Add_Gyr_X = 0.0, Add_Gyr_Y = 0.0, Add_Gyr_Z = 0.0;
float Avar_Gyr_X = 0.0, Avar_Gyr_Y = 0.0, Avar_Gyr_Z = 0.0;

const float GYRO_SCALE_FACTOR = 65.5; 

void setup() {
  Serial.begin(115200);
  Wire.begin(SDA_PIN, SCL_PIN); 

  Serial.println("Inizalizating MPU6050...");
  
  MPU.initialize();
  
  if (!MPU.testConnection()) {
    Serial.println("MPU6050 no conected");
    while (1); 
  }
  
  MPU.setFullScaleGyroRange(MPU6050_GYRO_FS_500); 

  Serial.println("Gyroscope Ready.");

  for (int i = 0; i < Num_Reading; i++) {
    Readings_Gyr_X[i] = 0.0;
    Readings_Gyr_Y[i] = 0.0;
    Readings_Gyr_Z[i] = 0.0;
  }
}

void loop() {
  int16_t gx, gy, gz;
  
  MPU.getRotation(&gx, &gy, &gz);

  float gyr_x_dps = gx / GYRO_SCALE_FACTOR;
  float gyr_y_dps = gy / GYRO_SCALE_FACTOR;
  float gyr_z_dps = gz / GYRO_SCALE_FACTOR;

  Add_Gyr_X -= Readings_Gyr_X[Index];
  Add_Gyr_Y -= Readings_Gyr_Y[Index];
  Add_Gyr_Z -= Readings_Gyr_Z[Index];

  Readings_Gyr_X[Index] = gyr_x_dps;
  Readings_Gyr_Y[Index] = gyr_y_dps;
  Readings_Gyr_Z[Index] = gyr_z_dps;

  Add_Gyr_X += Readings_Gyr_X[Index];
  Add_Gyr_Y += Readings_Gyr_Y[Index];
  Add_Gyr_Z += Readings_Gyr_Z[Index];

  Index++;
  if (Index >= Num_Reading) {
    Index = 0;
  }

  Avar_Gyr_X = Add_Gyr_X / Num_Reading;
  Avar_Gyr_Y = Add_Gyr_Y / Num_Reading;
  Avar_Gyr_Z = Add_Gyr_Z / Num_Reading;

  Serial.print("Gyroscope X: "); Serial.print(gyr_x_dps);
  Serial.print(" | Gyroscope X_Average:"); Serial.print(Avar_Gyr_X);
  Serial.print(" Gyroscope Y: "); Serial.print(gyr_y_dps);
  Serial.print(" | Gyroscope Y_Average:"); Serial.print(Avar_Gyr_Y);
  Serial.print(" Gyroscope Z: "); Serial.print(gyr_z_dps);
  Serial.print(" | Gyroscope Z_Average:"); Serial.println(Avar_Gyr_Z);

  delay(50);
}
                                

Explanation

The MPU6050 is an advanced 6-axis digital inertial measurement unit. Its internal gyroscope continuously measures angular velocity, the rate at which the sensor actively rotates around its X, Y, and Z axes in space.

Libraries and Definitions

• #include <MPU6050.h>: A library developed by Electronic Cats. It provides low-level hardware control over the sensor's registers, allowing direct access to raw data and advanced internal features.
• MPU6050 MPU;: Instantiates the main sensor object. Even though the I2C address is not explicitly defined in the code, this library is hardcoded to target the 0x68 default address, which matches my physical hardware configuration AD0 tied to GND.

Variables Configuration and Memory Allocation

• #define SDA_PIN 6 and #define SCL_PIN 7: Allows the reassignment of the default I2C communication pins for the XIAO ESP32-C3 through hardware multiplexing.
• const float GYRO_SCALE_FACTOR = 65.5: A vital mathematical constant derived from the sensor's datasheet. When the gyroscope is set to a ±500 Degrees Per Second range, the hardware outputs 65.5 raw digital points for every 1 degree of physical rotation.
• const int Num_Reading = 10: Defines the size of the memory window for the Simple Moving Average filter, balancing signal stability and responsiveness.
• float Readings_Gyr_X[...]: These arrays act as the physical memory blocks, storing the historical angular velocity data.
• float Add_Gyr_X = 0.0: Stores the running total of the buffer to avoid the heavy processing cost of mathematically adding all 10 array elements from scratch in every single loop.

Hardware Setup and Configuration

• MPU.initialize() and MPU.testConnection(): Wakes up the sensor on the I2C bus and verifies the physical connection. If the sensor is not detected, the program deliberately halts to prevent null mathematical errors.
• MPU.setFullScaleGyroRange(MPU6050_GYRO_FS_500): Configures the sensor's internal registers to measure a maximum rotation of ±500 DPS. This is crucial to ensure the hardware's output matches the software's GYRO_SCALE_FACTOR for accurate unit conversion.

Signal Processing: Circular Buffer SMA Algorithm

• Unit Conversion: The Electronic Cats library outputs raw, 16-bit integer gyroscope data (gx). Before entering the memory buffer, this raw number is divided by the scale factor (gx / GYRO_SCALE_FACTOR) to convert it into human-readable Degrees per second.
• Subtract the Old: Add_Gyr_X -= Readings_Gyr_X[Index] removes the oldest recorded value from the running total before it gets overwritten in the memory array.
• Store and Add the New: The newly converted DPS reading is saved into the current index slot (Readings_Gyr_X[Index] = gyr_x_dps) and immediately added to the running total.
• Calculate the Average: Finally, the updated running total is divided by the window size (Avar_Gyr_X = Add_Gyr_X / Num_Reading) to yield the clean, filtered rotational velocity.