void initRobotDetector( void ) {
registerInterrupt(ROBOT_DETECTOR_TIMER_INDEX, 2, detectsLaser);
currentMaxLeft = SERVO_LASER_MAX_LEFT_POSITION_DEFAULT_VALUE;
currentMaxRight = SERVO_LASER_MAX_RIGHT_POSITION_DEFAULT_VALUE;
lastServoPositionHit[0] = 0;
lastServoPositionHit[1] = 0;
oldServoPositionHit[0] = 0;
oldServoPositionHit[1] = 0;
hitDuringLast[0] = 0;
hitDuringLast[1] = 0;
lastNotifyObstacleTime = 0;
initPwmForServo(1500);
clearDistances();
}
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);
}
