What does the correctPos ("CP" serial command) do? It seems to only get called on a collision, but I don't understand why it recalculates / resets the position based on the encoder only upon collision.
If a crash is detected then it uses the encoder value to reset or correct the position to what the encoder says it should be.