Fix bugs
Fix variable typo, refactor joystick code
Fix incorrect motor variable reference
Add motortest program
Add SimpleTeleOp program
Merge branch 'master' into simpleTeleop