void updateTrajectoryAndClearCoders() {
updateTrajectoryWithNoThreshold();
lastAngle = position.orientation - position.initialOrientation;
clearCoders();
lastLeft = 0;
lastRight = 0;
}
void initCoders(void) {
// PortB as inputs for coders
CODERS_TRIS = 0xFF;
CODERS_PORT = 0xFF;
canal = CODERS_PORT;
CODERS_MASKA[0] = CODERS_MASKA_1;
CODERS_MASKB[0] = CODERS_MASKB_1;
CODERS_MASKZ[0] = CODERS_MASKZ_1;
CODERS_MASKA[1] = CODERS_MASKA_2;
CODERS_MASKB[1] = CODERS_MASKB_2;
CODERS_MASKZ[1] = CODERS_MASKZ_2;
clearCoders();
registerInterrupt(CODER_TIMER_INDEX, 0, updateCoders);
}