Diff: main.cpp

Revision:

0:267ae73c744e

Child:

1:db5986287d13

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Apr 28 01:24:23 2015 +0000
@@ -0,0 +1,112 @@
+#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;
+float new_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);
+        }
+
+
+    }
+}