Refined stop conditions, added nonfatal error
Getting ready to integrate servo controls
More logging, more comments
Merge branch 'master' into testing