File content as of revision 8:09f4495696d2:

#include "mbed.h"
#include "m3pi_ng.h"
#include "DigitalIn.h"
#include "btbee.h"


DigitalOut myled(LED1);
m3pi huey;
btbee btbee;

DigitalIn m3pi_IN[]={(p12),(p21)}; // IR-Sensor
DigitalOut mbed_led[] = {(LED1), (LED2),(LED3), (LED4)};
DigitalOut m3pi_led[] = {(p13), (p14), (p15), (p16), (p17), (p18), (p19), (p20)};
DigitalIn m3pi_pb(p21);

void printBattery()
{
    //prints battery voltage on led screen
    float bat = huey.battery();
    huey.printf("Bat volt");
    huey.locate(0,1);
    huey.printf("%f",bat); 
}

float myLinePos(void)
{
    int sensors[5];
    huey.calibrated_sensor(sensors);
    //oat leftSen, rightSen, midSen;
    //ftSen = sensors[1] / 1000;
    //ghtSen = sensors[3] / 1000;
    //dSen = sensors[2] / 1000;
    float val;
    val = (((1000*sensors[2] + 2000*sensors[3])/(sensors[1]+sensors[2]+sensors[3])));
    return -1+(val/1000);
    
}

int rightCorner()
{
    int sensors[5];
    huey.calibrated_sensor(sensors);
    if(sensors[4] > 900)
        return 1;
    else
        return 0;
}

int leftCorner()
{
    int sensors[5];
    huey.calibrated_sensor(sensors);
    if(sensors[0] > 900)
        return 1;
    else
        return 0;
}


void smoothFollow(float position, float speed)
{
    float u = speed;
    u = u * position;
    
    if(rightCorner() == 1)
    {
            huey.right(speed*2);
            //wait(0.2);
    }
    else if(leftCorner() == 1)
    {
        huey.left(speed*2);
        //wait(0.2);
    }  
    else if(speed+u > 1)
    {
        huey.stop();
        huey.printf("Fast Er");
    }
    else
    {
        huey.right_motor(speed-u);
        huey.left_motor(speed+u);
    }
    
}

void slowStop(float speed, float waitTime, int steps)
{
    float i;
    for(i = speed; i > 0; i = i - (speed/steps))
    {
        smoothFollow(huey.line_position(),i);
        wait(waitTime);      
    }
    huey.stop();
}


int pushToStart(void)
{
    DigitalIn button(p21);
    int value = button.read();
    return value;
}



/////////////////////////////////////////////////////////////////
int main() {
    
    /*
     //btbee trial
    // initialization stuff ////////////////////////////////////////////////////////////////////////////////////////////////////
    huey.locate(0,1);
    btbee.reset();
    for (int i = 0; i <4; i++) {
        mbed_led[i] = 0;
    }
    for (int i = 0; i <8; i++) {
        m3pi_led[i]=0;
    }
    m3pi_pb.mode(PullUp); // expected would be 1 when pb is pressed, 0 when not, opposite is the case

    // end initialization stuff ////////////////////////////////////////////////////////////////////////////////////////////////

    huey.locate(0,0);
    huey.printf("B:%0.3f%V",huey.battery());
    wait(0.3);
    
    huey.locate(0,0);
    huey.printf("%s","btTest");
    huey.locate(0,1);
    huey.printf("%s","PBonLNK");

    // wait for the user to push P21, should be pressed when the bt link is established (green led "link")
    while(m3pi_pb) {
        m3pi_led[0]=!m3pi_led[0];
        wait(0.1);
    }

    int iline=1;
    char arr_read[30]; // this should be long enough to store any reply coming in over bt.
    int  chars_read;   // number of chars read in a bt reply
    
    while (true) {
        // this writes "Line 001\n" to "Line 005\n" and then "end\n" to the btbee
        if ( btbee.writeable() ) {
            if (iline==6) {
                btbee.printf("end\n");
                iline++;
            }//if
            else {
                if (iline <6){
                btbee.printf("Line %0.3d \n",iline);
                m3pi_led[0]=0;
                huey.locate(0,0);
                huey.printf("Sent %0.3d",iline);
                iline++;
                }
            }//else
        }//if_write

        // check for answers after each write /not write
        while ( btbee.readable() ) {
            m3pi_led[7]=1;
            btbee.read_all(arr_read, 30, &chars_read );
            m3pi_led[6]=1;
            huey.locate(0,1);
            huey.print(arr_read,chars_read);
            m3pi_led[5]=1;
        }//while_readable
        wait(0.1);
    }//while_true
    */
//main
//end of btbee trial
   
    
    
    m3pi_IN[0].mode(PullUp);
    m3pi_IN[1].mode(PullUp);
    
    int start;
    do 
    {
        start = pushToStart();  
    }while(start == 1);
    
    wait(1);
    //calibrates sensors
    huey.sensor_auto_calibrate();  

    printBattery();
    
    
    float speed = 0.25;
    float pos;
    int z=1;
    while(z==1)
    {
        //huey.right_motor(speed);
        pos = myLinePos();//huey.line_position();
        smoothFollow(pos, speed);
        if(m3pi_IN[0]==0)
        {
            slowStop(speed, 0.05, 3);
            huey.stop();
            while(m3pi_IN[0]==0)
            {
                huey.printf("Stuck");    
            }
        }

    }

    
    
    
    
    
    while(1) 
    {
        myled = 1;
        wait(0.2);
        myled = 0;
        wait(0.2);
    }
}