File content as of revision 15:14d4e7021125:

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

/**
 * @brief Entry point. Main loop.
 */
int main()
{
    //
    // 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;

    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.2);
    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();
        pi.printf("T: %d", timer.read_ms());
    } else {
        pi.cls();
        pi.locate(0,0);
        pi.printf("lP:%f", pos);
        return 1;
    }
    //
    // Main program loop.
    // robot_loop();
    
    ps_file = fopen("/local/test.ps", "r");
    while () {
        memset(instbuf, 0, instbuflen);
        fread(instbuf, sizeof(char), instbuflen, ps_file);
    }
    //
    // We should never reach this point!
    return 0;
}

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