From fd45ec264ab59022d70b00e8ec7a73d1d0b103c3 Mon Sep 17 00:00:00 2001
From: Imbus <>
Date: Sun, 24 Mar 2024 01:23:44 +0100
Subject: [PATCH] Attempt at making MPU6050 work via i2c

---
 MPU6050.c | 51 ++++++++++++++++++++++++++++++++++++++++++++++++++-
 MPU6050.h | 21 ++++++++++++++++++++-
 i2c.h     |  3 +++
 main.c    | 28 ++++++++++++++++++++++++++++
 makefile  |  4 ++++
 5 files changed, 105 insertions(+), 2 deletions(-)

diff --git a/MPU6050.c b/MPU6050.c
index 54625b7..930f9c2 100644
--- a/MPU6050.c
+++ b/MPU6050.c
@@ -1 +1,50 @@
-#include "MPU6050.h"
\ No newline at end of file
+#include <stdint.h>
+#include <avr/io.h>
+#include <util/delay.h>
+#include "MPU6050.h"
+#include "i2c.h"
+
+// Function to initialize MPU-6050
+void MPU6050_Init() {
+    // 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
+    I2C_stop();
+}
+
+// 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);
+    I2C_write(MPU6050_REG_ACCEL_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
+    accel_data[0] = (buffer[0] << 8) | buffer[1]; // X-axis
+    accel_data[1] = (buffer[2] << 8) | buffer[3]; // Y-axis
+    accel_data[2] = (buffer[4] << 8) | buffer[5]; // Z-axis
+}
+
+// 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
+    // 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
+}
diff --git a/MPU6050.h b/MPU6050.h
index 0285381..9ba54f9 100644
--- a/MPU6050.h
+++ b/MPU6050.h
@@ -1,3 +1,22 @@
 #pragma once
 
-#include <stdint.h>
\ No newline at end of file
+// I2c address of MPU6050
+#define MPU6050_ADDR 0x68 // With AD0 pin grounded/floating, otherwise 0x69
+
+// MPU-6050 register addresses
+#define MPU6050_REG_ACCEL_XOUT_H 0x3B
+#define MPU6050_REG_GYRO_XOUT_H 0x43
+#define MPU6050_REG_PWR_MGMT_1 0x6B
+
+#include <stdint.h>
+#include <avr/io.h>
+#include <util/delay.h>
+
+// Function to initialize MPU-6050
+void MPU6050_Init();
+
+// Function to read accelerometer data from MPU-6050
+void MPU6050_Read_Accel(int16_t* accel_data);
+
+// Function to read gyroscope data from MPU-6050
+void MPU6050_Read_Gyro(int16_t* gyro_data);
diff --git a/i2c.h b/i2c.h
index 5749df3..ed984e9 100644
--- a/i2c.h
+++ b/i2c.h
@@ -1,6 +1,9 @@
 #pragma once
 #include <stdint.h>
 
+#define TW_WRITE 0
+#define TW_READ 1
+
 void I2C_init();
 uint8_t I2C_start(uint8_t addr);
 void I2C_stop();
diff --git a/main.c b/main.c
index 747a162..7de830c 100644
--- a/main.c
+++ b/main.c
@@ -34,6 +34,10 @@ void blink() {
 
 int main(void) {
     initUART();
+    UART_println("ATmega328P Initialized!");
+    I2C_init();
+    UART_println("I2C Initialized!");
+    MPU6050_Init();
 
     while(1) {
         UART_println("Hello, World!");
@@ -42,3 +46,27 @@ int main(void) {
 
     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();
+//     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;
+// }
\ No newline at end of file
diff --git a/makefile b/makefile
index efd39fb..aa20177 100644
--- a/makefile
+++ b/makefile
@@ -64,3 +64,7 @@ clean:
 
 size: $(TARGET).hex
 	avr-size --mcu=atmega328p $(TARGET).hex
+
+# Reset target
+reset:
+	avrdude -p $(MCU) -c $(PROGRAMMER) -E reset
\ No newline at end of file