Diff: main.cpp

Revision:

1:9eb8d4e6a719

Parent:

0:1cbed7d36912

Child:

2:a681e7e4bd72

--- a/main.cpp	Fri Apr 24 00:53:50 2015 +0000
+++ b/main.cpp	Fri Apr 24 02:02:48 2015 +0000
@@ -1,9 +1,16 @@
 #include "mbed.h"       //includes this library for all mbed functions
 #include "TCS3472_I2C.h"        //includes the library for the RGB sensor
 #include "Motor.h"
+AnalogIn sonic(p15);
 Motor R(p24, p25, p26);
 Motor L(p21, p22, p23);
 
+float data;         //ULTRASONIC SENSOR
+float sum;
+float average;
+int p;
+int count;
+
 PwmOut RGBsensorLED(p22);       //established pin to power the LED on the sensor
 TCS3472_I2C RGBsensor(p9, p10);     //establishes pins for the RGB sensor.
 
@@ -12,6 +19,7 @@
 
 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
 
@@ -21,51 +29,69 @@
     pc.baud(921600);        //sets the baud rate of the serial connection
 
     while (1) {     //loops the program until is broken
-
-        int colorcount[3] = {0,0,0};      //establishes an array for the possible regions the car is in
-        int scan = 0;       //variable for for loop
-
-        for (scan=0; scan<10; scan+=1) {
+        data=sonic.read();
+        sum=0;
+        for(p=0; p<=50; p++) {
+            sum =sum+data;
+        }
+        average= sum/50;
+        printf(" average is %f \n",average);
 
-            RGBsensor.getAllColors(RGB_data);       //read data from sensor for red, green, and blue along with magnitude
-            
-            int light = 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(average> 0.026) {
+            int colorcount[3] = {0,0,0};      //establishes an array for the possible regions the car is in
+            int scan = 0;       //variable for for loop
+
+            for (scan=0; scan<10; scan+=1) {
+
+                RGBsensor.getAllColors(RGB_data);       //read data from sensor for red, green, and blue along with magnitude
+
+                int light = 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.6822*red + 15.609)-light) < 10) && (abs((4.2525*green - 37.993)-light) < 10) && (abs((6.5565*blue - 44.793 )-light) < 10) ) {
-                colorcount[0] += 1;     //car is in red
-                L.speed(0.0);
-                R.speed(0.2);
+                if ( (abs((1.6822*red + 15.609)-light) < 10) && (abs((4.2525*green - 37.993)-light) < 10) && (abs((6.5565*blue - 44.793 )-light) < 10) ) {
+                    colorcount[0] += 1;     //car is in red
+                    L.speed(0.0);
+                    R.speed(0.2);
+                }
+
+                if ( (abs((3.7757*red - 134.86)-light) < 10) && (abs((3.4282*green - 8.791 )-light) < 10) && (abs((2.5685*blue + 63.205)-light) < 10) ) {
+                    colorcount[1] += 1;     //car is in blue
+                    R.speed(0.5);
+                    L.speed(0.5);
+                }
+
+                if ( (abs((3.8225*red - 138.09)-light) < 10) && (abs((2.1032*green + 39.654)-light) < 10) && (abs((5.3619*blue - 2.7884)-light) < 10) ) {
+                    colorcount[2] += 1;     //car is in green
+                    L.speed(0.2);
+                    R.speed(0.0);
+                }
             }
-            
-            if ( (abs((3.7757*red - 134.86)-light) < 10) && (abs((3.4282*green - 8.791 )-light) < 10) && (abs((2.5685*blue + 63.205)-light) < 10) ) {
-                colorcount[1] += 1;     //car is in blue
-                 R.speed(0.5);
-                 L.speed(0.5);
+            if (colorcount[0] >= 7) {       //data reads in the red
+                pc.printf("Making a right turn. \n");        //print to tera term
+
             }
-            
-            if ( (abs((3.8225*red - 138.09)-light) < 10) && (abs((2.1032*green + 39.654)-light) < 10) && (abs((5.3619*blue - 2.7884)-light) < 10) ) {
-                colorcount[2] += 1;     //car is in green
-                L.speed(0.2);
-                R.speed(0.0);
+            if (colorcount[1] >= 7) {       //data reads in the blue
+                pc.printf("Driving straight. \n");        //print to tera term
+
+            }
+            if (colorcount[2] >= 7) {       //data reads in the green
+                pc.printf("Making a left turn. \n");        //print to tera term
+
             }
         }
-        if (colorcount[0] >= 7) {       //data reads in the red
-            pc.printf("Making a right turn. \n");        //print to tera term
- 
+        if ((average> 0.014) && (average < 0.026)) {
+
+            L.speed(0.2);
+            R.speed(0.2);
+            printf("%f \n",average);
         }
-        if (colorcount[1] >= 7) {       //data reads in the blue
-            pc.printf("Driving straight. \n");        //print to tera term
-          
+        if(average< 0.014) {
+        wait(0.1);
+            L.speed(0.0);
+            R.speed(0.0);
         }
-        if (colorcount[2] >= 7) {       //data reads in the green
-            pc.printf("Making a left turn. \n");        //print to tera term
-           
-        }
-
-
     }
 
 }
\ No newline at end of file