/********* 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 Data**
	- Source Code NOT compiled for: ESP32 DevKit V1
	- Source Code created on: 2024-11-07 23:14:57

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

/****** SYSTEM REQUIREMENTS *****/
/****** SYSTEM REQUIREMENT 1 *****/
	/* read angle and trun on led while recive reading */
/****** END SYSTEM REQUIREMENTS *****/

/* START CODE */

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

/****** FUNCTION PROTOTYPES *****/
void setup(void);
void loop(void);
void dmpDataReady(); // Added prototype for interrupt function
void readAngles();    // Added prototype for reading angles

/****** DEFINITION OF LIBRARIES CLASS INSTANCES*****/
// class default I2C address is 0x68
MPU6050 mpu;
//MPU6050 mpu(0x69); // <-- use for AD0 high (commented out because we are using the default address)

#define LED_PIN 2 // (Arduino is 11, Teensy is 11, Teensy++ is 6)
bool blinkState = false;

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

// packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };

unsigned long currentMillis;

long previousMillis = 0;    // set up timers
long interval = 20;        // time constant for timers
unsigned long count;

int IMUdataReady = 0;

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high

// ****************** SETUP ******************************

void setup() {
    // join I2C bus (I2Cdev library doesn't do this automatically)
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif

    // initialize serial communication
    Serial.begin(115200);

    // initialize device
    mpu.initialize();
    devStatus = mpu.dmpInitialize();

    // supply your own gyro offsets here, scaled for min sensitivity
    mpu.setXGyroOffset(113);
    mpu.setYGyroOffset(-22);
    mpu.setZGyroOffset(10);
    mpu.setXAccelOffset(-1315);
    mpu.setYAccelOffset(-544);
    mpu.setZAccelOffset(1119);

    // make sure it worked (returns 0 if so)
    if (devStatus == 0) {
        // turn on the DMP, now that it's ready
        mpu.setDMPEnabled(true);

        // enable Arduino interrupt detection
        attachInterrupt(23, dmpDataReady, RISING); // Ensure pin 23 is available on ESP32
        mpuIntStatus = mpu.getIntStatus();

        // get expected DMP packet size for later comparison
        packetSize = mpu.dmpGetFIFOPacketSize();
    } else {
        // ERROR!
        Serial.print(F("DMP Initialization failed (code "));
        Serial.print(devStatus);
        Serial.println(F(")"));
    }

    // configure LED for output
    pinMode(LED_PIN, OUTPUT);
}

// ********************* MAIN LOOP *******************************

void loop() {  
    currentMillis = millis();
    if (currentMillis - previousMillis >= interval) {  // start timed event
        previousMillis = currentMillis;

        if (IMUdataReady == 1) {
            readAngles(); // Ensure this function is defined
        }

        Serial.print(ypr[1] * 180/M_PI); // Print pitch angle
        Serial.print('\n');

        // Turn on LED while receiving data
        digitalWrite(LED_PIN, HIGH);
    } else {
        // Turn off LED when not receiving data
        digitalWrite(LED_PIN, LOW);
    } // end of timed event          
} // end of main loop

// Function to read angles from the MPU6050
void readAngles() {
    // Get the quaternion values
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    // Get the Euler angles
    mpu.dmpGetEuler(euler, &q);
    // Convert to yaw, pitch, roll
    ypr[0] = euler[0]; // yaw
    ypr[1] = euler[1]; // pitch
    ypr[2] = euler[2]; // roll
    IMUdataReady = 0; // Reset data ready flag
}

// Interrupt function to signal data ready
void dmpDataReady() {
    IMUdataReady = 1; // Set data ready flag
}

/* END CODE */