Proper 2 wheel drive code

Adapted last years tele-op to two wheel drive.