Enabled getCurrentPositon and updateCurrentPosition
Added getCurrentPostion function
Removed un needed comments
Working on simple test code
Add setCurrentPosition and addWaypoint
Enabled setCurrentPosition and addWaypoint
Added the getNextWaypoint function
Continue to write testing code