i reverted the changes i made to waypoints.cpp
i'll try to figure out what's going on after the competition.

Signed-off-by: rkburnside <richard@burnsides.org>