add bump handling in visualServo and driveForward
Merge branch 'master' of https://github.com/rss-2014-team-6/rss-team-6-state-machine