TeleOp with drive motor control finished