Parameterized and cleaned up
Reset Kp to 1, average encoder pairs, manage encoder rollover