Updated the encoder and pid subsystem to drive wheel at given speed.

Encoder reads bogus values which prevent the wheel from driving smoothly.