main.cpp

00001 #include "mbed.h"
00002 #include "TCS3472_I2C.h"
00003 #include "Motor.h"
00004 
00005 PwmOut LB(p22); // PWM out signal to power LED on the sensor
00006 TCS3472_I2C rgb_sensor( p9, p10 ); // Establish RGB sensor object
00007 
00008 Motor m(p26, p30, p29);
00009 Motor m2(p25, p27, p28);
00010 AnalogIn ultraSonic(p17);
00011 int bob=0;
00012 
00013 int main()
00014 {
00015     rgb_sensor.enablePowerAndRGBC(); // Enable RGB sensor
00016     rgb_sensor.setIntegrationTime(100); // Set integration time of sensor
00017 
00018     float measurements = ultraSonic.read();
00019     float meas_dist = 531.3726*measurements + 0.8440;
00020 
00021 
00022     int rgb_data[4]; // declare a 3 element array to store RGB sensor readings
00023     float PWMbrightness = 1.0; // declare a float specifying brightness of LED (between 0.0 and 1.0)
00024     LB = PWMbrightness; // set brightness of sensor LED
00025     wait(1);
00026     while(meas_dist > 20) {
00027 
00028 
00029         rgb_sensor.getAllColors( rgb_data ); // read the sensor to get red, green, and blue color data along with overall brightness
00030 
00031         // Print data to serial port
00032         m2.speed(0);
00033         m.speed(.6);
00034         while ((rgb_data[2] >= 300)) {//Right If Green color is greater than 200 (the color is green)
00035             m2.speed(.6);
00036             m.speed(0);
00037             rgb_sensor.getAllColors( rgb_data );
00038 
00039             measurements = ultraSonic.read();
00040             meas_dist = 531.3726*measurements + 0.8440;
00041             if (meas_dist <10)
00042             {
00043                 //int bob=1;
00044                 break;};
00045         };
00046         while ((rgb_data[1] <= 225)) {//if red color is less than 180 (the color is blue)
00047             m2.speed(0);
00048             m.speed(0.5);
00049             rgb_sensor.getAllColors( rgb_data );
00050 
00051             measurements = ultraSonic.read();
00052             meas_dist = 531.3726*measurements + 0.8440;
00053              if (meas_dist <10)
00054             {
00055                 //int bob=1;
00056                 break;};
00057         };
00058         while ((rgb_data[3] <= 180)) {//Left If the blue is less than 95 (the color is red)
00059             m2.speed(-.6);
00060             m.speed(0);
00061             rgb_sensor.getAllColors( rgb_data );
00062 
00063             measurements = ultraSonic.read();
00064             meas_dist = 531.3726*measurements + 0.8440;
00065              if (meas_dist <10)
00066             {
00067                 //int bob=1;
00068                 break;
00069             };
00070         };
00071        
00072     };
00073     m.speed(0);
00074     m2.speed(0);
00075     wait(.75);
00076         
00077 }