Pac Man Robot

from gopigo import *
from time import sleep

#Node class for the maze
class MazePoint:
    def __init__(self, direction, distance):

        self.direction = direction
        self.distance = distance

    def getDirectionInverse(self):
        if(self.direction == 'right'):
            return 'left'
        elif(self.direction == 'left'):
            return 'right'
        else:
            return 'none'
#End of node class

#Stack object taken from:
#http://interactivepython.org/runestone/static/pythonds/BasicDS/ImplementingaStackinPython.html
class Stack:
     def __init__(self):
         self.items = []
         self.size = 0

     def isEmpty(self):
         return self.items == []

     def push(self, item):
         self.items.append(item)
         self.size += 1

     def pop(self):
         return self.items.pop()
         self.size -= 1

     def peek(self):
         return self.items[len(self.items)-1]

     def size(self):
         return self.size
#End of stack object

#Stopping function
def stopFunc():
    stop()
    servo(90)
    disable_servo()

ROTATE_TIME = .10
ROTATE_FACTOR = 1.5
FORWARD_TIME = .10
US_PIN = 15
STOP_DISTANCE = 10

def main():
    moves = Stack()

    goAgain = True

    while(goAgain):
        leftDistance = 0
        rightDistance = 0
        forwardDistance = 0
        maxAngle = 0

        #Get distances from the sides and front
        sleep(0.6)
        servo(90)
        sleep(0.6)
        forwardDistance = us_dist(US_PIN)
        sleep(0.6)
        servo(0)
        sleep(0.6)
        rightDistance = us_dist(US_PIN)
        sleep(0.6)
        servo(180)
        sleep(0.6)
        leftDistance = us_dist(US_PIN)
        sleep(0.6)

        if(rightDistance > 10 or leftDistance > 10 or forwardDistance > 10): #Keep going

            #Look for the side furthest away from a wall
            maxDistance = max(forwardDistance, leftDistance, rightDistance)

            if(maxDistance == forwardDistance):
                maxAngle = 90
            elif(maxDistance == leftDistance):
                maxAngle = 180
            else:
                maxAngle = 0

            servo(90)

            #Find the way to go and save it to the stack
            if maxAngle < 60:
                right_rot()
                print("rotating right")
                sleep(ROTATE_FACTOR * float(60 - maxAngle) / 120.0)
                moves.push(MazePoint('right', us_dist(US_PIN)))
            elif maxAngle > 120:
                left_rot()
                print('rotating left')
                sleep(ROTATE_FACTOR * float(maxAngle - 120) / 120.0)
                moves.push(MazePoint('left', us_dist(US_PIN)))
            else:
                moves.push(MazePoint('forward', us_dist(US_PIN)))

            fwd()
            print 'Going forward'
            while us_dist(US_PIN) > STOP_DISTANCE:
                sleep(FORWARD_TIME)
            stop()

        else: #Backtracking
            if(moves.size > 0):
                lastMove = moves.pop()

                #Rotate 180
                sleep(1)

                fwd()
                while us_dist(US_PIN) > STOP_DISTANCE:
                    sleep(FORWARD_TIME)
                stop()

                #Rotate to the way contrary to the one already taken
                if(lastMove.getDirectionInverse == 'right'):
                    right_rot()
                    sleep(ROTATE_FACTOR * float(60 - 0) / 120.0)

                elif(lastMove.getDirectionInverse == 'left'):
                    left_rot()
                    sleep(ROTATE_FACTOR * float(60 - 180) / 120.0)


if __name__ == '__main__':
    set_left_speed(100)
    set_right_speed(100)
    enable_servo()
    main()
    stopFunc()