From c43fae4e45a3a42c1f2c0e43c19dc6374d0d514b Mon Sep 17 00:00:00 2001
From: Imbus <>
Date: Sun, 24 Mar 2024 02:35:05 +0100
Subject: [PATCH] DEBUG printing, comments

---
 MPU6050.c | 13 +++++++++++--
 1 file changed, 11 insertions(+), 2 deletions(-)

diff --git a/MPU6050.c b/MPU6050.c
index 930f9c2..5213df0 100644
--- a/MPU6050.c
+++ b/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