from robobopy.Robobo import Robobo #import Robobo Library
from robobopy.utils.IR import   IR
from robobopy.utils.Orientation import Orientation
from robobopy.utils.Color import *

def turn_righ(grades): #turn right the robot 80 degrees or other value
    rotation_degress = rob.readOrientationSensor().yaw + grades

    if rotation_degress > 180:
        rotation_degress = rotation_degress - 360
    rob.moveWheels(-10,10)

    while True:
        rob.wait(0.1)
        if rob.readOrientationSensor().yaw > rotation_degress -20 and rob.readOrientationSensor().yaw < rotation_degress: 
            break

    rob.stopMotors()

def turn_left(grades): #turn left the robot 80 degrees or other value
    rotation_degress = rob.readOrientationSensor().yaw - grades

    if rotation_degress < -180:
        rotation_degress = rotation_degress + 360
    rob.moveWheels(10,-10)

    while True:
        rob.wait(0.1)
        if abs(rob.readOrientationSensor().yaw) < rotation_degress and abs(rob.readOrientationSensor().yaw) > rotation_degress -20: 
            break
    
    rob.stopMotors()

def cross_wall():
    while True:
        rob.moveWheels(speed, speed)
        rob.wait(0.1)
        if (rob.readIRSensor(IR.FrontLL) <= 2): 
            rob.moveWheelsByTime(speed,speed,2)
            break


def color_detect():
    while True:
        rob.wait(0.1)
        rob.moveWheels(5,-5)  #rotate until you see the green color in the center
        if rob.readColorBlob(Color.GREEN).posx > 20: 
            rob.moveWheels(speed,speed) #advance until the robot are close to the color green
            if rob.readColorBlob(Color.GREEN).size > 300:
                rob.stopMotors()
                break

# MAIN PROGRAM
IP = 'localhost'
rob = Robobo(IP) #create a instance of Robobo class
rob.connect()  #connect robobo
speed = 20     
ir_value = 150
rob.moveTiltTo(90,50)
rob.setActiveBlobs(False,True,False, True) 

rob.moveWheels(speed,speed)  
while (rob.readIRSensor(IR.FrontC) < ir_value): #move while IR sensor value is below 100 (or other value)
    rob.wait(0.1)
 
turn_righ(90) #turn right 90 degrees

cross_wall() #the robot advances to pass the box

turn_left(80) #turn left 90 degrees

cross_wall()

color_detect() # detect green color 

rob.stopMotors() #stop the motors

rob.disconnect() #disconnect robobo