from machine import PWM,ADC,Pin,I2C
import time
import math
from imu import MPU6050

i2c = I2C(0,scl=Pin(17), sda=Pin(16),freq=40000)
mpu = MPU6050(i2c)

servo = PWM(Pin(15))
servo.freq(50)
servo.duty_u16(4900)

xJoy = ADC(27)
yJoy = ADC(26)

while True:
    #MPU6050
    #For this one we can control the servo motor based on the x axis acceleration sensed by the MPU6050 i.e -2g corresponds to 180 deg and 2g corresponds to 0 deg.
    try:
        xAccel = mpu.accel.x
        yAccel = mpu.accel.y
        zAccel = mpu.accel.z
        print('\r','x:', xAccel, ' G y: ', yAccel, ' G z: ', zAccel, ' G ',end='')
        pwm = (-0.0007407*xAccel)+7600
        servo.duty_u16(int(pwm))
        time.sleep(0.2)
    except:
        pass
        time.sleep(0.2)
    '''

    #JOYSTICK
    xVal = xJoy.read_u16() 
    yVal = yJoy.read_u16()
    xCal = (xVal*-200/65535)+100 #Reassigning the x and y values of joystick in range of -100 to 100
    yCal = (yVal*-200/65535)+100
    if xVal == 32759:
        xCal = 0
    if yCal == 32759:
        yCal = 0
    rad= math.atan2(yCal,xCal) #Calculating the angle made by the joystick
    deg = (rad/2/math.pi)*360
    if xCal ==0 and yCal == 0:
        angle = 90
    if deg<0:
        deg+=360
    if deg>180 and deg<270: #Exception cases as servo cannot rotate 360 degrees
        angle = 180
    elif deg<0 and deg>270:
        angle = 0
    else:
        angle = deg
    pwm = (-angle*30)+7600
    servo.duty_u16(int(pwm))

    print('\r',deg,end='')
    time.sleep(0.1)
    '''