CS148 asgn3: Path Planning and Soccer Playing

Author: Patrick Doran
Login: pdoran
Group: Enamored
Due: 03/08/2009

Defined Variables:
    me = robot I am controlling
    path = list of points to follow
    pathFollower = Proportional-Derivative controller that can follow a path
    rrs = instance of a class than can perform the rapidly exploring random search algorithm
        

    case OFFENSE_SEEK_BALL:
    if (ball on the field)
        if (isPathValid(path) or ball.distanceFrom(me) > BALL_RADIUS * 2.0 )
            Point pt = getBallKickPoint(ballPoint, theirGoalPoint)
            if(rrs.createPath(moverId,getBallKickPoint(ballPoint, theirGoalPoint),path))
                pathFollower.setPath(path)
                pathFollower.updateRobot(posProxy,SPEED_SEEK)
            else
                path.clear()
                path.push_back(getBallKickPoint(ballPoint, theirGoalPoint))
                pathFollower.setPath(path)
                pathFollower.updateRobot(posProxy,SPEED_LUDICROUS)
        else
            if (pathFollower.pathIsDone())
                state = OFFENSE_TAKE_SHOT
                path.clear()
            else
                pathFollower.updateRobot(posProxy, SPEED_SEEK)  
    else
        state = RESTART
        path.clear()
        

    case OFFENSE_TAKE_SHOT:
    if (ball on the field) 
        meToBallVector = ball-me;
        currentHeading = Point robotHeading(sin(myYaw), -cos(myYaw))
        if (ball.distanceFrom(me) >= RESEEK_BALL_DIST) {Too far from ball}
            state = OFFENSE_SEEK_BALL
            path.clear()
            pathFollower.setPath(path)
        else if (meToBallVector.dot(currentHeading) < 0.25) {No longer facing ball}
            state = OFFENSE_SEEK_BALL
            path.clear()
            pathFollower.setPath(path)
        else
            path.clear()
            path.push_back(theirGoalPoint)
            pathFollower.setPath(path)
            pathFollower.updateRobot(posProxy,SPEED_KICK)
    else
        state = RESTART
        path.clear()
        

    case DEFENSE:
    state = OFFENSE_SEEK_BALL
    break
        

             
    case RESTART:
    if(ball on the field)
        state = OFFENSE_SEEK_BALL
        path.clear()
    else
        if (isPathValid(path))
            if (rrs.createPath(moverId,myResetPoint,path))
                pathFollower.setPath(path)
            else
                path.clear()
                path.push_back(myResetPoint)
                pathFollower.setPath(path)
                pathFollower.updateRobot(posProxy,SPEED_LUDICROUS)
        else
            pathFollower.updateRobot(posProxy,SPEED_SEEK)
    break