Fix bugs
Fix variable typo, refactor joystick code
Fix incorrect motor variable reference
Add motortest program