File content as of revision 0:0a03e4314cb7:

#include "mbed.h"
#include "TCS3472_I2C.h"
#include "Servo.h"
#include "Motor.h"

PwmOut LB(p22); // PWM out signal to power LED on the sensor
TCS3472_I2C rgb_sensor( p9, p10 ); // Establish RGB sensor object
Serial pc(USBTX,USBRX); // Establish serial connection with computer
Motor rmv(p26,p29,p30); //sets up DC motor
Servo myservo(p21); //sets up the servo motor
DigitalOut led1(LED1);

//This is all for the photo diodes
AnalogIn pd1(p17);
AnalogIn pd2(p16);
AnalogIn pd3(p15);

//This is all for the ultrasound sensor
AnalogIn ted(p18); //sets up the ultrasound sensor
float x;
Timer t;
float data = x;
float distance;


int main()
{
    rgb_sensor.enablePowerAndRGBC(); // Enable RGB sensor
    rgb_sensor.setIntegrationTime(100); // Set integration time of sensor

    pc.baud(9600); // Set Baud rate of serial connection
    pc.format(8, SerialBase::None,1);

    int rgb_data[4]; // declare a 4 element array to store RGB sensor readings
    float PWMbrightness = 1.0; // declare a float specifying brightness of LED (between 0.0 and 1.0)
    int red;
    int blue;
    int green;

    //FOR INFRARED PHOTODIODES
    float in1;
    float in2;
    float in3;
    int turns=1; //main loop while vehicles is navigating through red/green snake
    while(turns==1) {

        LB = PWMbrightness; // set brightness of sensor LED
        rgb_sensor.getAllColors( rgb_data ); // read the sensor to get red, green, and blue color data along with overall brightness

        red=rgb_data[1]; //makes it easier to interpret the data
        green=rgb_data[2];
        blue=rgb_data[3];

        //    myservo=.35; //this is straight for the servo
        rmv.speed(.2);//makes our vehicle go forward
        wait(.1);
        rmv.speed(0);
        wait(.1);



        // Print data to serial port CALIBRATION
        //.printf( "unfiltered: %d, red: %d, green: %d, blue: %d \n\r", rgb_data[0], rgb_data[1], rgb_data[2], rgb_data[3]);
        //  pc.printf("%d,%d,%d,%d\n", rgb_data[0], rgb_data[1], rgb_data[2], rgb_data[3]);
        //wait(0.1);

//RGB SENSOR COLOR TURN
        if((blue>red) && (blue>green)) { //if the blue is on
            //pc.printf("Maintain current bearing\n\r");
            myservo=.35; //goes straight
            wait(0.1);

        }
        if((green>red) && (green>blue)) { //if the green is on
            //pc.printf("Turn right\n\r");
            myservo=.01;//turns right (if motor is going backward)
            rmv.speed(-.2);//makes our vehicle go backward
            wait(.2);
            rmv.speed(0);
            wait(.1);

        }
        if((red>green) && (red>blue)) { //if the red is on
            //pc.printf("Turn left\n\r");
            myservo=.99; //turns left (if motor is going backwar)
            rmv.speed(-.2);//makes our vehicle go backward
            wait(.2);
            rmv.speed(0);
            wait(.1);

        }

        
        //lower numbers go right, higher numbers go left. .35=straight.
        
        in1 = pd1.read();
        in2 = pd2.read();
        in3 = pd3.read();
        //pc.printf("sensor 1: %d,sensor 2: %d,sensor 3: %d\n\r",in1,in2,in3);
        
        if(in1 > .6|| in2 > .6|| in3 > .6){
            turns=0; //exits outs of main while loop, starts a new one in the turns=0 while loop (where IR sensor takes over)
            led1=1;
        }
        
    }//ends turns = 1 loop
    
    while(turns==0) {
        
        
        
        //color sensor stuff
         LB = PWMbrightness; // set brightness of sensor LED
        rgb_sensor.getAllColors( rgb_data ); // read the sensor to get red, green, and blue color data along with overall brightness

        red=rgb_data[1]; //makes it easier to interpret the data
        green=rgb_data[2];
        blue=rgb_data[3];
        
        
        
        //for infrared photodiodes
        in1 = pd1.read();
        in2 = pd2.read();
        in3 = pd3.read();
        
        //ULTRASOUND ACTION
        x = ted.read();
        distance = 689.8006*pow(x,1) - 2.6437; //ultrasound distance calibration equation
       // pc.printf("%f\n\r",distance);
        
        
        //INFRARED TURN ACTION
            if(in1 >in2 && in1 > in3) { //if the right infrared sensor is reading high
                //pc.printf("Turn Right\n");
                myservo=.1; //turns right if motor is going backwards
                rmv.speed(-.2);
                wait(.1);
                rmv.speed(0);
                wait(.1);
            }

            if(in2 >in1 && in2 > in3 && blue>red &&blue>green) { //if the middle infrared sensor is reading high
                //pc.printf("Go Straight\n");
                myservo=.35; //goes straight
                rmv.speed(.2);
                 wait(.1);
                 rmv.speed(0);
                 wait(.1);
                
            }

            if(in3 >in1 && in3 > in2) { //if the left PD is reading high
                //pc.printf("Turn Left\n");
                myservo=.99; //turns left if the motor is going backwards
                rmv.speed(-.2);
                wait(.1);
                rmv.speed(0);
                wait(.1);
            }
        
       if((green>red) && (green>blue)) { //if the green is on
            //pc.printf("Turn right\n\r");
            myservo=.01;//turns right (if motor is going backward)
            rmv.speed(-.2);//makes our vehicle go backward
            wait(.2);
            rmv.speed(0);
            wait(.1);

        }
      //  if((red>green) && (red>blue)) { //if the red is on
      //      //pc.printf("Turn left\n\r");
     //       myservo=.99; //turns left (if motor is going backwar)
     //       rmv.speed(-.2);//makes our vehicle go backward
     //       wait(.2);
     //       rmv.speed(0);
    //        wait(.1);

    //    }
        
        //ultrasound stop action
        if(distance <11) {
            rmv.speed(0);
            wait(10);
        }
    } //end turn = 0 loop
}// end main