Feature/state machine

Implemented waypoint maneuvers, haven't yet finished Seek or Grab code