left_motor = "47250246222630351826392"
right_motor = "47242769666972617759775"
servo_controller = "33090094875193840563108"
rfid_sensor = "51977090869270841821982"
wrist_servo = "servo0"
elbow_servo = "servo1"

def autonomous_setup():
    print("Autonomous mode has started!")

def autonomous_main():
    pass

async def autonomous_actions():
    print("Autonomous action sequence started")
    await Actions.sleep(1.0)
    print("1 second has passed in autonomous mode")

def teleop_setup():
    print("Tele-operated mode has started!")

def teleop_main():

    set_left_motor(1 * Gamepad.get_value("joystick_right_y"))
    set_right_motor(1 * Gamepad.get_value("joystick_left_y"))
    # set_right_motor(1)
    # Robot.run(jiggle, .2)
    if Gamepad.get_value("l_bumper"):
        Robot.run(wave)
    if Gamepad.get_value("r_bumper"):
        Robot.run(high_five)
    if Gamepad.get_value("r_trigger"):
        reset_elbow()
    # if Gamepad.get_value("joystick_right_y") > 0.5:
    #     Robot.set_value(left_motor, "duty_cycle", -1.0)
    #     Robot.set_value(right_motor, "duty_cycle", -1.0)
    # else:
    #     Robot.set_value(left_motor, "duty_cycle", 0)
    #     Robot.set_value(right_motor, "duty_cycle", 0)
    if (Robot.get_value(rfid_sensor, "tag_detect")):
        if (Robot.get_value(rfid_sensor, "id") > 100):
            Robot.run(wave)
        else:
            Robot.run(high_five)
    
def set_left_motor(power):
    """Sets the power of the left motor"""
    Robot.set_value(left_motor, "duty_cycle", -1 *power)
    
def set_right_motor(power):
    """Sets the power of the right motor"""
    Robot.set_value(right_motor, "duty_cycle", 1 * power)
    
def set_wrist_servo(angle):
    Robot.set_value(servo_controller, wrist_servo, angle)
    
def set_elbow_servo(angle):
    Robot.set_value(servo_controller, elbow_servo, angle)

def reset_elbow():
    Robot.set_value(servo_controller, elbow_servo, 0)

async def wave():
    set_wrist_servo(0)
    for _ in range(3):
        set_wrist_servo(-1)
        await Actions.sleep(1)
        set_wrist_servo(1)
        await Actions.sleep(1)
    set_wrist_servo(0)

async def high_five():
    set_elbow_servo(0)
    await Actions.sleep(1)
    set_elbow_servo(-1)
    await Actions.sleep(1)
    set_elbow_servo(0)
    print("resetting elbow servo")
    await Actions.sleep(1)

async def wave_and_high_five():
    set_wrist_servo(0)
    for _ in range(2):
        set_elbow_servo(0)
        await Actions.sleep(1)
        set_elbow_servo(-1)
        await Actions.sleep(1)

    set_elbow_servo(.1)
    await Actions.sleep(.3)
    set_elbow_servo(.05)
    await Actions.sleep(.3)
    set_elbow_servo(0)
    await Actions.sleep(1)
    
    for _ in range(3):
        set_wrist_servo(-1)
        await Actions.sleep(1)
        set_wrist_servo(1)
        await Actions.sleep(1)
    
async def jiggle(power):
    set_right_motor(power)
    set_left_motor(power)
    await Actions.sleep(.5)
    set_right_motor(0)
    set_left_motor(0)
    await Actions.sleep(1)
    set_right_motor(-power)
    set_left_motor(-power)
    await Actions.sleep(.9)
    set_right_motor(0)
    set_left_motor(0)
    await Actions.sleep(.5)
    

@@ -14,28 +14,28 @@
 %22472
-50246222630351826392
+45441265854464253044
 %22%0Ari
@@ -55,27 +55,27 @@
 4724
-2769666972617759775
+9523884738466958276
 %22%0Ase
@@ -100,27 +100,27 @@
 3309
-0094875193840563108
+2823689496063720264
 %22%0Arf
@@ -555,131 +555,312 @@
 ():%0A
-%0A
     
-set_left_motor(1 * Gamepad.get_value(%22joystick_right_y%22))%0A    set_right_motor(1 * Gamepad.get_value(%22joystick_left_y%22)
+x_dir = Gamepad.get_value(%22joystick_left_x%22)%0A    y_dir = Gamepad.get_value(%22joystick_left_y%22)%0A    bound = lambda x : max(-1, min( 1, x))%0A    left_motor_power = bound(y_dir - x_dir)%0A    right_motor_power = bound(y_dir + x_dir)%0A    set_right_motor(right_motor_power)%0A    set_left_motor(left_motor_power
 )%0A  
@@ -2213,33 +2213,34 @@
 t Actions.sleep(
-1
+.5
 )%0A        set_wr
@@ -2272,33 +2272,34 @@
 t Actions.sleep(
-1
+.5
 )%0A    set_wrist_
@@ -2370,33 +2370,35 @@
 t Actions.sleep(
-1
+.75
 )%0A    set_elbow_
@@ -2423,33 +2423,35 @@
 t Actions.sleep(
-1
+.75
 )%0A    set_elbow_

Update Staff Curry Code