# VEX EDR Python-Project
import sys
import vex

#region config
armangle   = vex.AngleSensor(1)
clawangle  = vex.AngleSensor(2)
freeda     = vex.Motor(2, True)
fletcher   = vex.Motor(3)
lama       = vex.Motor(4)
whiskey    = vex.Motor(5)
champagne  = vex.Motor(6)
ram        = vex.Motor(7, True)
brad       = vex.Motor(8)
blinda     = vex.Motor(9, True)
joystick   = vex.Joystick()
joystick_p = vex.Joystick(True)
#endregion config

# create a concurrent child process to execute background tasks
def child_thread():
    while True:
        Arm_Hold()
        sys.run_in_thread(child_thread)

# function to auto hold arm at set position
SETARM = 0
def Arm_Hold():
    global SETARM
    if SETARM == 0:
        ram.off()
        lama.off()
    else:
        power = (armangle.angle_degrees() - SETARM) * 1.5
    if power > 80:
        power = 80
    if power < -80:
        power = -80
    if abs(power) < 10:
        power = 0
    ram.run(power)
    lama.run(power)
#Base---------------------------------------------------------------------------

while True:
#axis 1&2 : Arcade Drive
# get current joystick position/values

    steer = joystick.axis1()
    move = -joystick.axis2()

#calculate left and right power
    left_power = move + steer
    right_power = move - steer

#apply power to motors
    fletcher.run(left_power)
    blinda.run(left_power)
    freeda.run(right_power)
    brad.run(right_power)

#Arm----------------------------------------------------------------------------
    if joystick.b6down():
        SETARM = SETARM - 1
    elif joystick.b6up():
        SETARM = SETARM + 1

    SETARM=100

    if SETARM < 0 or SETARM > 360:
        SETARM = 0
    sys.sleep(.01)

#Claw---------------------------------------------------------------------------

    if joystick.b5up():
        champagne.run(70)
    elif joystick.b5down():
        champagne.run(-70)
    else:
        champagne.off()