File content as of revision 1:ac7fc0b5f878:

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

DigitalOut myled(LED1);
m3pi huey;

//stops without hitting
void slowStop(float speed)
{
    float i;
    for(i = speed; i > 0; i = i - 0.05)
    {
        huey.forward(i);
        wait(0.075);      
    }
    huey.stop();
}

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

void smoothFollow(float position, float speed)
{
    float u = speed/4;
    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 follow(float position, float speed)
{
    float tol = 0.05;
    if(position > tol)
    {
        huey.right(speed);     
    }
    else if(position < (-tol))
    {
        huey.left(speed);    
    }
    else
    {
        huey.forward(speed);
    }
        
}

int main() {
    //calibrates sensors
    huey.sensor_auto_calibrate();  

    //printBattery();
    
    
    //huey.printf("%f", huey.line_position());
    
    float speed = 0.1;
    float pos;
    int z=1;
    while(z==1)
    {
        //huey.right_motor(speed);
        pos = huey.line_position();
        huey.printf("%f",pos );
        smoothFollow(pos, speed);

    }

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