File content as of revision 19:47759cf4f9b9:

/**
 * @file    driver.c
 * @brief   Basic driver program for our robot's controller logic. 
 *
 * Maybe add lots of stuff here or maybe split it off into
 * multiple subfiles?
 *
 * @author  John Wilkey - aw hells no! you ain't takin' credit for all this!
 */
#include "main.h"
#include "control.h"

/** 
 * These are global data Used externally in all other files 
 */
m3pi        pi;
Timer       timer;

//
// Digital inputs to the mBed
DigitalIn   start_button(p21);

//
// Digital outputs from the mBed. Note that by default these are
// used to drive the 8 LED's on the top board. 
DigitalOut  pin15(p15);
DigitalOut  pin16(p16);
DigitalOut  pin17(p17);
DigitalOut  pin18(p18);
DigitalOut  pin19(p19);
DigitalOut  pin20(p20);

// 
// mBed onboard LEDs
DigitalOut  oled_1(LED1);
DigitalOut  oled_2(LED2);
DigitalOut  oled_3(LED3);
DigitalOut  oled_4(LED4);

/* Local File System */
LocalFileSystem local("local");

/* Boolean for drawing/moving. */
int draw;

/**
 * @brief Entry point. Main loop.
 */
int main()
{
    FILE *ps_file;
    int instbuflen = 250;
    char instbuf[instbuflen];
    
    //
    // Basic setup information
    start_button.mode(PullUp);
        
    //
    // Drawing environment calibration.
    pi.sensor_auto_calibrate();
    //pi.backward(DRIVE_SPEED);
    float   pos         = 0;
    float   over_thresh = 0.05;
    float   correction  = 0.2*DRIVE_SPEED;
    float   cal_time;
    
    wait(1);

    do {
        pos = pi.line_position();
        if(pos > over_thresh) {
            pi.right_motor(-DRIVE_SPEED);
            pi.left_motor(-DRIVE_SPEED+correction);
        } else if(pos < -over_thresh) {
            pi.left_motor(-DRIVE_SPEED);
            pi.right_motor(-DRIVE_SPEED+correction);
        } else {
            pi.right_motor(-DRIVE_SPEED);
            pi.left_motor(-DRIVE_SPEED);
        }
        /*
        pi.cls();
        pi.locate(0,0);
        pi.printf("P: %f", pos);
        */
    } while (pos != -1 && pos <= 0.3);
    pi.stop();
    wait(1);
    if (pos != -1) {
        timer.start();
        pi.forward(DRIVE_SPEED);
    } else {
        pi.cls();
        pi.locate(0,0);
        pi.printf("LP: %f",pos);
        return 1;
    }

    wait(1);
    
    do {
        pos = pi.line_position();
        if(pos > over_thresh) {
            pi.right_motor(DRIVE_SPEED);
            pi.left_motor(DRIVE_SPEED-correction);
        } else if(pos < -over_thresh) {
            pi.left_motor(DRIVE_SPEED);
            pi.right_motor(DRIVE_SPEED-correction);
        } else {
            pi.right_motor(DRIVE_SPEED);
            pi.left_motor(DRIVE_SPEED);
        }
        pi.cls();
        pi.locate(0,0);
        pi.printf("Pos: %f", pos);
    } while(pos != -1 && pos <= 0.3);
    pi.stop();
    if(pos != -1) {
        oled_1 = 1;
        timer.stop();
        pi.printf("T: %d", timer.read_ms());
    } else {
        pi.cls();
        pi.locate(0,0);
        pi.printf("lP:%f", pos);
        return 1;
    }
    pi.right(TURN_SPEED);
    wait(0.5);
    pi.stop();
    do {
        pos = pi.line_position();
        if(pos > over_thresh) {
            pi.right_motor(-DRIVE_SPEED);
            pi.left_motor(-DRIVE_SPEED+correction);
        } else if(pos < -over_thresh) {
            pi.left_motor(-DRIVE_SPEED);
            pi.right_motor(-DRIVE_SPEED+correction);
        } else {
            pi.right_motor(-DRIVE_SPEED);
            pi.left_motor(-DRIVE_SPEED);
        }
        pi.cls();
        pi.locate(0,0);
        pi.printf("P: %f", pos);
    } while (pos != -1 && pos > -0.3);
    pi.stop();
    if(pos != -1) {
        oled_1 = 1;
        timer.stop();
        cal_time = timer.read();
        pi.printf("T: %d", timer.read_ms());
    } else {
        pi.cls();
        pi.locate(0,0);
        pi.printf("lP:%f", pos);
        return 1;
    }

    while(pi.line_position() == 1);
    do {
        pos = pi.line_position();
        if(pos > over_thresh) {
            pi.right_motor(DRIVE_SPEED);
            pi.left_motor(DRIVE_SPEED - correction);
        } else if(pos < -over_thresh) {
            pi.left_motor(DRIVE_SPEED);
            pi.right_motor(DRIVE_SPEED - correction);
        } else {
            pi.forward(DRIVE_SPEED);
        }
        pi.cls();
        pi.locate(0,0);
        pi.printf("Pos: %f", pos);
    } while(pos != -1 && pos != 1);
    if(pos == 1) {
        oled_1 = 1;
        timer.stop();
        pi.stop();
    } else {
        pi.stop();
        pi.cls();
        pi.locate(0,0);
        pi.printf("LP:%f", pos);
        while(1);
    }
    // If we got here, calibration is complete.
    
    //
    // Main program loop.
    // robot_loop();
    size_t bytes_read = 0;
    int err, x, y, last_x, last_y, delta_x, delta_y, delta_a;
    int dim_x, dim_y;
    int offset = 0;
    char *cur, *next;
    float angle;
    double dist, theta;
    angle = 0;
    theta = 0;
    last_x = 0;
    last_y = 0;
    ps_file = fopen("/local/test.ps", "r");
    /* PS parsing loop. */
    memset(instbuf, 0, instbuflen);
    bytes_read = fread(instbuf, sizeof(char), instbuflen, ps_file);
    err = sscanf(instbuf, "%d/%d", &dim_x, &dim_y);
    if (err != 2) {
        return 1;
    }
    cur = strchr(instbuf, '\n');
    while (1) {
        memset(instbuf+offset, 0, instbuflen-offset);
        bytes_read = fread(instbuf+offset, sizeof(char), instbuflen-1-offset, ps_file);
        if (bytes_read == 0) {
            break;
        }
        cur = instbuf;
        while (cur[0] != '\0') {
            err = retrieve_inst(instbuf, &x, &y, &draw);
            if (err == 0) {
                return 1;
            }
            delta_x = x-last_x;
            delta_y = y-last_y;

            /* Compute turn angle and turn. */
            theta = tan((double) delta_x/delta_y);
            theta *= 57.2957795;
            delta_a = theta-angle;
            if (delta_a > 0) {
                pi.left(TURN_SPEED);
            } else {
                pi.right(TURN_SPEED);
            }
            wait(0.5*(delta_a/360));

            /* Put pen into position. */
            if (draw) {
                oled_1 = 1;
            } else {
                oled_1 = 0;
            }
            /* Compute drive time and move forward. */
            dist = sqrt(pow((double) (delta_x),2) + pow((double) (delta_y), 2));
            if (dist < 0) {
                dist *= -1;
            }
            pi.forward(DRIVE_SPEED);
            wait(cal_time*(dist/dim_x));

            last_x = x;
            last_y = y;
            next = strchr(cur, '\n');
            if (next == NULL) {
                break;
            }
            cur = next+1;
        }
        offset = instbuf+instbuflen-cur;
        memcpy(instbuf, cur, offset);
    }
    //
    // We should never reach this point!
    //
    // Yes, we should...
    return 0;
}

int retrieve_inst(char *buf, int *x, int *y, int *draw)
{
    int matches;
    char *srch;
    matches = sscanf(buf, "%d %d", x, y);
    if (matches != 2) {
        return 0;
    }
    srch = strchr(buf, ' ');
    srch++;
    srch = strchr(buf, ' ');
    srch++;
    if (srch[0] == 'm') {
        *draw = 0;
    } else if (srch[0] == 'd') {
        *draw = 1;
    } else {
        return 0;
    }
    return 1;
}

int forward(int amt)
{
    Timer t;
    t.start();
    oled_2 = 1;
    pi.locate(0,0);
    pi.printf("Fwd %d", amt);
    pi.forward(DRIVE_SPEED);
    while(t.read_ms() < amt*DRIVE_RATE*1000);
    t.stop();
    oled_2 = 0;
    pi.stop();
    return EXIT_SUCCESS;
}

int backward(int amt)
{
    Timer t;
    oled_3 = 1;
    pi.locate(0,0);
    pi.printf("Back %d", amt);
    pi.backward(DRIVE_SPEED);
    t.start();
    while(t.read_ms() < amt*DRIVE_RATE*1000);
    t.stop();
    oled_3 = 0;
    pi.stop();
    return EXIT_SUCCESS;
}

int right(float deg)
{
    Timer t;
    oled_4 = 1;
    pi.locate(0,0);
    pi.printf("Right %f", deg);
    pi.right(TURN_SPEED);
    t.start();
    while(t.read_ms() < (deg/360)*1000);
    t.stop();
    oled_4 = 0;
    pi.stop();
    return EXIT_SUCCESS;
}

int left(float deg)
{
    Timer t;
    oled_4 = 1;
    oled_2 = 1;
    pi.locate(0,0);
    pi.printf("Left %f", deg);
    pi.left(TURN_SPEED);
    t.start();
    while(t.read_ms() < (deg/360)*1000);
    t.stop();
    oled_4 = 0;
    oled_2 = 0;
    pi.stop();
    return EXIT_SUCCESS;
}

void pen_down()
{
    oled_1 = 1;
}

void pen_up()
{
    oled_1 = 0;
}