diff --git a/PcBotWorld/.player b/PcBotWorld/.player
diff --git a/PcBotWorld/camera.cfg b/PcBotWorld/camera.cfg
@@ -0,0 +1,16 @@
+#The following driver accesses the Logitech QuickCam Pro 5000
+driver
+(
+  name "camerauvc"
+  provides ["camera:0"]
+  port "/dev/video0"
+#  size [120 90]
+)
+
+#driver
+#(
+#  name "upcbarcode"
+#  provides ["blobfinder:0"]
+#  requires ["camera:0"]
+#)
+
diff --git a/PcBotWorld/hospital_section.png b/PcBotWorld/hospital_section.png
diff --git a/PcBotWorld/laser.cfg b/PcBotWorld/laser.cfg
@@ -0,0 +1,7 @@
+driver
+(
+  name "urglaser"
+  provides ["laser:0"]
+  port "/dev/ttyACM0"
+)
+
diff --git a/PcBotWorld/laser.inc b/PcBotWorld/laser.inc
@@ -0,0 +1,22 @@
+
+define sick_laser laser
+(
+  range_min 0.0
+  range_max 8.0
+  fov 180.0
+  samples 361
+
+  color "blue"
+  size [ 0.14 0.14 ]	
+)
+
+define hokuyo_URG_laser laser
+(
+  range_min 0.0
+  range_max 5.6 
+  fov 230.0
+  samples 360
+
+  color "purple"
+  size [ 0.05 0.05]
+)
diff --git a/PcBotWorld/map.inc b/PcBotWorld/map.inc
@@ -0,0 +1,16 @@
+
+define map model
+(
+  # sombre, sensible, artistic
+  color "black"
+
+  # most maps will need a bounding box
+  boundary 1
+
+  gui_nose 0
+  gui_grid 1
+  gui_movemask 0
+  gui_outline 0
+
+  gripper_return 0
+)
diff --git a/PcBotWorld/roboticLabMap.png b/PcBotWorld/roboticLabMap.png
diff --git a/PcBotWorld/wbr914.inc b/PcBotWorld/wbr914.inc
@@ -0,0 +1,125 @@
+
+# Desc: Device definitions for Whitebox Robotics PC-BOT 914 robot.
+# Author: Neil Schubert
+# Date: 10 Mar 2007
+
+# The PC-BOT IR array
+define wbr914_ir ranger
+(
+
+  # sensor count used in this ranger
+  scount 8
+
+  # define the pose of each (sensor) transducer [xpos ypos heading]
+  spose[4] [ 0.03 0.19 90 ]
+  spose[3] [ 0.19 0.09 25 ]
+  spose[2] [ 0.21 0.00 0 ]
+  spose[1] [ 0.19 -0.09 -25 ]
+  spose[0] [ 0.03 -0.19 -90 ]
+
+  # these 3 transducers are the downward pointing for stairs and drops
+  # because they point down, they have little effect in the Stage 2D
+  # environment
+
+  spose[7] [ 0.20 0.06 56 ]
+  spose[6] [ 0.21 0.00 0 ]
+  spose[5] [ 0.20 -0.06 -56 ]
+
+
+  # define the field of view of each transducer [range_min range_max view_angle]
+
+  sview [0.1 0.8 10]
+
+  # the view of the downward facing ones are set at the ground hit
+  # range even though they do extend farther. 
+
+  sview[5] [0.2 0.68 10]
+  sview[6] [0.2 0.46 10]
+  sview[7] [0.2 0.68 10]
+
+  # define the size of each transducer [xsize ysize] in meters
+  ssize [0.01 0.03]
+)
+
+# a PC-BOT 914 in standard configuration
+define wbr914 position
+(
+
+  # actual size of robot in meters for scaling
+
+  size [0.30 0.30]
+
+  # the PC-BOT's center of rotation is offset from its center of area
+
+  origin [0 0.0 0]
+
+  # draw a nose on the robot so we can see which way it points
+
+  gui_nose 1
+
+  # estimated mass in KG
+
+  mass 25.0 
+
+  # this polygon approximates the shape of a PC-BOT 914
+  # Use two polygons to draw the robot
+
+  polygons 2
+
+  # details of the first polygon
+  # polygon[index].points (total number of polygon points)
+
+  polygon[0].points 12
+
+  # polygon[index].point[index] [ xpos ypos ]
+
+  polygon[0].point[0] [  0.11  0.17 ]
+  polygon[0].point[1] [  0.13  0.12 ]
+  polygon[0].point[2] [  0.18  0.11 ]
+  polygon[0].point[3] [  0.18 -0.11 ]
+  polygon[0].point[4] [  0.13 -0.12 ]
+  polygon[0].point[5] [  0.11 -0.17 ]
+  polygon[0].point[6] [ -0.11 -0.17 ]
+  polygon[0].point[7] [ -0.14 -0.12 ]
+  polygon[0].point[8] [ -0.18 -0.11 ]
+  polygon[0].point[9] [ -0.18  0.11 ]
+  polygon[0].point[10] [ -0.14  0.12 ]
+  polygon[0].point[11] [ -0.11 0.17 ]
+
+  # details of the second polygon
+
+  polygon[1].points 4
+  polygon[1].point[0] [ 0.18  0.08 ]
+  polygon[1].point[1] [ 0.23  0.05 ]
+  polygon[1].point[2] [ 0.23 -0.05 ]
+  polygon[1].point[3] [ 0.18 -0.08 ]
+
+
+  # differential steering model
+
+  drive "diff"
+
+  # laser_return refers to making the robot detectable by laser sensors
+  # if the robot's sensor was mounted on the head, it would not detect other 
+  # similar robots because the beam would go over the robot. This adds more
+  # reality to the simulation (and worse detection) if you uncomment the line
+
+  # laser_return 0
+
+  # use the IR array defined above
+
+  wbr914_ir()  
+  hokuyo_URG_laser( pose [ 0.0 0.0 0.0 ])
+) 
+
+define hokuyo_URG_laser laser
+(
+	range_min 0.0
+	range_max 4.095 
+	fov 240.0
+	samples 666
+
+	color "red"
+	size [ 0.05 0.05]
+
+)
diff --git a/PcBotWorld/wbr914sim.cfg b/PcBotWorld/wbr914sim.cfg
@@ -0,0 +1,25 @@
+
+# Desc: PC-Bot Player sample configuration file for controlling Stage 
+# devices
+# Date: 18 Mar 2007
+
+driver
+(
+  name "stage"
+  provides ["simulation:0"]
+  plugin "libstageplugin"
+
+  # world file has links to .inc files and defines for all the models and 
+  # maps
+
+  worldfile "wbr914sim.world"	
+)
+
+driver( name "stage" provides ["map:0" ] model "hospital" )
+
+
+driver( 
+ name "stage"  
+ provides ["6665:position2d:0" "6665:sonar:0" "6665:laser:0"] 
+ model "wbr914_1" 
+)
diff --git a/PcBotWorld/wbr914sim.world b/PcBotWorld/wbr914sim.world
@@ -0,0 +1,63 @@
+
+# Desc: PC-BOT Stage demo with lots of models
+
+# the size of a pixel in Stage's underlying raytrace model in meters
+resolution     0.025
+
+interval_sim 100  # milliseconds per update step
+interval_real 100 # real-time milliseconds per update step
+
+
+# defines PC-BOT robot
+include "wbr914.inc"
+
+# defines 'map' object used for floorplans
+include "map.inc"
+
+# defines the laser models `sick_laser' configured like a Sick LMS-200
+# and defines Hokuyo URG Laser
+#include "laser.inc"
+
+#defines the size of the world
+size [13.75 9.50 ]
+
+gui_disable 0
+gui_interval 100
+gui_menu_interval 20
+
+window( 
+  size [ 755.000 684.000 ] 
+#  size [ 500 500 ]
+  center [0 0] 
+  scale 0.019 
+)
+
+map( 
+  bitmap "roboticLabMap.png" 
+  map_resolution 0.025
+  size [13.75 9.50] 
+  name "hospital"
+)
+
+
+# a block for gripping
+define puck model(
+  size [ 0.08 0.08 ]
+  gripper_return 1
+  gui_movemask 3
+  gui_nose 0
+  fiducial_return 10
+)
+
+
+
+
+
+wbr914
+(
+  color "white"
+  name "wbr914_1"
+  pose [2.1875 -2.8875 20]
+)
+
+
diff --git a/Plans/GoToPointPlan.cpp b/Plans/GoToPointPlan.cpp
@@ -12,8 +12,7 @@ GoToPointPlan::GoToPointPlan(Robot* robot) :
 	_currLocation = robot->getCurrLocation();
 	_dstLocation = new Location(0, 0, 0);
 	_rotateToPointBeh = new RotateToPoint(_robot, _currLocation, _dstLocation);
-	_advanceToPointBeh = new AdvanceToPoint(_robot, _currLocation,
-			_dstLocation);
+	_advanceToPointBeh = new AdvanceToPoint(_robot, _currLocation, _dstLocation);
 
 	_rotateToPointBeh->addNext(_advanceToPointBeh);
 	_advanceToPointBeh->addNext(_rotateToPointBeh);

diff --git a/Robot.cpp b/Robot.cpp
@@ -1,4 +1,5 @@
 #include "Robot.h"
+#include "Helper.h"
 
 Robot::Robot(char* ip, int port, ConfigurationManager* config)
 {
@@ -8,9 +9,11 @@ Robot::Robot(char* ip, int port, ConfigurationManager* config)
 	_pc = new PlayerClient(ip,port);
 	_pp = new Position2dProxy(_pc);
 	_lp = new LaserProxy(_pc);
-	double matrixToMeterRatio = config->GetMapResolutionCM() / 100;
-	_location = new Location(config->GetStartLocationX() * matrixToMeterRatio, config->GetStartLocationY() * matrixToMeterRatio, config->GetStartLocationYaw());
-	_pp->SetOdometry(_location->getX(), _location->getY(), DTOR(_location->getYaw()));
+
+	Location startLoc = Helper::MapCellToMetersLocation(config->GetStartLocationX(), config->GetStartLocationY());
+	_location = new Location(startLoc.getX(), startLoc.getY(), DTOR(config->GetStartLocationYaw()));
+	_prevRobotLoc = new Location(_location->getX(), _location->getY(), _location->getYaw());
+	_pp->SetOdometry(_location->getX(), _location->getY(), _location->getYaw());
 
 	_pp->SetMotorEnable(true);
 
@@ -30,13 +33,18 @@ void Robot::getDelta(double &dX,double &dY,double &dYaw)
 	double yNew = _pp->GetYPos();
 	double yawNew = _pp->GetYaw();
 
-	dX = xNew - _location->getX();
-	dY = yNew - _location->getY();
-	dYaw = yawNew - _location->getYaw();
-	//cout<< xNew << " " << yNew << " " << yawNew << endl;
+	dX = xNew - _prevRobotLoc->getX();
+	dY = yNew - _prevRobotLoc->getY();
+	dYaw = yawNew - _prevRobotLoc->getYaw();
+
+	// Saves the robot's last location. The robots [0,0] is the map center
+	_prevRobotLoc->setX(xNew);
+	_prevRobotLoc->setY(yNew);
+	_prevRobotLoc->setYaw(yawNew);
 
+	// Sets our location. [0,0] is top left
 	_location->setX(xNew);
-	_location->setY(yNew);
+	_location->setY(_location->getY() - dY);
 	_location->setYaw(yawNew);
 }
 
@@ -52,7 +60,7 @@ double Robot::getLaserDistance(int index)
 }
 
 Location* Robot::getCurrLocation() {
-	return new Location(_pp->GetXPos(), _pp->GetYPos(), _pp->GetYaw());
+	return new Location(_location->getX(), _location->getY(), _location->getYaw());
 }
 
 double Robot::getWidth()

diff --git a/Robot.h b/Robot.h
@@ -15,6 +15,7 @@ class Robot
 	PlayerClient* _pc;
 	Position2dProxy* _pp;
 	LaserProxy* _lp;
+	Location* _prevRobotLoc;
 public:
 	// This location is in meters
 	Location* _location;

diff --git a/WaypointManager.cpp b/WaypointManager.cpp
@@ -8,6 +8,7 @@
 #include <math.h>
 #include "WaypointManager.h"
 #include "Location.h"
+#include "Helper.h"
 
 WaypointManager::WaypointManager(std::vector<GraphLocation> const &path) {
 	constructWayPoints(path);
@@ -28,9 +29,24 @@ bool WaypointManager::isFinished() {
 void WaypointManager::constructWayPoints(std::vector<GraphLocation> const &path) {
 	GraphLocation currLocation = path.front();
 
+	Location lastGraphLoc = Helper::MapCellToMetersLocation(path[0].x, path[0].y);
+	Location nextGraphLoc;
+
 	for (std::vector<GraphLocation>::const_iterator it = (path.begin() + 1); it != path.end(); ++it) {
-		_waypoints.push_back(constructLocation(currLocation, *it));
-		currLocation = *it;
+		nextGraphLoc = Helper::MapCellToMetersLocation(it->x, it->y);
+
+		if (Helper::distanceBetweenTwoLocations(&lastGraphLoc, &nextGraphLoc) > 0.2) {
+			_waypoints.push_back(constructLocation(currLocation, *it));
+
+			currLocation = *it;
+			lastGraphLoc = Helper::MapCellToMetersLocation(currLocation.x, currLocation.y);
+		}
+	}
+
+	GraphLocation lastLocation = path.back();
+
+	if (lastLocation.x != currLocation.x || lastLocation.y != currLocation.y) {
+		_waypoints.push_back(constructLocation(currLocation, lastLocation));
 	}
 }
 

diff --git a/main.cpp b/main.cpp
@@ -10,13 +10,14 @@ using namespace std;
 int main(int argc, char** argv)
 {
 	ConfigurationManager configManager;
+	Helper::setMapResolutionCM(configManager.GetMapResolutionCM());
 //  Real Robot
 //	Robot robot("10.10.245.64",6665, &configManager);
 
 //  Simulator
 	Robot robot("localhost",6665, &configManager);
 	Map map(&configManager, &robot);
-//	PlnObstacleAvoid pln(&robot);
+
 	GoToPointPlan pln(&robot);
 	MapToGraphConverter mapToGraphConverter;
 	PathFinder pathFinder;