from pybricks.hubs import PrimeHub
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch
from pybricks.tools import run_task, multitask

hub = PrimeHub()

left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
right_motor = Motor(Port.B)

left_arm = Motor(Port.C, Direction.COUNTERCLOCKWISE)
right_arm = Motor(Port.D)

drive_base = DriveBase(left_motor, right_motor, wheel_diameter=68.8, axle_track=180)

drive_base.settings(600,500,300,200)
drive_base.use_gyro(True)

async def main():
    #Setup for mission
    left_arm.run_angle(1000, 300)
    right_arm.run_angle(1000,500)
    await drive_base.straight(320)
    # Solve "Silo"
    await right_arm.run_angle(5000,-500, Stop.HOLD)
    await right_arm.run_angle(5000,500, Stop.HOLD)
    await right_arm.run_angle(5000,-500, Stop.HOLD)
    await right_arm.run_angle(5000,500, Stop.HOLD)
    await right_arm.run_angle(5000,-500, Stop.HOLD)
    #Get to next mission
    await drive_base.turn(-20)
    await drive_base.straight(277)
    await drive_base.turn(20)
    await drive_base.straight(65)
    #Solve "Who Lived Here?"
    await drive_base.turn(-30)
    right_arm.run_angle(70,500)
    #Get to next mission
    await drive_base.turn(45)
    await drive_base.straight(-145)
    await drive_base.turn(-60)
    await drive_base.straight(90)
    #Solve "What's on Sale?"
    await left_arm.run_angle(1000,-450)
    await drive_base.straight(-145)
    await left_arm.run_angle(1000,450)
    await drive_base.straight(10)
    #Return to home
    await drive_base.turn(35)
    await drive_base.straight(-600)
run_task(main())