import machine
import time
import MPU6050
from machine import I2C
from ssd1306 import SSD1306_I2C

# Set up the I2C interface for the MPU6050
i2c_mpu = I2C(1, sda=machine.Pin(14), scl=machine.Pin(15))

# Set up the MPU6050 class + wake up the sensor
mpu = MPU6050.MPU6050(i2c_mpu)
mpu.wake()

# Set up I2C interface for the OLED display
i2c_oled = I2C(0, sda=machine.Pin(0), scl=machine.Pin(1))
oled = SSD1306_I2C(128, 64, i2c_oled)

# Continuously print the data on the OLED display
while True:
  gyro = mpu.read_gyro_data()
  accel = mpu.read_accel_data()
  temp = mpu.read_temperature()

  # Extract x, y, and z axis values from gyro and accel data
  gyro_x, gyro_y, gyro_z = gyro
  accel_x, accel_y, accel_z = accel

  # Read temperature from sensor
  temp = temp

  # Clear the display
  oled.fill(0)

  # Print gyroscope and accelerometer data on the OLED display
  oled.text("Gyro X: " + str(gyro_x), 0, 0)
  oled.text("Gyro Y: " + str(gyro_y), 0, 9)
  oled.text("Gyro Z: " + str(gyro_z), 0, 18)

  oled.text("Accel X: " + str(accel_x), 0, 27)
  oled.text("Accel Y: " + str(accel_y), 0, 36)
  oled.text("Accel Z: " + str(accel_z), 0, 45)

  oled.text("Temp: " + str(temp), 0, 57)

  oled.show()

  time.sleep(0.1)