#!/usr/bin/env pybricks-micropython

from pybricks import ev3brick as brick
from pybricks.ev3devices import (Motor, TouchSensor, ColorSensor,
                                 InfraredSensor, UltrasonicSensor, GyroSensor)
from pybricks.parameters import (Port, Stop, Direction, Button, Color,
                                 SoundFile, ImageFile, Align)
from pybricks.tools import print, wait, StopWatch
from pybricks.robotics import DriveBase
import math
brick.sound.beep()
wait(500)
brick.sound.beep()
wait(500)
L = Motor(Port.A)
R = Motor(Port.B)
color_1P=ColorSensor(Port.S1)
color_2P=ColorSensor(Port.S4)
color_B1P=ColorSensor(Port.S2)
color_B2P=ColorSensor(Port.S3)
wheel_diameter = 56
axle_track = 110
robot = DriveBase( L, R,  wheel_diameter, axle_track)
#robot.drive_time(0, 90, 1000)
#robot.stop(Stop.BRAKE)
def main_runFWD() :
    while True :
        color1=color_1P.color()
        color2=color_2P.color()
        robot.drive(180,0)
        if color1 == 1 or color2 == 1 :
         robot.stop(Stop.BRAKE)
         wait(100)
         L.reset_angle(0)
         R.reset_angle(0)
         break

    #while L.angle() < 180:
        #robot.drive (-200, 0)
        #break
    robot.stop(Stop.BRAKE)
def main_RR() :
    robot.drive_time(0, 90, 1000)
    robot.stop(Stop.BRAKE)
    wait(100)
def main_LL() :
    robot.drive_time(0, -90, 1000)
    robot.stop(Stop.BRAKE)
    wait(100)
def pushbin() :
    while True :
    M_motor.on_for_degrees(speed=100,degrees=120)
    sleep(0.2)
    M_motor.on_for_degrees(speed=-100,degrees=120)
    sleep(3)
main_runFWD()
pushbin()
main_RR()
main_runFWD()
main_RR()
main_runFWD()
main_LL()
main_runFWD()
main_LL()
main_runFWD()