File content as of revision 13:070846d87d4a:

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


/**
 * @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;
    float   over_thresh = 0.5;
    float   correction  = 0.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("P: %f", pos);
    } while(pos != -1 && pos != 1);
    if(pos == 1) {
        timer.start();
        pi.forward(DRIVE_SPEED);
    } else {
        pi.stop();
        pi.cls();
        pi.locate(0,0);
        pi.printf("LP: %f",pos);
        while(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);
    }
    
    //
    // Main program loop.
    // robot_loop();
    
    //
    // We should never reach this point!
    while(1);
}

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