int main(int argc, char *argv[]) {
//led_set_blue(ledb_vals[0] | ledb_vals[1]);
//button_wait(0);
led_set_blue(ledb_vals[2] | ledb_vals[3]);
//ir_sender_setup();
//ir_sender_set((ROOMBA_ID&0x03)<<4 | IR_SENDER_ID | IR_LED_LEFT, (ROOMBA_ID&0x03)<<4 | IR_SENDER_ID | IR_LED_CENTER, (ROOMBA_ID&0x03)<<4 | IR_SENDER_ID | IR_LED_RIGHT, (ROOMBA_ID&0x03)<<4 | ir_next_base_id | IR_LED_NEAR);
IOWR32(A_IR_SENDER, 0x4, base_id_codes[base_id-1] | 0x5);
//IOWR32(A_IR_SENDER, 0x4, 0xF8F4F2F1);
//ir_sender_set(0x80 | IR_LED_RIGHT, 0x80 | IR_LED_CENTER, 0x80 | IR_LED_LEFT, 0x80 | IR_LED_NEAR);
ir_sender_on();
while(true) {
ir_sender_off();
my_msleep(500);
ir_sender_on();
my_msleep(50);
}
return 0;
}
void handleOutOfCourse(detectedType activeSensorSide) {
// Detect side on which we drive out
if (activeSensorSide == BORDER_FRONT_LEFT && !drive_in) {
side = 0;
} else if (activeSensorSide == BORDER_FRONT_RIGHT && !drive_in) {
side = 1;
}
// Drive back to line
else if (activeSensorSide == BORDER_BOTH && !drive_in && step == 0) {
// When mushroom is active, we are allowed to drive out
if (mushroomActive && outsideCourse == 0) {
// We drove outside
outsideCourse = 1;
// Wait a bit to drive over crep
my_msleep(1000);
} else if (mushroomActive && outsideCourse == 1) {
// We are driving back inside
outsideCourse = 0;
my_msleep(1000);
} else {
driveIn(RIGHT_WHEEL);
}
} else if ((activeSensorSide == BORDER_RIGHT || activeSensorSide == BORDER_SIDE_BOTH) && step == 0 && drive_in) {
step = 1;
driveIn(LEFT_WHEEL);
} else if ((activeSensorSide == BORDER_LEFT || activeSensorSide == BORDER_SIDE_BOTH) && drive_in && step == 1) {
step = 2;
driveIn(RIGHT_WHEEL);
}
// Drive back to middle of the road
else if (activeSensorSide == BORDER_SIDE_BOTH && step == 2 && drive_in) {
driveInContinued(activeSensorSide);
}
}
void driveIn(uint8_t wheel) {
drive_in = 1;
// Stop roomba
drive_stop();
// Turn the roomba till sensor reaches line
if (wheel == LEFT_WHEEL) {
drive_direction(-DRIVE_STRAIGHT_SPEED, 0);
} else {
drive_direction(0, -DRIVE_STRAIGHT_SPEED);
}
my_msleep(350);
}
uint8_t getRoombaNrFromRemote() {
while (1) {
uint8_t result = read_user_input();
//TODO for testing
//uint16_t result = 1;
// Error, when not 1 or 2
if (result != 1 && result != 2) {
char result[4] = {'1', 'O', 'R', '2'};
set_Display(result);
my_msleep(2000);
} else {
return result;
}
}
}
