Updated Waypoints Test

Got three of the the five tests working, I don't think we need to use
testDraw() as this method can be tested in DrawableTest.java