from microbit import *
from microbit import sleep, i2c
from PCA9685 import *
from servo import *

num1 = 60
num2 = 90
num3 = 60
num4 = 130

Left_Y = 0
Left_X = 0
Right_Y = 0
Right_B = 0
pin14.PULL_UP
pin15.PULL_DOWN
pin14.write_digital(1)
pin15.write_digital(0)

# Initialise the PCA9685 using the default address (0x40).
pwm = PCA9685.PCA9685(i2c)

# Set frequency to 60hz, good for servos.
pwm.set_pwm_freq(60)

print('Moving servo on channel 0, press Ctrl-C to quit...')

# Use servo 1 helper class to move channel 4 by degrees
s1 = Servos(i2c)
# Use servo 2 helper class to move channel 5 by degrees
s2 = Servos(i2c)
# Use servo 3 helper class to move channel 6 by degrees
s3 = Servos(i2c)
# Use servo 4 helper class to move channel 7 by degrees
s4 = Servos(i2c)

#Set the initial angle of servo 1(right servo,channel 4)
s1.position(4, 60)
sleep(200)
#Set the initial angle of servo 2(the base rotates the servo,channel 5)
s2.position(5, 90)
sleep(200)
#Set the initial angle of servo 3(claw servo,channel 6)
s3.position(6, 60)
sleep(200)
#Set the initial angle of servo 4(left servo,channel 7)
s4.position(7, 130)
sleep(200)

def baseplate_rotate():
    global num2
    if Left_X < 100:
        num2 = num2 - 1
        s2.position(5, num2)
        sleep(5)
        if num2 <= 30:
            s2.position(5, 30)
            sleep(200)
    if Left_X > 900:
        num2 = num2 + 1
        s2.position(5, num2)
        sleep(5)
        if num2 >= 150:
            s2.position(5, 150)
            sleep(200)

def clamp_claw():
    if Right_B == True:
        s3.position(6, 180)
        sleep(2)
    if Right_B == False:
        s3.position(6, 60)
        sleep(2)

def left_arm():
    global num4
    if Left_Y > 900:
        num4 =num4 + 1
        s4.position(7, num4)
        sleep(5)
        if num4 >= 180:
            s4.position(7, 180)
            sleep(200)

    if Left_Y < 100:
        num4 = num4 - 1
        s4.position(7, num4)
        sleep(5)
        if num4 <= 35:
            s4.position(7, 35)
            sleep(200)

def right_arm():
    global num1
    if Right_Y > 800:
        num1 =num1 - 1
        s1.position(4, num1)
        sleep(5)
        if num1 <= 0:
            s1.position(4, 0)
            sleep(200)
    if Right_Y < 100:
        num1 =num1 + 1
        s1.position(4, num1)
        sleep(5)
        if num1 >= 180:
            s1.position(4, 180)
            sleep(200)
while True:
    Left_Y = pin0.read_analog()
    Left_X = pin1.read_analog()
    Right_Y = pin2.read_analog()
    Right_B = pin13.read_digital()
    print("Left_Y analog signal:",Left_Y)
    print("Left_X analog signal:",Left_X)
    print("Right_Y analog signal:",Right_Y)
    print("Right_B analog signal:",Right_B)

    baseplate_rotate()
    clamp_claw()
    left_arm()
    right_arm()
