File content as of revision 7:d0c19729e7fd:

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

DigitalOut myled(LED1);
m3pi huey;
DigitalIn m3pi_IN[]={(p12),(p21)}; // IR-Sensor


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

void smoothFollow(float position, float speed)
{
    float u = speed;
    u = u * position;
    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() {
    
    
    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);
    }
}