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)
lazer_ranger = UltrasonicSensor(Port.E)

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():
    #Get to mission
    await drive_base.straight(920)
    await drive_base.turn(-90,Stop.HOLD)
    await drive_base.straight(65)
    #Solve mission
    drive_base.turn(-10)
    await left_arm.run_angle(10000,-4000)
    #Get to Red Start
    await drive_base.straight(-110)
    await drive_base.turn(90)
    await drive_base.straight(500)
    while True:
        distance_mm = await lazer_ranger.distance()
        print('distancing...',distance_mm)

        if distance_mm < 300:
            drive_base.stop
            break
        else:
            drive_base.straight(300)
            print('running...')
        await wait(10)

run_task(main())