Fixed bugs and problems in the TeleOp
Merge remote-tracking branch 'origin/EmmasTeleOp'