Add working waypoint logic, pull ultrasonic stuff into seperate class