void correctRobot() {
if(e_get_calibrated_prox(S_LEFT) > 10) {
correctRight = 0;
e_set_speed_left(100);
e_set_speed_right(-100);
while(e_get_calibrated_prox(S_FRONT_LEFT) > 20);
waitABit();
e_set_speed_left(700);
e_set_speed_right(700);
waitABit();
e_set_speed_left(0);
e_set_speed_right(0);
}else if(e_get_calibrated_prox(S_RIGHT) > 10) {
correctRight = 1;
e_set_speed_left(-100);
e_set_speed_right(100);
while(e_get_calibrated_prox(S_FRONT_RIGHT) > 20);
waitABit();
e_set_speed_left(700);
e_set_speed_right(700);
waitABit();
e_set_speed_left(0);
e_set_speed_right(0);
}else if(e_get_calibrated_prox(S_FRONT_LEFT) > 100 || e_get_calibrated_prox(S_FRONT_RIGHT) > 100) {
e_set_speed_left(-300);
e_set_speed_right(-300);
waitABit();
e_set_speed_left(0);
e_set_speed_right(0);
}
}
void initLFXT() {
DIO->rPJSEL0 |= 0x0003;
DIO->rPJSEL1 &= ~0x0003;
//unlock clock control registers
CS->rKEY.b.bKEY = 0x695A;
waitABit();
CS->rCTL3.b.bFCNTLF_EN = 0;
CS->rCTL2.b.bLFXT_EN = 1;
CS->rCLRIFG.b.bCLR_FCNTLFIFG = 1;
CS->rCLRIFG.b.bCLR_LFXTIFG = 1;
waitABit();
CS->rCLRIFG.b.bCLR_LFXTIFG = 1;
waitABit();
if (CS->rIFG.b.bLFXTIFG) {
while (1) {
;
}
}
CS->rCTL1.b.bSELA = 0;
//lock clock control registers (this can be anything except 0x695A)
CS->rKEY.b.bKEY = 0xBEEF;
}