Moved the offset system into the PID controller
tweaking the encoders
Changed teh encoder reading to add the 500us delay between reads