from machine import Pin, I2C, sleep, PWM
import mpu6050
import time
from math import atan,sqrt,pow,pi
from oled import I2C as oled_i2c
from a4988 import A4988

i2c = I2C(scl = Pin(22), sda = Pin(21))
mpu = mpu6050.accel(i2c)
oled = oled_i2c(128, 64, i2c)

buzzer = PWM(Pin(12))
buzzer.freq(300)

def print_oled(string):
    oled.clear()

    
while True:
    values = (mpu.get_values())
    oled.clear()

    gyz = values["GyZ"] / 131
    gyy = values["GyY"] / 131
    gyx = values["GyX"] / 131
    acz = values["AcZ"] / 16384
    acy = values["AcY"] / 16384
    acx = values["AcX"] / 16384
    # apply trigonometry to get the pitch and roll:
    pitch = atan(acx/sqrt(pow(acy,2) + pow(acz,2)));
    roll = atan(acy/sqrt(pow(acx,2) + pow(acz,2)));
    # convert radians into degrees
    pitch = pitch * (180.0/pi)
    roll = roll * (180.0/pi)
    print('Pitch',pitch)
    print('Roll',roll)
    if pitch > 20 :
        oled.text('Leaning Forward', 3, 2)
        buzzer.duty_u16(60000)
    elif pitch < -20 : 
        oled.text('Leaning Backward',3,2)
        buzzer.duty_u16(60000)
    else:
        oled.text('Not Leaning',3,2)
        buzzer.duty_u16(0)

    if roll > 20 :
        oled.text('Rolling left',3,3)
        buzzer.duty_u16(60000)
    elif roll < -20 :
        oled.text('Rolling Right',3,3)
        buzzer.duty_u16(60000)
    else:
        oled.text("Upright",3,3)
        buzzer.duty_u16(0)


    oled.show()
    time.sleep(0.02)