Sensor Display rev_01

/********* Pleasedontcode.com **********

    Pleasedontcode thanks you for automatic code generation! Enjoy your code!

    - Terms and Conditions:
    You have a non-exclusive, revocable, worldwide, royalty-free license
    for personal and commercial use. Attribution is optional; modifications
    are allowed, but you're responsible for code maintenance. We're not
    liable for any loss or damage. For full terms,
    please visit pleasedontcode.com/termsandconditions.

    - Project: Sensor Display
    - Source Code NOT compiled for: Arduino Nano
    - Source Code created on: 2024-11-02 12:32:03

********* Pleasedontcode.com **********/

/****** SYSTEM REQUIREMENTS *****/
/****** SYSTEM REQUIREMENT 1 *****/
    /* 读取MPU6050信息，并做好信息好滤波，并打印到串口上。 */
/****** END SYSTEM REQUIREMENTS *****/

/****** DEFINITION OF LIBRARIES *****/
#include <Wire.h>
#include <MPU6050.h>    // https://github.com/electroniccats/mpu6050

/****** FUNCTION PROTOTYPES *****/
void setup(void);
void loop(void);

/***** DEFINITION OF I2C PINS *****/
const uint8_t wang_MPU6050_I2C_PIN_SDA_A4       = A4;
const uint8_t wang_MPU6050_I2C_PIN_SCL_A5       = A5;
const uint8_t wang_MPU6050_I2C_SLAVE_ADDRESS        = 104;

/****** DEFINITION OF LIBRARIES CLASS INSTANCES*****/
// Create an instance of the MPU6050 class
MPU6050 mpu; // Initialize the MPU6050 object

// Variables to hold accelerometer and gyroscope data
int16_t ax, ay, az; // Accelerometer data
int16_t gx, gy, gz; // Gyroscope data

// Variables for filtering
const int filterSize = 5; // Size of the filter
int16_t axFilter[filterSize] = {0}; // Filter array for accelerometer x
int16_t ayFilter[filterSize] = {0}; // Filter array for accelerometer y
int16_t azFilter[filterSize] = {0}; // Filter array for accelerometer z
int16_t gxFilter[filterSize] = {0}; // Filter array for gyroscope x
int16_t gyFilter[filterSize] = {0}; // Filter array for gyroscope y
int16_t gzFilter[filterSize] = {0}; // Filter array for gyroscope z
int filterIndex = 0; // Current index for the filter arrays

void setup(void)
{
    // Initialize I2C communication
    Wire.begin();
    Serial.begin(115200); // Start serial communication at 115200 baud rate

    Serial.println("Initializing I2C devices...");
    mpu.initialize(); // Initialize the MPU6050

    // Check if the MPU6050 is connected
    if (mpu.testConnection()) {
        Serial.println("MPU6050 connection successful");
    } else {
        Serial.println("MPU6050 connection failed");
    }

    pinMode(LED_BUILTIN, OUTPUT); // Set the built-in LED pin as output
}

void loop(void)
{
    // Read accelerometer and gyroscope data
    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    // Update the filter arrays
    axFilter[filterIndex] = ax;
    ayFilter[filterIndex] = ay;
    azFilter[filterIndex] = az;
    gxFilter[filterIndex] = gx;
    gyFilter[filterIndex] = gy;
    gzFilter[filterIndex] = gz;

    // Calculate the average for each axis
    int16_t axFiltered = 0, ayFiltered = 0, azFiltered = 0;
    int16_t gxFiltered = 0, gyFiltered = 0, gzFiltered = 0;

    for (int i = 0; i < filterSize; i++) {
        axFiltered += axFilter[i]; // Sum accelerometer x values
        ayFiltered += ayFilter[i]; // Sum accelerometer y values
        azFiltered += azFilter[i]; // Sum accelerometer z values
        gxFiltered += gxFilter[i]; // Sum gyroscope x values
        gyFiltered += gyFilter[i]; // Sum gyroscope y values
        gzFiltered += gzFilter[i]; // Sum gyroscope z values
    }

    axFiltered /= filterSize; // Average for accelerometer x
    ayFiltered /= filterSize; // Average for accelerometer y
    azFiltered /= filterSize; // Average for accelerometer z
    gxFiltered /= filterSize; // Average for gyroscope x
    gyFiltered /= filterSize; // Average for gyroscope y
    gzFiltered /= filterSize; // Average for gyroscope z

    // Print the filtered accelerometer and gyroscope data to the serial monitor
    Serial.print("Filtered Accel: ");
    Serial.print(axFiltered); Serial.print(", ");
    Serial.print(ayFiltered); Serial.print(", ");
    Serial.print(azFiltered); Serial.print(" | ");
    Serial.print("Filtered Gyro: ");
    Serial.print(gxFiltered); Serial.print(", ");
    Serial.print(gyFiltered); Serial.print(", ");
    Serial.println(gzFiltered);

    // Blink the built-in LED to indicate activity
    digitalWrite(LED_BUILTIN, HIGH); // Turn the LED on
    delay(100); // Wait for 100 milliseconds
    digitalWrite(LED_BUILTIN, LOW); // Turn the LED off
    delay(100); // Wait for 100 milliseconds

    // Increment the filter index and wrap around if necessary
    filterIndex = (filterIndex + 1) % filterSize; // Update filter index
}

/* END CODE */