fix issue with mode switching between velociy and postion
Merge pull request #3 from rethink-cgindel/fix_mode_switching

Makes perfect sense, thanks!

fix issue with mode switching between velocity and position