# The software for the chased robot
# This software can have a robot run on its own, backing up and turning
# when it observes an obstacle with the IR sensors, or it can be controlled
# from a game pad device attached to the controlling computer. The two
# versions of control are subsumed together, with the latter overriding
# the former.

# James Heliotis
# Matt Brenner
# 13 June 2008

from myro import *

SECONDS = 1.0
MINUTES = 60 * SECONDS
RUN_TIME = 5 * MINUTES

class Arbitrator:
    """Implementor of the standard robotic subsumption architecture's
       arbitration algorithm.

       James Heliotis
    """

    def __init__( self, defaultTimeout = None ):
        """Set the intial list of goal behaviors. If a
           defaultTimeout is given, that will be the delay
           between polls to the goalBehaviors. If None, then
           the goalBehaviors themselves must provide the delay
           to the next polling time.
        """
        self.goals = []
        self.delay = defaultTimeout

    def addGoal( self, goalBehavior ):
        """Add an additional goal. Its priority will be lower
           than all the others already in the Arbitrator.
           goalBehavior is a callable object that returns a
           tuple containing:
           1. True/False: whether or not to use this result:
           2. Linear speed suggestion
           3. Rotational speed suggestion
        """
        self.goals.append( goalBehavior )

    def arbitrate( self ):
        """Check all the goal behaviors and return a translate and
           rotate command as a tuple.
           If any goal behavior returns None, this is a signal to
           shut down operations. Return None.
           If no goal behavior returns its first tuple element
           True, also shut down. (Return None.)
        """
        if len( self.goals ) == 0: raise "No goals!"
        result = None
        for goal in self.goals:
            command = goal()
            if command == None: # The secret stop signal
                result = None
                break
            elif command[ 0 ]:
                if self.delay == None:
                    result = ( command[ 1 ], command[ 2 ], command[ 3 ] )
                else:
                    result = ( command[ 1 ], command[ 2 ], self.delay )
                print result
                break
        return result

class Gamecontrol:
    """A higher level API to the game pad for the controls used in the
       robot chasing program

       Matt Brenner
    """

    # Constants returned by this class's methods

    LEFT = 1   # bit mask 0001
    RIGHT = 2  # bit mask 0010
    UP = 4     # bit mask 0100
    DOWN = 8   # bit mask 1000

    SLOW = 1
    FAST = 2

    XAXIS = 0
    YAXIS = 1

    RIGHTFRONTOP = 6
    RIGHTFRONTBOTTOM = 7
    BOTTOMFRONT = 0 # the Stop button


    def getdirections (self):
        "Return which directions are currently being pushed on the axis."
        
        axis = myro.getGamepadNow ("axis")
        xaxis = axis[Gamecontrol.XAXIS]
        yaxis = axis[Gamecontrol.YAXIS]

        dir = 0
        if xaxis < -.5:
            dir |= Gamecontrol.LEFT
        elif xaxis > .5:
            dir |= Gamecontrol.RIGHT
        
        if yaxis < -.5:
            dir |= Gamecontrol.UP
        elif yaxis > .5:
            dir |= Gamecontrol.DOWN

        return  dir

    
    def isoveride (self):
        "Return true iff one of the speed buttons is being pushed."
        
        buttons = myro.getGamepadNow ("button");
        return buttons[Gamecontrol.RIGHTFRONTOP] or \
               buttons[Gamecontrol.RIGHTFRONTBOTTOM]


    def getspeed (self):
        """Return the speed requested by the user's current choice of
           speed button pushes."""
        
        speed = 0;
        
        buttons = myro.getGamepadNow ("button")
        if buttons[Gamecontrol.BOTTOMFRONT] > 0:
            raise "STOP"
        elif buttons[Gamecontrol.RIGHTFRONTOP] > 0:
            speed = Gamecontrol.FAST
        elif buttons[Gamecontrol.RIGHTFRONTBOTTOM] > 0:
            speed = Gamecontrol.SLOW

        return  speed


    def dirnames (self, dir):
        "Display names of directions currently chosen. Diagnostic method."

        text = ""
    
        if dir & Gamecontrol.LEFT:
            text += "LEFT "
        elif dir & Gamecontrol.RIGHT:
            text += "RIGHT "
            
        if dir & Gamecontrol.UP:
            text += "UP "
        elif dir & Gamecontrol.DOWN:
            text += "DOWN"

        return  text

class Wanderer:
    """This goal class contains code for the scribbler to avoid walls.

       James Heliotis
    """

    FORWARD = 0.35 # Standard forward speed
    BACKWARD = -0.2 # Standard backup-to-avoid-wall speed
    TURN = 0.4 # Standard turn-to-avoid-wall speed

    NORMAL_DELAY = 1.0 * SECONDS # How long to wait before checking IR again
    BACKWARD_DELAY = 5.0 * SECONDS # How long to backup after a wall
    TURN_DELAY = 2.0 * SECONDS # How long to keep turning after a wall

    def __init__( self ):
        "Initialize to allow forward movement if coast is clear."
        self.tooClose = False

    def whatToDo( self ):
        """Return a standard goal tuple. (See Arbitrator for format.)
           If the coast is clear, move forward"""
        left, right = myro.getIR()
        if not self.tooClose:
            if ( left, right ) == ( 1, 1 ):
                cmd = ( True, Wanderer.FORWARD, 0, Wanderer.NORMAL_DELAY )
            else:
                self.tooClose = True
                cmd = ( True, Wanderer.BACKWARD, 0, Wanderer.BACKWARD_DELAY )
        else: # Robot WAS too close. It has backed up. Now turn.
            self.tooClose = False
            cmd = ( True, 0, Wanderer.TURN, Wanderer.TURN_DELAY )
        return cmd

# Game pad control constants

SLOWSPEED = .3
HIGHSPEED = 1

ROTATE = .5

GAMEPAD_POLL_DELAY = 0.1 * SECONDS # How responsive the game pad should be

def getmovement( gc ):
    """Goal function for game pad control.
       The axis controller controls the direction.
       Buttons 7 and 8 (right side, in back) control the speed.

       gc: instance of Gamecontrol class that accesses the game pad
    """
    overide = False
    howfast = 0
    rotate = 0
    
    if gc.isoveride():
        overide = True

        try:
            speed = gc.getspeed();
        except:
            print "Stop signal received."
            return None
        if speed != 0:
            dirs = gc.getdirections()

            if dirs & Gamecontrol.LEFT:
                rotate = ROTATE
            elif dirs & Gamecontrol.RIGHT:
                rotate = -ROTATE
            else:
                rotate = 0
            
            if speed == Gamecontrol.SLOW:
                howfast = SLOWSPEED
                rotate /= 2
            else:
                howfast = HIGHSPEED
            
            if dirs & Gamecontrol.DOWN:
                howfast = -howfast
                rotate = -rotate
            elif dirs & Gamecontrol.UP:
                howfast = howfast
            else:
                howfast = 0
            
    return (overide, howfast, rotate, GAMEPAD_POLL_DELAY)

initialize( "COM6" )

def brain():
    """Wander around, doing whatever the goals tell me to do. This
       is the standard subsumption architecture algorithm."""
    arbitrator = Arbitrator()
    gc = Gamecontrol()
    wanderer = Wanderer()
    arbitrator.addGoal( lambda: getmovement( gc ) )
    arbitrator.addGoal( lambda: Wanderer.whatToDo( wanderer ) )

    speak( "Help me. He is after my ball!", 0 )
    try:
        decision = "OK" # Anything but None so the loop will start
        while decision != None:
            decision = arbitrator.arbitrate()
            if decision != None:
                t, r, delay = decision
                move( t, r )
                wait( delay )
            else:
                speak( "You have asked me to stop.", 1 )
                print "Loop killed."
        print "Operation is complete."
    except str, s:
        print 'Sorry, arbitration failed: "' + s + '"'
    move( 0, 0 )

if __name__ == "__main__":
    brain()