from machine import Pin, I2C
import time

class MPU6050():
    def __init__(self, addr=0x68):
        self.iic = I2C(1, scl=Pin(2), sda=Pin(15), freq=400000)
        self.addr = addr

    def get_raw_values(self):
        a = self.iic.readfrom_mem(self.addr, 0x3B, 14)
        return a

    def get_ints(self):
        b = self.get_raw_values()
        c = []
        for i in b:
            c.append(i)
        return c

    def bytes_toint(self, firstbyte, secondbyte):
        if not firstbyte & 0x80:
            return firstbyte << 8 | secondbyte
        return - (((firstbyte ^ 255) << 8) | (secondbyte ^ 255) + 1)

    def get_values(self):
        raw_ints = self.get_raw_values()
        vals = {}
        vals["AcX"] = (self.bytes_toint(raw_ints[0], raw_ints[1]))/(32767/2)
        vals["AcY"] = (self.bytes_toint(raw_ints[2], raw_ints[3]))/(32767/2)
        vals["AcZ"] = (self.bytes_toint(raw_ints[4], raw_ints[5]))/(32767/2)
        vals["Tmp"] = self.bytes_toint(raw_ints[6], raw_ints[7]) / 340.00 + 36.53
        vals["GyX"] = (self.bytes_toint(raw_ints[8], raw_ints[9]))/655*5
        vals["GyY"] = (self.bytes_toint(raw_ints[10], raw_ints[11]))/655*5
        vals["GyZ"] = (self.bytes_toint(raw_ints[12], raw_ints[13]))/655*5
        return vals  # returned in range of Int16

if __name__=="__main__":
    print("hello")
    mpu6050 = MPU6050()
    while True:
        print(mpu6050.get_values())
        time.sleep(1)