File content as of revision 39:cc8691700d2a:

/**
 * @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()
{
    Timer       caltimer;
    FILE        *ps_file;
    double      dist;
    float       delta_a, angle, cal_time;
    size_t      bytes_read;
    int         corner, err, x, y, last_x, last_y, dim_x, dim_y, offset;
    char        instbuf[INST_BUF_SIZE];
    char        *cur, *next;
    angle       =   0;
    last_x      =   0;
    last_y      =   0;
    bytes_read  =   0;
    offset      =   0;
    int init = 0;

    /* First calibrate robot and figure out space dimensions. */
    robot_printf(0, "%f mV", pi.battery());

    /* Calibrate robot reflective sensors to line. */
    pi.sensor_auto_calibrate();
    timerWait(0.2);

    /* Find left bottom right corner of frame. */
    corner = find_corner(1);
    if (!corner) {
        robot_printf(0, "no left\n");
        while (1);
    }
    left(HALF_TURN);

    /*
     * Find bottom left corner of frame and
     * and measure length of frame side.
     */
    caltimer.start();
    corner = find_corner(0);
    if (!corner) {
        robot_printf(0, "no right\n");
        while (1);
    }
    caltimer.stop();
    cal_time = caltimer.read_ms();

    /* Back up into corner (coordinate x=0,y=0). */
    right(HALF_TURN);
    timerWait(0.2);
    backward(400);

    /* Open instruction file. */
    ps_file = fopen("/local/test.ps", "r");
    if (ps_file == NULL) {
        return 1;
    }

    /* Read beginning of file into buffer. */
    memset(instbuf, 0, INST_BUF_SIZE);
    bytes_read = fread(instbuf, sizeof(char), INST_BUF_SIZE-1, ps_file);
    if (bytes_read == 0) {
        return 1;
    }
    
    /* Read first line of file - the dimensions of the frame. */
    err = sscanf(instbuf, "%d/%d", &dim_x, &dim_y);
    if (err != 2) {
        robot_printf(0, "sscanf1");
        while(1);
    }
    cur = strchr(instbuf, '\n');
    cur++;
    offset = instbuf+INST_BUF_SIZE-cur-1;
    memcpy(instbuf, cur, offset);
    /*
     * File open and buffer setup. Begin reading instructions and
     * moving robot. Refill buffer after each instruction. 
     */
    while (1) {
        memset(instbuf+offset, 0, INST_BUF_SIZE-offset);
        bytes_read = fread(instbuf+offset, sizeof(char), INST_BUF_SIZE-1-offset, ps_file);
        if (instbuf[0] == '\0') {
            robot_printf(0, "%s", instbuf+1);
            oled_3 = 0;
            while(1);
        }
        
        cur = instbuf;
        err = retrieve_inst(instbuf, &x, &y, &draw);
        if (err == 0) {
            robot_printf(0, "noinst");
            return 1;
        }
        
        if (0 && !draw && init) {
            float pos;
            right(angle+QUARTER_TURN);
            find_line();
            forward(100);
            pi.left(CAL_SPEED);
            pos = pi.line_position();
            while (pos > CORRECTION_THRESHOLD || pos < 0) {
                pos = pi.line_position();
            }
            pi.stop();
            find_corner(1);
            left(180);
            find_corner(0);
            right(HALF_TURN);
            timerWait(0.5);
            backward(400); 
            angle = 0;
            last_x = 0;
            last_y = 0;
        }
        
        init = 1;
        /* Compute turn angle and turn. */
        delta_a = compute_turn_angle(last_x, last_y, x, y, angle);
        angle += delta_a;
        if (angle >= FULL_TURN) {
            angle -= FULL_TURN;
        }
        robot_printf(0, "a:%f", delta_a);
        left(delta_a);

        /* Put pen into position. */
        if (draw) {
            oled_3 = 1;
        } else {
            oled_3 = 0;
        }

        /* Compute drive time and move forward. */
        dist = fabs(distance(last_x, last_y, x, y));
        robot_printf(0, "d:%f", CAL_FACTOR*cal_time*(dist/(double)dim_x));
        forward(CAL_FACTOR*cal_time*(dist/(double)dim_x));
        last_x = x;
        last_y = y;
        oled_3 = 0;
        
        /* Seek to find next instruction. */
        next = strchr(cur, '\n');
        if (next == NULL) {
            robot_printf(0, "nonext");
            break;
        }
        cur = next+1;
        offset = instbuf+INST_BUF_SIZE-cur-1;
        memcpy(instbuf, cur, offset);
    }
    robot_printf(0, "Done");
    pi.stop(); 
    oled_3 = 0;
    while (1);
}

float compute_turn_angle(int last_x, int last_y, int x, int y, float angle)
{
    int delta_x, delta_y;
    float theta, delta_a;
    delta_x = x-last_x;
    delta_y = y-last_y;
    if (delta_y == 0) {
        if (delta_x < 0) {
            theta = HALF_TURN;
        } else {
            theta = 0;
        }
    } else if (delta_x == 0) {
        if (delta_y < 0) {
            theta = -QUARTER_TURN;
        } else {
            theta = QUARTER_TURN;
        }
    } else {
        theta = atan(((double) delta_y)/((double) delta_x));
        theta *= RAD_TO_DEG;
    }
    if (delta_x < 0 && delta_y > 0) {
        theta += HALF_TURN;
    }
    delta_a = theta-angle;

    if (delta_x < 0 && delta_y < 0) {
        delta_a += HALF_TURN;
    }
    
    if (delta_a > HALF_TURN) {
        delta_a -= FULL_TURN;
    } else if (delta_a < -HALF_TURN) {
        delta_a = FULL_TURN + delta_a;
    }
    return delta_a;
}

void robot_printf(int line, const char *format, ...)
{
    char buf[18];
    va_list args;
    pi.cls();
    pi.locate(0,line);
    va_start(args, format);
    vsprintf(buf, format, args);
    pi.printf("%s", buf);
    va_end(args);
}

float distance(int x1, int y1, int x2, int y2)
{
    return sqrt(pow((double) (x2-x1),2) + pow((double) (y2-y1), 2));
}

void find_line()
{
    float pos, pos1;
    pi.forward(DRIVE_SPEED);
    pos1 = pi.line_position();
    pos = pos1;
    while (fabs(pos1-pos) < 0.1 || pos == -1) {
        pos = pi.line_position();
        robot_printf(0, "%f\n", pos);
    }
    robot_printf(0, "f:%f\n", pos);
    pi.stop();
}    

int find_corner(int left)
{
    float pos;
    float threshold = CORNER_THRESHOLD;
    if (left) {
        threshold *= -1;
    }
    do {
        pos = pi.line_position();
        if(pos > CORRECTION_THRESHOLD) { 
            pi.right_motor(CAL_SPEED);
            pi.left_motor(CAL_SPEED-CORRECTION_SPEED);
        } else if(pos < -CORRECTION_THRESHOLD) {
            pi.left_motor(CAL_SPEED);
            pi.right_motor(CAL_SPEED-CORRECTION_SPEED);
        } else {
            pi.right_motor(CAL_SPEED);
            pi.left_motor(CAL_SPEED);
        }
        robot_printf(0, "Pos: %f", pos);
    } while(pos != -1 && ((left && pos > threshold) || (!left && pos < threshold)));
    pi.stop();
    if(pos == -1) {
        oled_1 = 1;
        return 0;
    }
    return 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) {
        robot_printf(0, "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 {
        robot_printf(0, "%.8s", srch);
        return 0;
    }
    return 1;
}

void forward(int amt)
{
    Timer t;
    float ms = 0;
    float last_ms = 0;
    robot_printf(0, "Fwd %d", amt);
    t.start();
    pi.left_motor(DRIVE_SPEED+.002);
    pi.right_motor(DRIVE_SPEED);
    ms = t.read_ms();
    while(ms < amt) {
        if (ms-last_ms > 500) {
            t.stop();
            right(1.5);
            last_ms = ms;
            t.start();
            pi.left_motor(DRIVE_SPEED+.002);
            pi.right_motor(DRIVE_SPEED);
        }
        ms = t.read_ms();
    }
    pi.stop();
}

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

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

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

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