File content as of revision 46:a3b9c0fe8f36:

/**
 * @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");

/* States specifying the robot's behavior. */
enum RobotState {
    ERROR,
    LINE_CAL,
    FIND_CORNER,
    REV_TURN,
    DRAW_INIT,
    PARSE,
    TURN,
    MOVE,
    DRAW,
    FINISHED,
};

/** @brief Entry point. Main loop. */
int main()
{
    Timer       caltimer;
    FILE        *ps_file;
    RobotState  state, last_state;
    double      dist;
    float       delta_a, angle, cal_time, pos, last_pos;
    size_t      bytes_read;
    int         corner, err, init, x, y, last_x, last_y, dim_x, dim_y, offset, right_found;
    char        instbuf[INST_BUF_SIZE];
    char        *cur, *next;
    angle       =   0;
    x           =   0;
    y           =   0;
    last_x      =   0;
    last_y      =   0;
    bytes_read  =   0;
    offset      =   0;
    init        =   0;
    right_found =   0;
    delta_a     =   0;
    angle       =   0;
    cal_time    =   0;
    pos         =   0;
    last_pos    =   0;
    cur         =   NULL;
    next        =   NULL;
    ps_file     =   NULL;
    caltimer    =   Timer();
    
    /*
     * File open and buffer setup. Begin reading instructions and
     * moving robot. Refill buffer after each instruction. 
     */
    state = LINE_CAL;
    last_state = LINE_CAL;
    while (1) {
        switch (state) {

            // Calibrate the m3pi line sensors
            case LINE_CAL:
                pi.sensor_auto_calibrate();
                timerWait(0.2);
                last_state = state;
                state = FIND_CORNER;
                break;

            // Find a corner of the frame
            case FIND_CORNER:
                last_state = state;
                // Corner we are looking for specified by right_found
                // If we already found the right corner, measure frame
                if (right_found) {
                    caltimer.start();
                }
                corner = find_corner(1-right_found);
                if (!corner) {
                    state = ERROR;
                    break;
                }
                if (right_found) {
                    caltimer.stop();
                    cal_time = caltimer.read_ms();
                }
                state = REV_TURN;
                break;

            // Reverse robot to face other corner of frame
            case REV_TURN:
                last_state = state;
                if (right_found) {
                    right(HALF_TURN);
                    state = DRAW_INIT;
                } else {
                    left(HALF_TURN);
                    state = FIND_CORNER;
                    right_found = 1;
                }
                break;

            // Setup for drawing stage
            case DRAW_INIT:
                // Backup to start at (0,0)
                timerWait(0.2);
                backward(400);
                last_state = state;
            
                // Open the instructions file
                ps_file = fopen("/local/control.ps", "r");
                if (ps_file == NULL) {
                    state = ERROR;
                    break;
                }
            
                // Read start 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) {
                    state = ERROR;
                    break;
                }

                // Find dimensions of PS instructions
                err = sscanf(instbuf, "%d/%d", &dim_x, &dim_y);
                if (err != 2) {
                    state = ERROR;
                    break;
                }
                cur = strchr(instbuf, '\n');
                cur++;
                offset = instbuf+INST_BUF_SIZE-cur-1;
                memcpy(instbuf, cur, offset);
                state = PARSE;
                break;

            // Retrieve the next instruction
            // Update robot vars to give desired position
            case PARSE:
                // Parsing uses in place buffer
                // next instruction is at start of buffer
                last_state = state;
                if (init) {
                    // Advance to next instruction
                    next = strchr(cur, '\n');
                    if (next == NULL) {
                        state = ERROR;
                        break;
                    }
                    cur = next+1;
                    offset = instbuf+INST_BUF_SIZE-cur-1;
                    memcpy(instbuf, cur, offset);
                }
                init = 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') {
                    pen_up();
                    state = FINISHED;
                    break;
                }

                // Update robot position
                last_x = x;
                last_y = y;

                // Get next instruction
                cur = instbuf;
                err = retrieve_inst(instbuf, &x, &y, &draw);
                if (err == 0) {
                    state = ERROR;
                    break;
                } else {
                    state = TURN;
                    break;
                }
                
            // Move angle needed to reach desired position
            case TURN:
                last_state = state;

                // Computed necessary angle                
                delta_a = compute_turn_angle(last_x, last_y, x, y, angle);

                // Update robot angle
                angle += delta_a;
                if (angle >= FULL_TURN) {
                    angle -= FULL_TURN;
                }
                if (angle <= -FULL_TURN) {
                    angle += FULL_TURN;
                }

                // Move desired angle
                left(delta_a);
                if (draw) {
                    state = DRAW;
                    break;
                } else {
                    state = MOVE;
                    break;
                }

            // Move to desired position while drawing
            case DRAW:
                // Compute Euclidean distance between points
                dist = fabs(distance(last_x, last_y, x, y));
                pen_down();
                // Move desired distance
                forward(CAL_FACTOR*cal_time*(dist/(double)dim_x));
                pen_up();
                last_state = state;
                state = PARSE;
                break;

            // Move to desired position w/o drawing
            // (lifting pen and starting somewhere else)
            case MOVE:
                // Compute Euclidean distance
                dist = fabs(distance(last_x, last_y, x, y));
                // Move desired distance
                forward(CAL_FACTOR*cal_time*(dist/(double)dim_x));
                last_state = state;
                state = PARSE;
                break;

            // Done drawing - alert user
            case FINISHED:
                pi.stop(); 
                pen_up();
                robot_printf(0, "done");
                while(1);

            // Error - alert user
            case ERROR:
                pi.stop(); 
                pen_up();
                robot_printf(0, "E:%d", last_state);
                while(1);

            // Unrecognized state - go to ERROR
            default:
                last_state = state;
                state = ERROR;
                break;
        }
    }
}

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", pos);
    }
    robot_printf(0, "f:%f", 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);
}

void pen_down()
{
    oled_3 = 1;
}

void pen_up()
{
    oled_3 = 0;
}