Fixed PID calculation and added getEncoderRate() method