'''
Uses Car_lib
Gyro info from GyroSensor class = no need for Global variables
'''
import machine
from time import sleep_ms, ticks_ms
import pca9685
import motor
from MPU6050 import MPU6050
from Car_lib import accel_calibration, motor_move, GyroSensor

# set up i2c and pca9685
sdaPIN=machine.Pin(13)
sclPIN=machine.Pin(14)
i2c=machine.I2C(0,sda=sdaPIN, scl=sclPIN)

# Initialize the PCA9685 using the default address (0x40).
pwm = pca9685.PCA9685(address=0x5f, i2c = i2c)
# Set the PWM frequency to 60Hz, good for servos.
pwm.freq(60)

# initialize motors
motors = motor.DCMotors(address=0x5f, i2c = i2c)

# initial accelerometer and gyro
mpu = MPU6050()

#  ------------   main program   -------------------
# delay to unplug car
sleep_ms(1000)

# find calibration values
axc, ayc, axsd, aysd, gzc = accel_calibration(mpu)
gyro_sensor = GyroSensor(mpu, gzc)

# scale the motor speed
motor_speed = 2000   # not used
scale_speed = 0.6

# set bias for  turn (the car usually overshoots the desired angle)
angle_bias = 15

def car_rotate(gyro_sensor, mode, angle, angle_bias=0):
    angZ = gyro_sensor.get_gyro(reset=True)
    if mode == 'left':
        while True:
            angZ = gyro_sensor.get_gyro()
#             if angZ > angle - angle_bias:     # sensor mounted upright
            if angZ < -angle + angle_bias:      # sensor upside down
                break
    elif mode == 'right':
         while True:
            angZ = gyro_sensor.get_gyro()
#             if angZ < -angle + angle_bias:    # sensor mounted upright
            if angZ > angle - angle_bias:      # sensor upside down
                break

# start manuvers
# move forward
motor_move(motors, m1_speed=2000, m2_speed=2000, m3_speed=2000, m4_speed=2000, scale=scale_speed)
sleep_ms(1000)

# rotate left
motor_move(motors, m1_speed=-2000, m2_speed=-2000, m3_speed=2000, m4_speed=2000, scale=scale_speed)
car_rotate(gyro_sensor, 'left', angle=90, angle_bias=angle_bias)

# move forward
motor_move(motors, m1_speed=2000, m2_speed=2000, m3_speed=2000, m4_speed=2000, scale=scale_speed)
sleep_ms(1000)

# rotate right 180
motor_move(motors, m1_speed=2000, m2_speed=2000, m3_speed=-2000, m4_speed=-2000, scale=scale_speed)
car_rotate(gyro_sensor, 'right', angle=180, angle_bias=angle_bias)

# move forward
motor_move(motors, m1_speed=2000, m2_speed=2000, m3_speed=2000, m4_speed=2000, scale=scale_speed)
sleep_ms(1000)

# rotate right 90 degrees
motor_move(motors, m1_speed=2000, m2_speed=2000, m3_speed=-2000, m4_speed=-2000, scale=scale_speed)
car_rotate(gyro_sensor, 'right', angle=90, angle_bias=angle_bias)
    
# move forward
motor_move(motors, m1_speed=2000, m2_speed=2000, m3_speed=2000, m4_speed=2000, scale=scale_speed)
sleep_ms(1000)

# rotate left 180 degrees
motor_move(motors, m1_speed=-2000, m2_speed=-2000, m3_speed=2000, m4_speed=2000, scale=scale_speed)
car_rotate(gyro_sensor, 'left', angle=180, angle_bias=angle_bias)

# stop
motor_move(motors)