CS148 asgn3: Path Planning and Soccer Playing

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

Defined Variables:
    world = state of the world (stores all objects)
    waypointIter = iterator over list of positions, points to the current waypoint in the list
    previousError = previous error from the proportional derivative controller
    facePoint = point the robot should be facing toward after completing the path
        

function PathFollower.updateRobot(robotProxy, forwardSpeed) 
    robot = world.getObjectState(robotId);

    if (robot is null)
        return

    while ((not at final waypoint) and (robot is at current waypoint))
        waypointIter++;

    if (not at final waypoint) 
        waypoint = getCurrentWaypoint();
        
        robotToWaypointVec = waypoint - robot.position;
        robotToWaypointVec.normalize();

        Point rotatedHeading
        rotatedHeading.x = cos(-robotYaw)*robotToWaypointVec.x - sin(-robotYaw)*robotToWaypointVec.y;
        rotatedHeading.y = sin(-robotYaw)*robotToWaypointVec.x + cos(-robotYaw)*robotToWaypointVec.y;
        
        headingError = atan2(rotatedHeading.x, -rotatedHeading.y);
        
        errorDeriv = (headingError - previousHeadingError) * DT;
        rotate = KP * headingError + KD * errorDeriv;
        previousError = headingError;

        robotProxy.SetSpeed(forwardSpeed, -rotate);

    else
        if (not facing goal)
            waypoint = facePoint;
            
            Point robotToWaypointVec = waypoint - robotPos;
            robotToWaypointVec.normalize();
    
            Point rotatedHeading;
            rotatedHeading.x = cos(-robotYaw)*robotToWaypointVec.x - sin(-robotYaw)*robotToWaypointVec.y;
            rotatedHeading.y = sin(-robotYaw)*robotToWaypointVec.x + cos(-robotYaw)*robotToWaypointVec.y;
            
            float headingError = atan2(rotatedHeading.x, -rotatedHeading.y);
            
            float errorDeriv = (headingError - m_previousHeadingError) * DT;
            float rotate = KP * headingError + KD * errorDeriv;
            m_previousHeadingError = headingError;
    
            robotProxy.SetSpeed(0.0, -rotate);