File content as of revision 1:db5986287d13:

#include "mbed.h"       //includes this library for all mbed functions
#include "TCS3472_I2C.h"        //includes the library for the RGB sensor
#include "Servo.h"      //includes the library for the servo motor
#include "Motor.h"      //includes library for the dc motor

PwmOut RGBsensorLED(p22);       //established pin to power the LED on the sensor
TCS3472_I2C RGBsensor(p9, p10);     //establishes pins for the RGB sensor.
Servo s1(p24);      //establishes pin for servo motor
Motor m1(p26,p29,p30);      //establishes pins for the dc motor


AnalogIn range(p19);
Serial pc(USBTX, USBRX);
float distance;

int main()      //starts the program
{
    int RGB_data[4];        //declares an array to store the data read from the RGB sensor
    float PWMbrightness = 1.0;      //declares float for LED brightness

    RGBsensor.enablePowerAndRGBC();     //enables RGB sensor
    RGBsensor.setIntegrationTime(100);      //sets the intergration time of the RGB sensor
    RGBsensorLED = PWMbrightness;       //set brightness of sensor LED
    pc.baud(921600);        //sets the baud rate of the serial connection
    //pc.format(7,SerialBase::None,1);

    s1 = .5;        //starting position
    wait(5.0);

    while (1) {     //loops the program until is broken

        int region[5] = {0,0,0,0,0};      //establishes an array for the possible regions the car is in
        int scan = 0;       //variable for for loop
        distance = 565.9273577*(range.read()) -.63993889;
        for (scan=0; scan<5; scan+=1) {

            RGBsensor.getAllColors(RGB_data);       //read data from sensor for red, green, and blue along with magnitude

            //pc.printf("%d,%d,%d,%d\n", RGB_data[0], RGB_data[1], RGB_data[2], RGB_data[3]);
            //pc.printf("unfiltered: %d, red: %d, green: %d, blue: %d \n", RGB_data[0], RGB_data[1], RGB_data[2], RGB_data[3]);

            int Unfil = RGB_data[0];        //sets a variable for the light data so I don't have to type that every time
            int Red = RGB_data[1];      //sets a variable for the red data so I don't have to type that every time
            int Green = RGB_data[2];        //sets a variable for the blue data so I don't have to type that every time
            int Blue = RGB_data[3];     //sets a variable for the green data so I don't have to type that every time

            if ( (abs((1.7574*Red + 3.9239)-Unfil) < 13) && (abs((4.0508*Green - 32.749)-Unfil) < 13) && (abs((5.959*Blue - 21.767 )-Unfil) < 13) ) {
                region[0] += 1;     //car is in red
            }
            if ( (abs((2.3592*Red - 40.53 )-Unfil) < 13) && (abs((3.6436*Green - 17.036 )-Unfil) < 13) && (abs((3.6735*Blue + 40.292)-Unfil) < 13) ) {
                region[1] += 1;     //car is in red and blue
            }
            if ( (abs((3.6835*Red - 130.71)-Unfil) < 13) && (abs((3.4125*Green - 11.762 )-Unfil) < 13) && (abs((2.542*Blue + 64.922)-Unfil) < 13) ) {
                region[2] += 1;     //car is in blue
            }
            if ( (abs((3.5816*Red - 120.33 )-Unfil) < 13) && (abs((2.4781*Green + 22.624)-Unfil) < 13) && (abs((3.6996*Blue + 37.829)-Unfil) < 13) ) {
                region[3] += 1;     //car is in blue and green
            }
            if ( (abs((3.6619*Red - 126.41)-Unfil) < 13) && (abs((2.109*Green + 40.777)-Unfil) < 13) && (abs((5.0975*Blue - 1.0619)-Unfil) < 13) ) {
                region[4] += 1;     //car is in green
            }
        }

        if (region[0] >= 4) {       //data reads in the red
            pc.printf("Red Region. Make Right Turn. \n");        //print to tera term
            s1 = .75;       //make right turn
            // m1.speed(.22);
            // wait(.25);
            // m1.speed(0.0);
            // s1 = .5;        //straighten it out
        }
        if (region[1] >= 4) {       //data reads on the red/blue line
            pc.printf("Red/Blue Region. Make Slight Right Turn. \n");        //print to tera term
            s1 = .625;       //make slight right turn
            m1.speed(.22);
            //wait(.25);
            // m1.speed(0.0);
            // s1 = .5;        //straighten it out
        }
        if (region[2] >= 4) {       //data reads in the blue
            pc.printf("Blue Region. Drive Straight. We Cruising. \n");        //print to tera term
            s1 = .5;       //make no turn
            //m1.speed(.22);
            // wait(.25);
            // m1.speed(0.0);
            // s1 = .5;        //straighten it out
        }
        if (region[3] >= 4) {       //data reads on the green/blue line
            pc.printf("Green/Blue Region. Make Slight Left Turn. \n");        //print to tera term
            s1 = .375;       //make slight left turn
            // m1.speed(.22);
            // wait(.25);
            //  m1.speed(0.0);
            //  s1 = .5;        //straighten it out
        }
        if (region[4] >= 4) {       //data reads in the green
            pc.printf("Green Region. Make Left Turn. \n");        //print to tera term
            s1 = .25;       //make left turn
            //  m1.speed(.22);
            // wait(.25);
            //m1.speed(0.0);
            // s1 = .5;        //straighten it out
        }
        if ((region[0] < 4)&&(region[1] < 4)&&(region[2] < 4)&&(region[3] < 4)&&(region[4] < 4)) {
            //pc.printf("No region. Bad data. \n");      //prints to tera term
            //  m1.speed(0.0);
        }
        distance = 565.9273577*(range.read()) -.63993889;   //calculate the distance with the range sensor using a calibrated curve
        if(distance>15) {       //if the obstacle is still very far away
            m1.speed(0.5);      //move the motor forward (It will turn according to the RGB servo motor command above)
            wait(0.1);
            m1.speed(0);        //stop the motor and go back to the RGB sensor
        } else {
            while(distance>8) { //slow the movement down once you start to get close and no longer use the RGB sensor for directtion
                s1 = 0.5;           //make it go straight towards the obstacle.
                m1.speed(0.5);
                wait(0.05);
                m1.speed(0);
            }
            while(1) {
                printf("Complete!\n");//entire an infinite loop so it doesn't move again after reaching the objective
                wait(1);
            }

        }
    }
}