File content as of revision 21:0c80a5d89ea3:

/**
 * @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);
        
    start:
    pi.cls();
    pi.locate(0,0);
    pi.printf("PiCO");
    pi.locate(0,1);
    pi.printf("%f mV", pi.battery());
    wait(.5);
    /* while(start_button) {
        oled_2 = 1;
        wait(.5);
        pi.locate(0,0);
        pi.printf("Ready");
        oled_2 = 0;
    } /**/
    pi.cls();
    pi.locate(0,0);
    // pi.printf("GO!");
    wait(.5);

    /*
    while(1) {
        if(!start_button) {
            pi.stop();
            goto start;
        } else {
            goto calibrate;    
        }
    }
    /**/
        
    //
    // Drawing environment calibration.
    int cal_count = 1;
    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.left_motor(-CAL_SPEED);
            pi.right_motor(-CAL_SPEED+correction);
        } else if(pos < -over_thresh) {
            pi.right_motor(-CAL_SPEED);
            pi.left_motor(-CAL_SPEED+correction);
        } else {
            pi.backward(CAL_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(CAL_SPEED);
    } else {
        pi.cls();
        pi.locate(0,0);
        pi.printf("LP: %f",pos);
        forward(1000);
        goto calibrate;
        return 1;
    }
    /**/
    
    // wait(1);
    
    do {
        pos = pi.line_position();
        if(pos > over_thresh) {
            pi.right_motor(CAL_SPEED);
            pi.left_motor(CAL_SPEED-correction);
        } else if(pos < -over_thresh) {
            pi.left_motor(CAL_SPEED);
            pi.right_motor(CAL_SPEED-correction);
        } else {
            pi.right_motor(CAL_SPEED);
            pi.left_motor(CAL_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;
    }
    right(180);
    wait(1);
    do {
        pos = pi.line_position();
        if(pos > over_thresh) {
            pi.right_motor(CAL_SPEED);
            pi.left_motor(CAL_SPEED-correction);
        } else if(pos < -over_thresh) {
            pi.left_motor(CAL_SPEED);
            pi.right_motor(CAL_SPEED-correction);
        } else {
            pi.right_motor(CAL_SPEED);
            pi.left_motor(CAL_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;
    }
    right(180);
    timerWait(.07);
    pos = pi.line_position();
    while(fabs(pos) > CLOSE_ENOUGH) {
        pi.right((pos < 0 ? -CAL_SPEED : CAL_SPEED));
        timerWait(.08);
        pi.cls();
        pi.locate(0,0);
        pi.printf("O: %f", pos);
        pi.stop();
        pos = pi.line_position();
        timerWait(.2);
    }
    timerWait(2);
    
    //
    // Pivot 180 degrees to go to the starting position.
    /*
    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.backward(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.cls();
        pi.locate(0,0);
        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");
    if (ps_file == NULL) {
        return 1;
    }
    pi.cls();
    pi.locate(0,0);
    pi.printf("open");
    /* PS parsing loop. */
    memset(instbuf, 0, instbuflen);
    bytes_read = fread(instbuf, sizeof(char), instbuflen-1, ps_file);
    pi.cls();
    pi.locate(0,0);
    pi.printf("%.7s", instbuf);
    err = sscanf(instbuf, "%d/%d", &dim_x, &dim_y);
    if (err != 2) {
        pi.cls();
        pi.locate(0,0);
        pi.printf("sscanf1");
        return 1;
    }
    cur = strchr(instbuf, '\n');
    cur++;
    offset = instbuf+instbuflen-cur;
    memcpy(instbuf, cur, offset);
    while (1) {
        memset(instbuf+offset, 0, instbuflen-offset);
        bytes_read = fread(instbuf+offset, sizeof(char), instbuflen-1-offset, ps_file);
        if (bytes_read == 0) {
            pi.cls();
            pi.locate(0,0);
            pi.printf("bytes0");
            break;
        }
        cur = instbuf;
        while (cur[0] != '\0') {
            err = retrieve_inst(instbuf, &x, &y, &draw);
            if (err == 0) {
                pi.cls();
                pi.locate(0,0);
                pi.printf("noinst");
                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;
            pi.cls();
            pi.locate(0,0);
            pi.printf("%d", delta_a);
            if (delta_a > 0) {
                left(delta_a);
            } else {
                right(delta_a);
            }
          
            /* 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;
            }
            timerWait(.2);
            forward(1500);
            right(90);
            timerWait(.2);
            forward(1500);
            right(90);
            timerWait(.2);
            forward(1500);
            right(90);
            timerWait(.2);
            forward(1500);
            right(90);
            timerWait(.2);

            last_x = x;
            last_y = y;
            next = strchr(cur, '\n');
            if (next == NULL) {
                pi.cls();
                pi.locate(0,0);
                pi.printf("nonext");
                break;
            }
            cur = next+1;
            pi.cls();
            pi.locate(0,0);
            pi.printf("waiting!");
        }
        offset = instbuf+instbuflen-cur;
        memcpy(instbuf, cur, offset);
    }
    //
    // We should never reach this point!
    //
    // Yes, we should...
    pi.cls();
    pi.locate(0,0);
    pi.printf("done");
    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) {
        pi.cls();
        pi.locate(0,0);
        pi.printf("nomatch");
        return 0;
    }
    srch = strchr(buf, ' ');
    srch++;
    srch = strchr(srch, ' ');
    srch++;
    if (srch[0] == 'm') {
        *draw = 0;
    } else if (srch[0] == 'l') {
        *draw = 1;
    } else {
        pi.cls();
        pi.locate(0,0);
        pi.printf("%.8s", srch);
        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.left_motor(DRIVE_SPEED+.0023);
    pi.right_motor(DRIVE_SPEED);
    while(t.read_ms() < (amt-500));
    pi.left_motor(.7*DRIVE_SPEED+.0023);
    pi.right_motor(.7*DRIVE_SPEED);
    while(t.read_ms() < amt);
    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);
    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)*TIME_FACT);
    t.stop();
    oled_4 = 0;
    pi.stop();
    return EXIT_SUCCESS;
}

void timerWait(float seconds)
{
    Timer t;
    t.start();
    while(t.read_us() < 1000000*seconds);
    t.stop();
}

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)*TIME_FACT);
    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;
}