changes from robot
Fixed such that the planned path and robot position are in the same limits
Working code from robot
Merge pull request #15 from JohanJohansson/master

Updates from Robot