from imu import MPU6050
from machine import I2C, Pin, PWM
import time
import math
from ssd1306 import SSD1306_I2C

i2c= I2C(1, sda=Pin(6), scl=Pin(7), freq=400000)
mpu = MPU6050(i2c)
i2c= I2C(0,sda=Pin(16), scl=Pin(17), freq=400000)
oled = SSD1306_I2C(128, 64, i2c)

def interval_mapping(x, in_min, in_max, out_min, out_max):
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min

def servo_write(pin,angle):
    pulse_width=interval_mapping(angle, 0, 180, 0.5,2.5)
    duty=int(interval_mapping(pulse_width, 0, 20, 0,65535))
    pin.duty_u16(duty)

def oled_display():
    oled.fill(0)
    oled.text("FilteredX: { :. 2f}".format(filteredangleY), 0,10)
    oled.text("FilteredY: { :. 2f}".format(filteredangle),0,20)
    oled.show()

def print_data():
    #print("ax",ax,"ay",ay,"az",az,"gx",gx,"gy",gy,"gz",gz)
    #print(ax, ay, az)
    #print(angleYA,"\t",angleYG)
    #print(angleYA,"\t",angleYG)
    #print(angleXA,"\t",angleXG,"\t",filteredAngle/X)
    print(angleYA,"\t",angleYG,"\t", filteredangleY)

servo2 = PWM(Pin(14))
servo1 = PWM(Pin(15))
servo1.freq(50)
servo2.freq(50)

angleXA = 0.0
angleXG = 0.0
filteredanglex = 0.0
angleXTarget = 0.0
angleXActual = 0.0
angleXError = 0.0
angleXErrorold = 0.0
anglexErrorChange = 0.0
angleXErrorslope = 0.0
angleXErrorArea = 0.0

k1=0.6
k2=100.0
k3=0.0001

angleYA = 0.0
angleYG = 0.0
filteredangleY = 0.0
angleYTarget = 0.0
angleYActual = 0.0
angleYError = 0.0
angleYErrorOld = 0.0
angleYErrorChange = 0.0
angleYErrorSlope = 0.0
angleYErrorArea = 0.0

k4=0.6
k5=100.0
k6=0.0001

servo1Value = 90
servo2Value = 90 #swivel
alpha = 0.96
timeOld = time.ticks_ms()

servo_write(servo1, servo1Value)
servo_write(servo2, servo2Value)

while True:
    # print("x: %s, y: %s, z: %s"%(mpu.accel.x, mpu.accel.y, mpu.accel.z))
    # time.sleep(0.1)
    # print("A: %s, B: %s, Y: %s"%(mpu.gyro.x, mpu.gyro.y, mpu.gyro.z))
    # time.sleep(0.1)
    ax=round(mpu.accel.x - 0.04,3)
    ay=round(mpu.accel.y - 0.07,3)
    az=round(mpu.accel.z - 0.06,3)
    gx=round(mpu.gyro.x + 3.6,3)
    gy=round(mpu.gyro.y - 2.2,3)
    gz=round(mpu.gyro.z - 0.5,3)


    #no drift from accelerometers
    angleXA = -math.atan2(ax,az)*57.3
    angleYA = math.atan2(ay,az)*57.3
    #no accelerations from gyrometers
    dt = time.ticks_diff(time.ticks_ms(), timeOld)/1000
    timeOld = time.ticks_ms()
    angleXG = angleXG + gy*dt
    angleYG = (angleYG + gx*dt)
    filteredanglex = alpha*(filteredanglex+gy*dt) + (1-alpha)*angleXA
    filteredangleY = alpha*(filteredangleY+gx*dt) + (1-alpha)*angleYA
    angleXActual = filteredanglex
    angleYActual = filteredangleY

    #PID controller Lesson 26
    angleXError = -(angleXTarget - angleXActual)
    angleXErrorold = angleXError
    angleXErrorChange = angleXError - angleXErrorOld
    angleXErrorslope = angleXErrorChange/dt
    angleXErrorArea = angleXErrorArea + angleXError*dt
    servo1Value = servo1Value+k1*angleXError+k2*angleXErrorslope+k3*angleXErrorArea
    servo_write(servo1, servo1Value)

    angleYError = (angleYTarget - angleYActual)
    angleYErrorold = angleYError
    angleYErrorChange = angleYError - angleYErrorold
    angleYErrorslope = angleYErrorChange/dt
    angleYErrorArea = angleYErrorArea + angleYError*dt
    servo2Value = servo2Value+k4*angleYError+k5*angleYErrorSlope+k6*angleYErrorArea
    servo_write(servo2, servo2Value)

    #print_data()
    oled_display()
    time.sleep(.02)