File content as of revision 18:85196734207e:

#include "drivecontrol.h"
#include "io_modules.h"
#include "mbed.h"
//// PIN ASSIGNMENTS
AnalogIn battery(PA_3);
DigitalOut led_1(PB_12);
DigitalOut led_2(PB_13);
DigitalOut led_3(PB_14);
DigitalOut led_4(PB_15);
Serial serial (PA_9, PA_10);
// CONTROL CONSTANTS
const int STRAIGHT = 0, LEFT = 1, RIGHT = 2, UTURN = 3, REVERSE  = 4;
const int START_POS = 0, END_POS = 0;
const int CONTROL = 1;

int state, count = 0;
DriveControl * driver;
bool finished_traverse_one_cell = true;
Timer t;

void check_battery () {
    // pc.baud(9600);
    // using the serial functions will have an impact on timing.
    // serial.printf("voltage value is: %3.3f%%\r\n", battery.read()*100.0f);
    // serial.printf("normalized: 0x%04X \r\n", battery.read_u16());
    if (battery.read() < 0.70f){
        // flash led
        led_1 = 1;
        led_2 = 1;
        led_3 = 1;
        led_4 = 1;
    }
}

void flash_led (int _led_1, int _led_2, int _led_3, int _led_4) {
    led_1 = _led_1;
    led_2 = _led_2;
    led_3 = _led_3;
    led_4 = _led_4;
}

int get_next_state() {
    if (finished_traverse_one_cell == true) {
            t.reset();
            if (!driver->has_right_wall()) {
                state = RIGHT;
                driver->resetEncoders();
                wait(1);  
                t.start();
            }
            else if (!driver->has_left_wall()) {
                state = LEFT;
                driver->resetEncoders();   
                wait(1);
                t.start();
            }
            else if(!driver->has_front_wall()){
                state = STRAIGHT;
                driver->resetEncoders();
                t.start();
            }
            else{
                state = UTURN;
                driver->resetEncoders();
                count = 0;
                wait(1);
                t.start();
            }
            finished_traverse_one_cell = false;
        }
    return state;
}

void print() {
    wait(2);
    driver->print_serial_ports();
}


int main() {
    driver = new DriveControl (START_POS, END_POS);
    driver->resetEncoders();
    //wait(1.5);
    while(CONTROL) {
        check_battery();
        wait_ms(1);
        // Check if the mouse has been stucked on the wall
        float curr_time = t.read();
        /*
        if (curr_time > 5 && driver->should_reverse()) {
            if (curr_time < 1) {
                state = REVERSE;
                driver->reverse();
            }
            else {
                state = STRAIGHT;
            }
        } 
        if (state != REVERSE) {
            state = get_next_state();
        } */
        
        state = get_next_state();
        if (state == STRAIGHT) {
            if (!driver->should_finish_drive_forward() && !driver-> has_front_wall()) {
                driver->drive_forward();
                flash_led(1, 0, 0, 0);
            }
            else {
                driver->clear_pid();
                driver->stop();
                driver->resetEncoders();
                // wait(0.2);
                flash_led (0, 0, 0, 0);
                finished_traverse_one_cell = true;
                continue;
            }
        }
        if (state == RIGHT) {
            if (!driver->should_finish_turn_right()) {
                driver->turn_right();
                flash_led (0, 1, 0, 0);
            }
            else {
                driver->stop();
                wait(0.2);
                flash_led (0, 0, 0, 0);
                //finished_traverse_one_cell = true;
                state = STRAIGHT;
                driver->resetEncoders();
                driver->clear_pid();
                continue;
            }
        }
        if (state == LEFT) {
            if (!driver->should_finish_turn_left()) {
                driver->turn_left();
                flash_led (0, 0, 1, 0);
            }
            else {
                driver->stop();
                wait(0.2);
                flash_led (0, 0, 0, 0);
                //finished_traverse_one_cell = true;
                state = STRAIGHT;
                driver->resetEncoders();
                driver->clear_pid();
                continue;
            }
        }
        
        if (state == UTURN){
            if (!driver->should_finish_turn_left()) {
                driver->turn_left();
                flash_led (0, 1, 1, 0);
            }
            else {
                driver->stop();
                driver->resetEncoders();
                wait(0.2);
                flash_led (0, 0, 0, 0);
                count == 1 ? finished_traverse_one_cell = true: count++;
                continue;
            }
        } 
    }
}


/*

    if (!driver->has_front_wall()) {
            if (!driver->should_finish_drive_forward()) {
                driver->drive_forward();
                flash_led(0, 1, 1, 0);
            }
            else {
                driver->stop();
                wait(1);
                flash_led (0,0,0,0);
                driver->resetEncoders();
                finished_traverse_one_cell = true;
                continue;
            }
        }
        else {
            driver->stop();
            driver->resetEncoders();
            flash_led (1, 1, 1, 0);
        }




*/