import time 
from machine import Pin, I2C

def reg_write(i2c, addr, reg, data):
    msg = bytearray()
    msg.append(data)
    i2c.writeto_mem(addr, reg, msg)
    
def reg_read(i2c, addr, reg, nbytes=1):
    if nbytes < 1:
        return bytearray()
    data = i2c.readfrom_mem(addr, reg, nbytes)
    return data

def accel_read(reg):
    x = reg_read(i2c, addr, reg, 2)
    y = (x[0] << 8) + x[1]
    if(y > 0x8000):
        y = y - 0x10000
    y = y / 0x8000
    return(y)

i2c = machine.I2C(0, scl=machine.Pin(1), sda=machine.Pin(0))


# Print out any addresses found
devices = i2c.scan()
if devices:
    for d in devices:
        print('I2C Device Found:',hex(d))

addr = devices[0]
print('Communicating with ', hex(addr))

# set bandwidth
reg_write(i2c, 0x68, 0x1a, 6)
# set range
reg_write(i2c, 0x68, 0x1c, 0x00)
RANGE = 2
# set clock freq
reg_write(i2c, 0x68, 0x6b, 0)

   
if(1):
    for i in range(0,100):
        x = accel_read(0x3b) * RANGE
        y = accel_read(0x3d) * RANGE
        z = accel_read(0x3f) * RANGE
 
        mx = 'x" = ' + str('{: 5.3f}'.format(x))
        my = '    y" = ' + str('{: 5.3f}'.format(y))
        mz = '    z" = ' + str('{: 5.3f}'.format(z))

        print(x, y, z)
        #print(mx + my + mz, "   ",end="\r")
        time.sleep(0.1)