main.cpp

00001 #include "mbed.h"       //includes this library for all mbed functions
00002 #include "TCS3472_I2C.h"        //includes the library for the RGB sensor
00003 #include "Servo.h"      //includes the library for the servo motor
00004 #include "Motor.h"      //includes library for the dc motor
00005 
00006 PwmOut RGBsensorLED(p22);       //established pin to power the LED on the sensor
00007 TCS3472_I2C RGBsensor(p9, p10);     //establishes pins for the RGB sensor.
00008 Servo s1(p24);      //establishes pin for servo motor
00009 Motor m1(p26,p29,p30);      //establishes pins for the dc motor
00010 
00011 
00012 AnalogIn range(p19);
00013 Serial pc(USBTX, USBRX);
00014 float distance;
00015 
00016 int main()      //starts the program
00017 {
00018     int RGB_data[4];        //declares an array to store the data read from the RGB sensor
00019     float PWMbrightness = 1.0;      //declares float for LED brightness
00020 
00021     RGBsensor.enablePowerAndRGBC();     //enables RGB sensor
00022     RGBsensor.setIntegrationTime(100);      //sets the intergration time of the RGB sensor
00023     RGBsensorLED = PWMbrightness;       //set brightness of sensor LED
00024     pc.baud(921600);        //sets the baud rate of the serial connection
00025     //pc.format(7,SerialBase::None,1);
00026 
00027     s1 = .5;        //starting position
00028     wait(5.0);
00029 
00030     while (1) {     //loops the program until is broken
00031 
00032         int region[5] = {0,0,0,0,0};      //establishes an array for the possible regions the car is in
00033         int scan = 0;       //variable for for loop
00034         distance = 565.9273577*(range.read()) -.63993889;
00035         for (scan=0; scan<5; scan+=1) {
00036 
00037             RGBsensor.getAllColors(RGB_data);       //read data from sensor for red, green, and blue along with magnitude
00038 
00039             //pc.printf("%d,%d,%d,%d\n", RGB_data[0], RGB_data[1], RGB_data[2], RGB_data[3]);
00040             //pc.printf("unfiltered: %d, red: %d, green: %d, blue: %d \n", RGB_data[0], RGB_data[1], RGB_data[2], RGB_data[3]);
00041 
00042             int Unfil = RGB_data[0];        //sets a variable for the light data so I don't have to type that every time
00043             int Red = RGB_data[1];      //sets a variable for the red data so I don't have to type that every time
00044             int Green = RGB_data[2];        //sets a variable for the blue data so I don't have to type that every time
00045             int Blue = RGB_data[3];     //sets a variable for the green data so I don't have to type that every time
00046 
00047             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) ) {
00048                 region[0] += 1;     //car is in red
00049             }
00050             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) ) {
00051                 region[1] += 1;     //car is in red and blue
00052             }
00053             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) ) {
00054                 region[2] += 1;     //car is in blue
00055             }
00056             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) ) {
00057                 region[3] += 1;     //car is in blue and green
00058             }
00059             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) ) {
00060                 region[4] += 1;     //car is in green
00061             }
00062         }
00063 
00064         if (region[0] >= 4) {       //data reads in the red
00065             pc.printf("Red Region. Make Right Turn. \n");        //print to tera term
00066             s1 = .75;       //make right turn
00067             // m1.speed(.22);
00068             // wait(.25);
00069             // m1.speed(0.0);
00070             // s1 = .5;        //straighten it out
00071         }
00072         if (region[1] >= 4) {       //data reads on the red/blue line
00073             pc.printf("Red/Blue Region. Make Slight Right Turn. \n");        //print to tera term
00074             s1 = .625;       //make slight right turn
00075             m1.speed(.22);
00076             //wait(.25);
00077             // m1.speed(0.0);
00078             // s1 = .5;        //straighten it out
00079         }
00080         if (region[2] >= 4) {       //data reads in the blue
00081             pc.printf("Blue Region. Drive Straight. We Cruising. \n");        //print to tera term
00082             s1 = .5;       //make no turn
00083             //m1.speed(.22);
00084             // wait(.25);
00085             // m1.speed(0.0);
00086             // s1 = .5;        //straighten it out
00087         }
00088         if (region[3] >= 4) {       //data reads on the green/blue line
00089             pc.printf("Green/Blue Region. Make Slight Left Turn. \n");        //print to tera term
00090             s1 = .375;       //make slight left turn
00091             // m1.speed(.22);
00092             // wait(.25);
00093             //  m1.speed(0.0);
00094             //  s1 = .5;        //straighten it out
00095         }
00096         if (region[4] >= 4) {       //data reads in the green
00097             pc.printf("Green Region. Make Left Turn. \n");        //print to tera term
00098             s1 = .25;       //make left turn
00099             //  m1.speed(.22);
00100             // wait(.25);
00101             //m1.speed(0.0);
00102             // s1 = .5;        //straighten it out
00103         }
00104         if ((region[0] < 4)&&(region[1] < 4)&&(region[2] < 4)&&(region[3] < 4)&&(region[4] < 4)) {
00105             //pc.printf("No region. Bad data. \n");      //prints to tera term
00106             //  m1.speed(0.0);
00107         }
00108         distance = 565.9273577*(range.read()) -.63993889;   //calculate the distance with the range sensor using a calibrated curve
00109         if(distance>15) {       //if the obstacle is still very far away
00110             m1.speed(0.5);      //move the motor forward (It will turn according to the RGB servo motor command above)
00111             wait(0.1);
00112             m1.speed(0);        //stop the motor and go back to the RGB sensor
00113         } else {
00114             while(distance>8) { //slow the movement down once you start to get close and no longer use the RGB sensor for directtion
00115                 s1 = 0.5;           //make it go straight towards the obstacle.
00116                 m1.speed(0.5);
00117                 wait(0.05);
00118                 m1.speed(0);
00119             }
00120             while(1) {
00121                 printf("Complete!\n");//entire an infinite loop so it doesn't move again after reaching the objective
00122                 wait(1);
00123             }
00124 
00125         }
00126     }
00127 }