File content as of revision 30:3211e2962441:

/**
 * @file    driver.c
 * @brief   Primary control logic for robot.
 *
 * @author  John Wilkey
 * @author  Alec Guertin
 * @author  Chester Chu
 */
#include "main.h"

m3pi        pi;                 /**< m3pi Object for all control operations. */
Timer       timer;              /**< Timer used to wait in timerWait. */
DigitalIn   start_button(p21);  /**< Pi start button input on mBed pin 32 */
int         draw;               /**< Boolean for drawing/moving. */

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

/* 8 LEDs driven from mBed digital outs. */
DigitalOut  pin13(p13), pin14(p14), pin15(p15), pin16(p16),
            pin17(p17), pin18(p18), pin19(p19), pin20(p20);         

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

/** @brief Entry point. Main loop. */
int main()
{
    FILE    *ps_file;
    float   cal_time;
    float   pos         = 0;
    float   over_thresh = 0.05;
    float   correction  = 0.2*DRIVE_SPEED;
    int     instbuflen  = 250;
    char    instbuf[instbuflen];
 
    start_button.mode(PullUp);
    pi.cls();
    pi.locate(0,0);
    pi.printf("Ready");
    pi.locate(0,1);
    pi.printf("%f mV", pi.battery());
    wait(.5);
    pi.sensor_auto_calibrate();
    
    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) {
        timer.stop();
    } else {
        return 1;
    }
    right(180);
    Timer caltimer;
    caltimer.start();
    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);
    caltimer.stop();
    cal_time = caltimer.read_ms();
    pi.stop();
    if(pos != -1) {
        timer.stop();
    } else {
        return 1;
    }
    right(180);
    timerWait(.5);
    while(fabs(pos = pi.line_position()) > CLOSE_ENOUGH) {
        pi.right((pos < 0 ? -.6*CAL_SPEED : .6*CAL_SPEED));
        pi.cls();
        pi.locate(0,0);
        pi.printf("O: %f", pos);
    }
    pi.stop();    
    timerWait(0.5);
    backward(500);
    
    /* Maybe uncomment this depending on the surface.
    timerWait(.5);
    while(fabs(pos = pi.line_position()) > CLOSE_ENOUGH) {
        pi.right((pos < 0 ? -.6*CAL_SPEED : .6*CAL_SPEED));
        pi.cls();
        pi.locate(0,0);
        pi.printf("O: %f", pos);
    }
    pi.stop();
    */
    
    timerWait(.6);

    // Main program loop.
    size_t bytes_read = 0;
    int err, x, y, last_x, last_y, delta_x, delta_y;
    float 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;
    }

    /* PS parsing loop. */
    memset(instbuf, 0, instbuflen);
    bytes_read = fread(instbuf, sizeof(char), instbuflen-1, ps_file);
    if (bytes_read == 0) {
        return 1;
    }
    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-1;
    memcpy(instbuf, cur, offset);
    while (1) {
        timerWait(.18);
        memset(instbuf+offset, 0, instbuflen-offset);
        bytes_read = fread(instbuf+offset, sizeof(char), instbuflen-1-offset, ps_file);
        
        if (instbuf[0] == '\0') {
            pi.cls();
            pi.locate(0,0);
            pi.printf("%s", instbuf+1);
            while(1);
        }
        
        cur = instbuf;
        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 = atan(((double) delta_y)/((double) delta_x));
        theta *= 57.29;
        if (delta_x < 0 && delta_y > 0) {
            theta += 180;
        }
        delta_a = theta-angle;
        if (delta_x < 0 && delta_y < 0) {
            delta_a += 180;
        }
        if (delta_a > 180) {
            delta_a -= 360;
        }
        if (delta_a < -180) {
            delta_a = 360 + delta_a;
        }
        angle += delta_a;
        if (angle >= 360) {
            angle -= 360;
        }
        //pi.cls();
        //pi.locate(0,0);
        //pi.printf("a:%f", delta_a);
        //timerWait(1.5);
        left(delta_a);

        /* Put pen into position. */
        if (draw) {
            oled_3 = 1;
        } else {
            oled_3 = 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.cls();
        //pi.locate(0,0);
        //pi.printf("d:%f", 0.4*cal_time*(dist/(double)dim_x));
        //timerWait(1.5);
        forward(0.4*cal_time*(dist/(double)dim_x));

        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;
        offset = instbuf+instbuflen-cur-1;
        memcpy(instbuf, cur, offset);
    }
    pi.cls();
    pi.locate(0,0);
    pi.printf("Done");
    while (1);
}

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;
}

void forward(int amt)
{
    Timer t;
    pi.locate(0,0);
    pi.printf("Fwd %d", amt);
    t.start();
    pi.left_motor(DRIVE_SPEED+.0023);
    pi.right_motor(DRIVE_SPEED);
    while(t.read_ms() < amt);
    t.stop();
    pi.stop();
}

void backward(int amt)
{
    Timer t;
    t.start();
    pi.backward(.5*DRIVE_SPEED);
    while(t.read_ms() < (amt-500));
    pi.backward(.5*DRIVE_SPEED);
    while(t.read_ms() < amt);
    t.stop();
    pi.stop();
}

void right(float deg)
{
    if(deg < 0) {
        return left(-1*deg);
    }
    Timer t;
    float amt = ((float)(deg/360)*TIME_FACT);
    t.start();
    pi.right(TURN_SPEED);
    while(t.read_ms() < amt);
    t.stop();
    pi.stop();
}

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

void left(float deg)
{
    if(deg < 0) {
        return right(-1*deg);
    }
    Timer t;
    pi.left(TURN_SPEED);
    t.start();
    while(t.read_ms() < ((float)(deg/360)*TIME_FACT));
    t.stop();
    pi.stop();
}

void pen_down()
{
    oled_1 = 1;
}

void pen_up()
{
    oled_1 = 0;
}