File content as of revision 9:493011bacdcd:

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

//direction varriable is between 0 and 1, 0 meaning left 1 meaning right
//returns 1 if it turned, 0 otherwise
/*int crossing(float speed, int direction)
{
    if(rightCorner() == 1 && leftCorner() == 1)
    {
        if(direction == 0)
        {
            huey.left(speed*2);
            return 1;
        }
        else
        {
            huey.right(speed*2);
            return 1;    
        } 
    }
    else
    {
        if(rightCorner() == 1)
        {
            huey.right(speed*2);
            return 0;
        }
        else if(leftCorner() == 1)
        {
            huey.left(speed*2);
            return 0;
        }
        else
            return 0;
    }

}
*/
void smoothFollow(float position, float speed)//, int direction)
{
    float u = speed;
    u = u * position;
    
    /*if(rightCorner() == 1 && leftCorner() == 1)
    {
        if(direction == 0)
        {
            huey.left(speed*2);
            return 1;
        }
        else
        {
            huey.right(speed*2);
            return 1;    
        } 
    }
    else
    {*/
        if(rightCorner() == 1)
        {
            int sensors[5];
            huey.calibrated_sensor(sensors);
            while(sensors[4] > 100)
            {
                huey.forward(speed);
                huey.calibrated_sensor(sensors);  
            }
            //Stops and semi-properly turns at corners and t's but gets screwed up at + intersections because he turns until the middle sensors read black
            // But at a + intersection the sensors are black already so he doesnt turn
            huey.stop();
            wait(0.1);
            float pos = -1;
            while(pos > 0.05 || pos < -0.05)
            {    
                huey.right(speed);
                pos = myLinePos();
            }
            
            //return 0;
        }
        else if(leftCorner() == 1)
        {
            huey.left(speed*2);
            //return 0;
        }  
        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))
    {
        //FIGURE THIS OUT KJALSHDFKJASHFLKJASHFLKJASHDLKJASHDLKJHASDLKJHASLKDJH
        smoothFollow(huey.line_position(),i);
        wait(waitTime);      
    }
    huey.stop();
}


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



/////////////////////////////////////////////////////////////////
int main() {
    //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;
    int turns[3];
    turns[0] = 0;
    turns[1] = 1;
    turns[2] = 0;
    
    
    while(z==1)
    {
        
        //Trying to get it to follow a sequence of turns
        /*for(int q = 0; q < sizeof(turns); q++)
        {
            int change = 0;
            while(change == 0)
            {
                //change = crossing(speed, turns[q]);
                pos = myLinePos();
                change = smoothFollow(pos, speed, turns[q]);
                    
            }  
        }*/
        
        
        
        pos = myLinePos();

        smoothFollow(pos, speed);
           
        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);
    }
}