from machine import I2C, Pin
import ssd1306
import time
from mpu6050 import accel

screen = {}
screen['rst'] = Pin(16, Pin.OUT)
screen['rst'].value(1)
screen['scl'] = Pin(15, Pin.OUT, Pin.PULL_UP)
screen['sda'] = Pin(4, Pin.OUT, Pin.PULL_UP)
screen['i2c'] = I2C(scl=screen['scl'], sda=screen['sda'], freq=450000)
oled = ssd1306.SSD1306_I2C(128, 64, screen['i2c'], addr=0x3c)

oled.fill(0)
oled.text('Initializing',5,5)
oled.show()

mpu = {}
mpu['scl'] = Pin(19, Pin.OUT, Pin.PULL_UP)
mpu['sda'] = Pin(23, Pin.OUT, Pin.PULL_UP)
mpu['i2c'] = I2C(scl=mpu['scl'], sda=mpu['sda'])
accelerometer = accel(mpu['i2c'])

def output_positions(arg):
    oled.fill(0)
    oled.text('X: %(AcX)+05d' % arg, 5, 5)
    oled.text('Y: %(AcY)+05d' % arg, 5, 15)
    oled.text('Z: %(AcZ)+05d' % arg, 5, 25)
    oled.show()

for i in range(0,240):
    output_positions(accelerometer.get_values())
    time.sleep_ms(250)