integrate object avoidance into state machine and travel to waypoint. needs tested