File content as of revision 0:e9d343cd983c:

#include "mbed.h"
#include "Servo.h"
#include "SRF05.h"

Servo motor (p21);
Servo turn (p22);

SRF05 front (p24,p23);
SRF05 back (p26,p25);

DigitalOut b (p20);
DigitalOut led1 (LED1);
DigitalOut led2 (LED2);

LocalFileSystem local("local");
InterruptIn w(p16);
Timer timer;

void backward (float);
void forward (float);
void right (void);
void left (void);
void reset (void);
void stop (void);


//servo vars
int i = 0;
int j = 0;

//timer/sensor vars
int count;
int begin;
int end;
int single_spin;
//const int wheel = 257; //in mm
//int Tdist = 0;

//intrupt program exicuted
void trigger() {

    end = timer.read_ms(); //read timer
    count = count + 1; //add to total turn count
    single_spin = end - begin; //calculate total time for 1 spin

    FILE *fp = fopen("/local/out.csv", "a"); //open file in append
    fprintf(fp, "%d, %d\n", count, single_spin);// add data to file
    fclose(fp); //colse file

    begin = timer.read_ms(); // reset timer


}


int main() {

    timer.start(); //start timer
    w.rise(&trigger); //set trigger

    b = 0;
    reset();

    //main loop
    while (1) {

        //init
        i = front.read();
        j = back.read();


        //can go forward?
        if (i > 100) {
            while (i > 100) {

                forward(20);
                i = front.read();
                j = back.read();
            }

            stop();
        }

        //can it turn to avoid?
        if (i > 10 && i < 100) {
            led1 = 1;
            while (i > 10 && i < 100) {

                forward(20);
                turn.write(0);

                i = front.read();
                j = back.read();
            }
            led1 = 0;
            stop();
        }

        //is there room to ververse and turn?
        if (i < 20 && j > 10) {
            led2 = 1;
            while (i < 20 && j > 10) {

                backward(20);
                turn.write(1);

                i = front.read();
                j = back.read();
            }
            led2 = 0;
            stop();
        }
    }
}

void reset (void) {
    turn.write(0.5);
    motor.write(0.48);
}

// 0.5 = 0 speed & 0 = full speed
void forward (float speed) {
    float y = speed / 100;
    float x = y * 0.5;
    float z = 0.5 - x;

    motor.write(z);
}

// 0.5 = 0 speed & 1 = full speed

void backward (float speed) {
    float y = speed / 100;
    float x = y * 0.5;
    float z = 0.5 + x;

    motor.write(z);
}

// 0 = right 0.5 = straight 1 = left
void left (void) {
    turn.write(1);
}
void right (void) {
    turn.write(0);
}

void stop (void) {
    b = 1;
    wait(0.3);
    reset();
    wait(0.1);
    b = 0;
    wait(1);
}