Test

# Importing the required modules for the program
import time
import easygopigo3 as easy

# Constructing variables to determine whether we need to ask the users input
isBlockedLeft = False
isBlockedRight = False

# Creating a reference to the robot
GPG = easy.EasyGoPiGo3()
# Creating a refernce to the distance sensor
distSen = GPG.init_distance_sensor()
# Getting the distance in cm
distance = distSen.cm()

# Creating a function that will move forward until we are are certain distance away from the object
def moveForwardUntil(distanceAwayFromObject):
    # Creating a variable to store the inital distance that the robot is away from the object
    initial = -1
    while distance > distanceAwayFromObject:
        # Updating the variable only when it is -1, so the first time
        if initial == -1:
            initial = distance
        else:
            # Move forward in a loop until is 'distanceAwayFromObject' from the object
            time.sleep(1)
            GPG.forward()
    else:
        # Once the robot has reached its destination we want to get the distance it is away from the object now
        # We can now calculate how far it is traveled, we are not using the 'distanceAwayFromObject' because this would not be the actual distance traveled
        final = distance
        findDistanceTraveled(initial, final)
        # Once we have printed the traveled distance we must decide where to move
        checkBlockedLeftRight()

def checkBlockedLeftRight():
    # Reset the Right and Left
    isBlockedLeft = False
    isBlockedRight = False
    # Firstly check right
    turnAmount(90)
    sleep(1)
    # Determing if it is blocked on the right
    if int(distance) <= 10:
        print("Blocked Right")
        isBlockedRight = True

    if isBlockedRight:
        # Checking is blocked left only if it is blocked right
        turnAmount(180)
        sleep(1)

        if int(distance) <= 10:
          print("Blocked Left")
          isBlockedLeft = True

          # Checking if it is both blocked left and right
          if isBlockedLeft and isBlockedRight:
              print("Asking input...")

        elif int(distance) >= 15:
            print("Not Blocked On Left So We Should Move Left")
            # Already in the direction we need to move forward
            moveForwardUntil(15)
    else int(distance) >= 15:
        print("Not Blocked On Right So We Should Move Right")
        # Already in the direction we need to move forward
        moveForwardUntil(15)



def findDistanceTraveled(initialDistance, finalDistance):
    # Calculating the distance traveled before it reaches an obstacle or needs to turn
    movedDistance = int(int(initialDistance) - int(finalDistance))
    print("The robot has moved ", movedDistance, "cm")

def turnAmount(degrees):
    # Move the amount of degrees specified
    GPG.drive_degrees(degrees)


while True:
    # Continually update the distance
    distance = distSen.cm()
    moveForwardUntil(15)