File content as of revision 11:af3d70d153bb:

#include "mbed.h"
#include "m3pi_ng.h"
#include "DigitalIn.h"
#include "btbee.h"
#include "time.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;
    if(sensors[1] < 50 && sensors[2] < 50 && sensors[3] < 50)
        return -1;
    else
    {
        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] > 950)
        return 1;
    else
        return 0;
}

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

//returns 0 if side sensors aren't light, 1 if light
int sideSensorsLight(void)
{
    int sensors[5];
    huey.calibrated_sensor(sensors);
    if(sensors[4] > 50 && sensors[0] > 50)
        return 0;
    else
        return 1;
}

//returns 0 if side sensors aren't dark, 1 if dark
int sideSensorsDark(void)
{
    if(rightCorner() == 1 && leftCorner() == 1)
        return 1;
    else
        return 0; 
}

void rightTurn(float speed)
{
    int side = sideSensorsLight();
    huey.forward(speed/2);
    while(side == 0)
    {
        side = sideSensorsLight();
    }
    wait(0.20);
    float pos = myLinePos();
    huey.right(speed/2);
    while(pos > -0.1 && pos < 0.1)
    {
         pos = myLinePos();
    }
    pos = myLinePos();
    while(pos > 0.05 || pos < -0.05)
    {    
        pos = myLinePos();
    } 
}

void leftTurn(float speed)
{
    int side = sideSensorsLight();
    huey.forward(speed/2);
    while(side == 0)
    {
        side = sideSensorsLight(); 
    }
    wait(0.20);
            
    float pos = myLinePos();
    huey.left(speed/2);
    while(pos > -0.1 && pos < 0.1)
    {
        pos = myLinePos();
    }
    pos = myLinePos();
    while(pos > 0.05 || pos < -0.05)
    {    
        pos = myLinePos();
    }    
}

void turnAround(float speed)
{
    float pos = myLinePos();
    huey.right(speed/2);
    while(pos > -0.1 && pos < 0.1)
    {
         pos = myLinePos();
    }
    pos = myLinePos();
    while(pos > 0.05 || pos < -0.05)
    {    
        pos = myLinePos();
    }
    while(pos > -0.1 && pos < 0.1)
    {
         pos = myLinePos();
    }
    pos = myLinePos();
    while(pos > 0.05 || pos < -0.05)
    {    
        pos = myLinePos();
    }  
}

//returns 1 for all turns, 0 for non turns, 2 for end of a task;
int smoothFollow(float position, float speed, char direction)
{
    float u = speed;
    u = u * position;
    
        if(rightCorner() == 1 && direction == 'R')
        {         
            rightTurn(speed);
            huey.printf("R");
            return 1;
        }
        else if(leftCorner() == 1 && direction == 'L')
        {
            leftTurn(speed);
            huey.printf("L");
            return 1;
        }
        else if(sideSensorsDark() == 1 && direction == 'E')
        {
            turnAround(speed);
            huey.stop();
            return 2;  
        }
        else if((rightCorner() == 1 || leftCorner() == 1) && direction == 'S')
        {
            int side = sideSensorsLight();
            huey.forward(speed/2);
            while(side == 0)
            {
                side = sideSensorsLight(); 
            }
            return 1;
        }  
        else if(speed+u > 1)
        {
            huey.stop();
            huey.printf("Fast Er");
            return 0;
        }
        else
        {
            huey.right_motor(speed-u);
            huey.left_motor(speed+u);
            return 0;
        }
}



/*
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() {
    
    
    //Matrix of paths from one point to another: Right = R, Left = L, Straight = S
    char map[6][6][20] =  
    {
        {"", "LRLLRE", "LRLSSE", "LRLSRLLE", "LRRLSLRRRLE", "LRRLRE"},
        {"LRRLRE", "", "LLSE", "LLRLLE", "LSRLRRLE", "LRSLRE"},
        {"SSRLRE", "SRRE", "", "LLLE", "LLRSRLE", "SSSLRE"}, 
        {"RLSRLRE", "RLRRE", "RRE", "", "SSRLE", "SRLRLE"}, 
        {"RLLLRSRLLRE", "RLLRLSRE", "RLSLRE", "RLSSE", "", "RLLLRLE"}, 
        {"LRLLRE", "LRSLRE", "LRSSSE", "RLRLSE", "RLRRRLE", ""}   
    };
    
    int points[3] = {1,2,4};    
    
    
    
    //time_t startt, endt;
    //time(&startt);
    //clock_t clock;
    
     //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 (iline < 3) {
        // this writes "Line 001\n" to "Line 005\n" and then "end\n" to the btbee
        if ( btbee.writeable() ) {
            if (iline==2) {
                //btbee.printf("end\n");
                iline++;
            }//if
            else {
                if (iline <2){
                //time(&endt);
                //btbee.printf("%f seconds - ", clock);
                //btbee.printf("Battery Voltage: %f\n", huey.battery());
                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.cls();
        for(int p = 1; p < sizeof(points); p ++)
        {
            int s = points[p-1] - 1;
            int f = points[p] - 1;
            for(int i = 0; i < sizeof(map[s][f]); i++)
            {  
                int change = 0;
                while(change == 0)
                {
                    pos = myLinePos();
                    change = smoothFollow(pos, speed, map[s][f][i]);
                }
                if(change == 2)
                {
                    huey.stop();
                    wait(1);
                    break;
                }
            }
        } 
        if(m3pi_IN[0]==0)
        {
            //slowStop(speed, 0.05, 3);
            huey.stop();
            btbee.printf("Object in Front");
            while(m3pi_IN[0]==0)
            {
                huey.stop();    
            }
        }
        if(pushToStart() == 0)
        {
                btbee.printf("end\n");
                break;
        }
    }

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