Corrected bugs in the path planner. Path is now being correctly generated.