Somewhat working state

MPU6050.c

@@ -3,14 +3,23 @@
 #include <util/delay.h>
 #include "MPU6050.h"
 #include "i2c.h"
+#include "uart.h"
 
 // Function to initialize MPU-6050
 void MPU6050_Init() {
+    DEBUG("Initializing MPU6050...");
     // Wake up MPU-6050 by writing to PWR_MGMT_1 register
     I2C_start(MPU6050_ADDR << 1 | TW_WRITE);
-    I2C_write(MPU6050_REG_PWR_MGMT_1);
-    I2C_write(0x00); // Clear sleep mode bit
+
+    // These two seem to be causing problems
+    // DEBUG("Sending PWR_MGMT_1 register address");
+    // I2C_write(MPU6050_REG_PWR_MGMT_1);
+    // DEBUG("Writing 0x00 to PWR_MGMT_1 register");
+    // I2C_write(0x00); // Clear sleep mode bit
+
+    DEBUG("Stopping I2C communication");
     I2C_stop();
+    DEBUG("MPU6050 Initialized!");
 }
 
 // Function to read accelerometer data from MPU-6050

i2c.c

@@ -4,6 +4,7 @@
 #include <util/delay.h>
 #include <util/twi.h>
 #include <stdint.h>
+#include "uart.h" // DEBUG macro
 
 void I2C_init() {
     // Set the prescaler to 1
@@ -17,23 +18,29 @@ uint8_t I2C_start(uint8_t addr) {
     // Send the start condition
     TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN);
 
+    DEBUG("Waiting for start condition to be sent");
     // Wait for the start condition to be sent
     while (!(TWCR & (1 << TWINT)))
         ;
 
+    DEBUG("Start condition sent");
     // Load the address of the slave device
     TWDR = addr;
 
+    DEBUG("Sending address");
     // Clear the TWINT bit to start the transmission
     TWCR = (1 << TWINT) | (1 << TWEN);
 
+    DEBUG("Waiting for address to be sent");
     // Wait for the address to be sent
     while (!(TWCR & (1 << TWINT)))
         ;
 
+    DEBUG("Address sent");
     // Get the status of the transmission
     uint8_t status = TWSR & 0xF8;
 
+    DEBUG("Checking status");
     // Return true if the slave acknowledged the address
     return (status == TW_MT_SLA_ACK || status == TW_MR_SLA_ACK);
 }

i2c.h

@@ -4,8 +4,17 @@
 #define TW_WRITE 0
 #define TW_READ 1
 
+// Function to initialize I2C
 void I2C_init();
+
+// Function to send start condition
 uint8_t I2C_start(uint8_t addr);
+
+// Function to send stop condition
 void I2C_stop();
+
+// Write a byte to I2C bus
 uint8_t I2C_write(uint8_t data);
+
+// Read a byte from I2C bus
 uint8_t I2C_read(uint8_t ack);

main.c

@@ -8,8 +8,8 @@
 #define LED_PIN PB5  // Define the pin connected to the LED
 
 #include <avr/io.h>
-#include <util/delay.h>
 #include <math.h>
+#include <util/delay.h>
 
 #include "MPU6050.h"
 #include "i2c.h"
@@ -33,23 +33,34 @@ void blink() {
 }
 
 int main(void) {
+    // int16_t accel_data[3]; // Array to store accelerometer data (X, Y, Z)
+    // int16_t gyro_data[3]; // Array to store gyroscope data (X, Y, Z)
+    int32_t iteration = 0;
+
     initUART();
-    UART_println("ATmega328P Initialized!");
+    UART_println("UART Initialized!");
+
+    DEBUG("DEBUG mode enabled!");
+
     I2C_init();
     UART_println("I2C Initialized!");
-    MPU6050_Init();
 
-    while(1) {
-        UART_println("Hello, World!");
+    MPU6050_Init();
+    UART_println("MPU6050 Initialized!");
+
+    while (1) {
+        UART_println("%d Hello, World!", iteration++);
         _delay_ms(1000);
+        // Read accelerometer data
+        // MPU6050_Read_Accel(accel_data);
+        // // Read gyroscope data
+        // MPU6050_Read_Gyro(gyro_data);
     }
 
     return 0;
 }
 
 // int main() {
-//     int16_t accel_data[3]; // Array to store accelerometer data (X, Y, Z)
-//     int16_t gyro_data[3]; // Array to store gyroscope data (X, Y, Z)
 
 //     // Initialize MPU-6050
 //     i2c_init();
@@ -62,8 +73,9 @@ int main(void) {
 //         MPU6050_Read_Gyro(gyro_data);
 
 //         // Print accelerometer and gyroscope data
-//         printf("Accelerometer (mg): X=%d, Y=%d, Z=%d\n", accel_data[0], accel_data[1], accel_data[2]);
-//         printf("Gyroscope (°/s): X=%d, Y=%d, Z=%d\n", gyro_data[0], gyro_data[1], gyro_data[2]);
+//         printf("Accelerometer (mg): X=%d, Y=%d, Z=%d\n", accel_data[0],
+//         accel_data[1], accel_data[2]); printf("Gyroscope (°/s): X=%d, Y=%d,
+//         Z=%d\n", gyro_data[0], gyro_data[1], gyro_data[2]);
 
 //         _delay_ms(1000); // Delay for 1 second
 //     }