Moved encoder sampling to a timer ISR function at 1 KHZ
Tuned PID gains. Need to add antiwindup