Debug printing all over the place

MPU6050.c

@@ -7,31 +7,38 @@
 
 // Function to initialize MPU-6050
 void MPU6050_Init() {
-    DEBUG("Initializing MPU6050...");
+    DEBUG("MPU6050_init: Initializing...");
     // Wake up MPU-6050 by writing to PWR_MGMT_1 register
     I2C_start(MPU6050_ADDR << 1 | TW_WRITE);
 
     // These two seem to be causing problems
-    // DEBUG("Sending PWR_MGMT_1 register address");
+    DEBUG("MPU6050_init: Sending PWR_MGMT_1 register address");
-    // I2C_write(MPU6050_REG_PWR_MGMT_1);
+    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");
+    DEBUG("Writing 0x00 to PWR_MGMT_1 register");
+    I2C_write(0x00); // Clear sleep mode bit
+
+    DEBUG("MPU6050_init: Stopping...");
     I2C_stop();
-    DEBUG("MPU6050 Initialized!");
+    DEBUG("MPU6050_init: Initialized!");
 }
 
 // Function to read accelerometer data from MPU-6050
 void MPU6050_Read_Accel(int16_t* accel_data) {
     uint8_t buffer[6]; // Buffer to store accelerometer data
+
     // Read accelerometer data starting from ACCEL_XOUT_H register
     I2C_start(MPU6050_ADDR << 1 | TW_WRITE);
+    DEBUG("MPU6050_Read_Accel: Sending ACCEL_XOUT_H register address");
     I2C_write(MPU6050_REG_ACCEL_XOUT_H);
+
+    DEBUG("MPU6050_Read_Accel: Reading accelerometer data");
     I2C_start(MPU6050_ADDR << 1 | TW_READ);
     for (int i = 0; i < 6; i++) {
+        DEBUG("MPU6050_Read_Accel: Reading byte %d", i);
         buffer[i] = I2C_read(i < 5); // Read 6 bytes with ACK for all except last byte
     }
+    DEBUG("MPU6050_Read_Accel: Stopping...");
     I2C_stop();
 
     // Combine high and low bytes for each axis
@@ -42,18 +49,5 @@ void MPU6050_Read_Accel(int16_t* accel_data) {
 
 // Function to read gyroscope data from MPU-6050
 void MPU6050_Read_Gyro(int16_t* gyro_data) {
-    uint8_t buffer[6]; // Buffer to store gyroscope data
+    // TODO: Implement this function
-    // Read gyroscope data starting from GYRO_XOUT_H register
-    I2C_start(MPU6050_ADDR << 1 | TW_WRITE);
-    I2C_write(MPU6050_REG_GYRO_XOUT_H);
-    I2C_start(MPU6050_ADDR << 1 | TW_READ);
-    for (int i = 0; i < 6; i++) {
-        buffer[i] = I2C_read(i < 5); // Read 6 bytes with ACK for all except last byte
-    }
-    I2C_stop();
-
-    // Combine high and low bytes for each axis
-    gyro_data[0] = (buffer[0] << 8) | buffer[1]; // X-axis
-    gyro_data[1] = (buffer[2] << 8) | buffer[3]; // Y-axis
-    gyro_data[2] = (buffer[4] << 8) | buffer[5]; // Z-axis
 }

i2c.c

@@ -6,72 +6,95 @@
 #include <stdint.h>
 #include "uart.h" // DEBUG macro
 
-void I2C_init() {
+void I2C_init(uint32_t SCL_CLOCK) {
+    DEBUG("I2C: Initializing...");
+
     // Set the prescaler to 1
     TWSR &= ~(1 << TWPS0);
     TWSR &= ~(1 << TWPS1);
+
     // Set the bit rate to 100kHz
-    TWBR = ((F_CPU / 100000) - 16) / 2;
+    TWBR = ((F_CPU / SCL_CLOCK) - 16) / 2;
+
+    // Enable the TWI module
+    TWCR |= (1 << TWEN);
+
+    DEBUG("I2C: Initialized!");
 }
 
 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");
+    DEBUG("Start: Waiting for start condition to be sent");
     // Wait for the start condition to be sent
     while (!(TWCR & (1 << TWINT)))
         ;
 
-    DEBUG("Start condition sent");
+    DEBUG("Start: Start condition sent");
     // Load the address of the slave device
     TWDR = addr;
 
-    DEBUG("Sending address");
+    DEBUG("Start: Sending address");
     // Clear the TWINT bit to start the transmission
     TWCR = (1 << TWINT) | (1 << TWEN);
 
-    DEBUG("Waiting for address to be sent");
+    DEBUG("Start: Waiting for address to be sent");
     // Wait for the address to be sent
     while (!(TWCR & (1 << TWINT)))
         ;
 
-    DEBUG("Address sent");
+    DEBUG("Start: Address sent");
     // Get the status of the transmission
     uint8_t status = TWSR & 0xF8;
 
-    DEBUG("Checking status");
+    DEBUG("Start: Checking status");
     // Return true if the slave acknowledged the address
     return (status == TW_MT_SLA_ACK || status == TW_MR_SLA_ACK);
 }
 
 void I2C_stop() {
     // Send the stop condition
+    DEBUG("Stop: Sending stop condition");
     TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN);
+
     // Wait for the stop condition to be sent
+    DEBUG("Stop: Waiting for stop condition to be sent");
     while (TWCR & (1 << TWSTO))
         ;
+
+    DEBUG("Stop: Stop condition sent");
 }
 
 uint8_t I2C_write(uint8_t data) {
     // Load the data into the data register
     TWDR = data;
+
     // Start transmission of data
+    DEBUG("Write: Sending data");
     TWCR = (1 << TWINT) | (1 << TWEN);
+
     // Wait for the data to be sent
+    DEBUG("Write: Waiting for data to be sent");
     while (!(TWCR & (1 << TWINT)))
         ;
 
     // Return true if the data was sent
+    DEBUG("Write: Data transmission complete");
     return (TWSR & 0xF8) == TW_MT_DATA_ACK;
 }
 
 uint8_t I2C_read(uint8_t ack) {
+    DEBUG("Read: Waiting for data to be received");
     // Enable TWI, generate ACK (if ack = 1) and clear TWI interrupt flag
     TWCR = (1 << TWINT) | (1 << TWEN) | (ack << TWEA);
+
     // Wait until TWI finish its current job (read operation)
+    DEBUG("Read: Waiting for TWI to finish current job");
     while (!(TWCR & (1 << TWINT)))
         ;
+
+    DEBUG("Read: Data received");
     // Return received data
     return TWDR;
 }

i2c.h

@@ -5,7 +5,7 @@
 #define TW_READ 1
 
 // Function to initialize I2C
-void I2C_init();
+void I2C_init(uint32_t SCL_CLOCK);
 
 // Function to send start condition
 uint8_t I2C_start(uint8_t addr);

main.c

@@ -33,8 +33,7 @@ void blink() {
 }
 
 int main(void) {
-    // int16_t accel_data[3]; // Array to store accelerometer data (X, Y, Z)
+    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();
@@ -42,7 +41,7 @@ int main(void) {
 
     DEBUG("DEBUG mode enabled!");
 
-    I2C_init();
+    I2C_init(100000);
     UART_println("I2C Initialized!");
 
     MPU6050_Init();
@@ -50,35 +49,14 @@ int main(void) {
 
     while (1) {
         UART_println("%d Hello, World!", iteration++);
-        _delay_ms(1000);
+
         // Read accelerometer data
-        // MPU6050_Read_Accel(accel_data);
+        UART_println("Reading MPU6050 accelerometer data...");
-        // // Read gyroscope data
+        MPU6050_Read_Accel(accel_data);
-        // MPU6050_Read_Gyro(gyro_data);
+
+        UART_println("Accelerometer (mg): X=%d, Y=%d, Z=%d", accel_data[0], accel_data[1], accel_data[2]);
+        _delay_ms(1000);
     }
 
     return 0;
 }
-
-// int main() {
-
-//     // Initialize MPU-6050
-//     i2c_init();
-//     MPU6050_Init();
-
-//     while (1) {
-//         // Read accelerometer data
-//         MPU6050_Read_Accel(accel_data);
-//         // Read gyroscope data
-//         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]);
-
-//         _delay_ms(1000); // Delay for 1 second
-//     }
-
-//     return 0;
-// }