Fixed encoder operation code

Updated code for the motor encoders so that they work now.