File content as of revision 15:151e59899221:

#include "drivecontrol.h"
//#include "pin_assignment.h"
#include "io_modules.h"
#include "mbed.h"

// Battery Debugger Interface
AnalogIn battery(PA_3);


// Led Debugger Interface
DigitalOut led_1(PB_12);
DigitalOut led_2(PB_13);
DigitalOut led_3(PB_14);
DigitalOut led_4(PB_15);

// System io
Serial serial (PA_9, PA_10);

// Define states for debugging the mouse hardware
// const int DRIVE = 1, TURN = 2, DEBUG = 3, STOP = 4;
// Direction of which to turn
// const int LEFT = 0, RIGHT = 1;
// Start and End Pos
const int STRAIGHT = 0, LEFT = 1, RIGHT = 2;
const int START_POS = 0, END_POS = 0;

// Terminating condition for the main control loop
bool hasFoundCenter = false;

// Battery Consumption Indicator
void setup() {
    // 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;
    }
}

// Performs the basic drive control of the mouse
int main() {
    DriveControl * driver = new DriveControl (START_POS, END_POS);
    driver->resetEncoders();
    int state;
    bool finished_traverse_one_cell = true;
    while(1) {
        setup();
        // switch state
        wait_ms(1);
        if (finished_traverse_one_cell == true) {
            if (!driver->has_front_wall()) {
                state = STRAIGHT;
                driver->resetEncoders();
            }
            else if (!driver->has_left_wall()) {
                state = LEFT;  
                driver->resetEncoders();  
            }
            else {
                state = RIGHT;
                driver->resetEncoders();
            }
            finished_traverse_one_cell = false;
         
        /*
            if (!driver->has_left_wall()) {
                state = LEFT;  
                driver->resetEncoders();  
            }
            else if (!driver->has_right_wall()) {
                state = RIGHT;  
                driver->resetEncoders();  
            }
            else {
                state = STRAIGHT;
                driver->resetEncoders();
            }
            finished_traverse_one_cell = false;*/
        }
        
        if (state == STRAIGHT) {
            if (!driver->has_front_wall()) {
                driver->drive_forward();
            }
            else {
                driver->stop();
                wait(0.5);
                led_1 = 1;
                led_2 = 1;
                led_3 = 0;
                    led_4 = 0;
                    finished_traverse_one_cell = true;
                    continue;
            }
        }
        if (state == RIGHT) {
            if (!driver->should_finish_turn_right()) {
                driver->turn_right();
            }
            else {
                driver->stop();
                wait(0.5);
                led_1 = 0;
                led_2 = 1;
                led_3 = 1;
                led_4 = 0;
                finished_traverse_one_cell = true;
                continue;
            }
        }
        if (state == LEFT) {
            if (!driver->should_finish_turn_left()) {
                driver->turn_left();
            }
            else {
                driver->stop();
                wait(0.5);
                led_1 = 0;
                led_2 = 0;
                led_3 = 1;
                led_4 = 1;
                finished_traverse_one_cell = true;
                continue;
            }
        } 
        
    }
}