Code cleanup
Merge branch 'movement' of github.com:alecjohanson/robot into movement
Stop before waiting