Feature/state machine

Made drive_over and reorient_robot so that a rough "grab" is done. Sometimes the robot measures angle-to-fiducial poorly