import pyb
import mpu6050
import move
import time
import oled_938

gary = move.Robot()
imu = mpu6050.MPU6050(1, False)
gary.changeSpeed(30)

pot = pyb.ADC(Pin('X11'))

oled = oled_938.OLED_938(pinout={'sda': 'Y10', 'scl': 'Y9', 'res': 'Y8'}, height=64, external_vcc=False, i2c_devid=61)
oled.poweron()
oled.init_display()

K_p = 5
K_i = 0.5
K_d = 1







data = ""

def pitch_estimate(pitch, dt, alpha):
	theta = imu.pitch()
	pitch_dot = imu.get_gy()
	pitch = alpha * (pitch + pitch_dot * dt) + (1 - alpha) * theta
	return (pitch, pitch_dot)

def PID(pitch, pitch_dot, target, pError, pError_old, pError_sum, i, data):
	i = i + 1
	pError = pitch - target
	if 1 < abs(pitch) < 70:
		pError_sum += 0.005*pError
		differential = imu.get_gy()
		speed = K_p * pError - K_d * differential + K_i * pError_sum
		pError_old = pError
	else:
		speed = 0

	return speed, pError, pError_old, pError_sum, i, data

pitch = imu.pitch()
pError = 0
pError_old = 0
pError_sum = 0
tic = pyb.millis()
cf = 0
i = 0
trigger = pyb.Switch()

scale_kp = 5
scale = 2


oled.draw_text(0, 30, 'Press usr to begin')
oled.display()

while not trigger(): pass
while trigger(): pass

oled.clear()
oled.draw_text(0, 10, 'Kp = {:5.2f}'.format(K_p))
oled.draw_text(0, 30, 'Ki = {:5.2f}'.format(K_i))
oled.draw_text(0, 50, 'Kd = {:5.2f}'.format(K_d))
oled.display()

while True:
	if pyb.millis() - tic > 5:

		pitch, pitch_dot = pitch_estimate(pitch, (pyb.millis() - tic)*0.001, 0.75)
		speed, pError, pError_old, pError_sum, i, data = PID(pitch, pitch_dot, 0, pError, pError_old, pError_sum, i, data)

		print("Speed: {0}".format(speed))

		if speed > 100:
			speed = 100
		elif speed < -100:
			speed = -100

		gary.changeSpeed(abs(speed))
		if speed > 0:
			gary.moveBackwards()
		else:
			gary.moveForwards()
		tic = pyb.millis()