#!/usr/bin/env python3
from ev3dev2.motor import Motor, LargeMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D, SpeedPercent
from ev3dev2.sensor.lego import TouchSensor, UltrasonicSensor  
from ev3dev2.sensor import INPUT_1, INPUT_4
import sys

motorLeft = LargeMotor(OUTPUT_D)
motorRight = LargeMotor(OUTPUT_A)
motorUpDown = Motor(OUTPUT_C)
motorLeftRight = Motor(OUTPUT_B)
ultraSensor = UltrasonicSensor(INPUT_4)
touchSensor = TouchSensor(INPUT_1)

motorUpDown.on_for_seconds(SpeedPercent(-50), seconds=2.8)
motorLeftRight.on_for_degrees(SpeedPercent(int(sys.argv[1])), 400)

motorLeft.on(SpeedPercent(-70), block=False)
motorRight.on(SpeedPercent(-70))

while True:
    if ultraSensor.distance_centimeters < 10:
        motorLeft.on(SpeedPercent(-25), block=False)
        motorRight.on(SpeedPercent(-25))
        break


    
touchSensor.wait_for_pressed(timeout_ms=None, sleep_ms=10)
motorLeft.off()
motorRight.off()