Diff: main.cpp

Revision:

0:dd23b27532c5

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Apr 27 20:24:55 2015 +0000
@@ -0,0 +1,203 @@
+#include "mbed.h"
+#include "Motor.h"
+#include "Servo.h"
+#include "TCS3472_I2C.h"
+
+
+PwmOut LB(p22); // PWM out signal to power LED on the sensor
+TCS3472_I2C rgb_sensor( p9, p10 ); // Establish RGB sensor object
+Motor m(p25, p27, p28);
+Serial pc(USBTX, USBRX); //USB Port Declare
+AnalogIn sensorin(p20); //Analog Pin Declare
+AnalogIn photoin(p16); // photoresistor into pin 16
+AnalogIn photoin2(p18);
+AnalogIn photoin3(p17);
+Servo s1(p21);
+float distanceinches = 20;
+
+int main()
+{
+    pc.printf("starting...");
+    rgb_sensor.enablePowerAndRGBC(); // Enable RGB sensor
+    rgb_sensor.setIntegrationTime(100); // Set integration time of sensor
+    pc.baud(9600);//Port Parameters
+    pc.format(8, SerialBase::None, 1);
+
+    float volt = 0.0; // variable float
+    float volt2= 0.0;
+    float volt3= 0.0;
+    float PWMbrightness = 1.0; // declare a float specifying brightness of LED
+    int rgb_data[4]; // declare a 4 element array to store RGB sensor readings
+    float straight = .605;
+    float left = .755;
+    float right = .455;
+    float bleft = .455;
+    float manspeed = 0;
+    float bright = .755;
+    m.speed(0);//Stop Motor
+    s1 = straight;//Set Servo Straight Ahead
+    float motspeed = 0;
+    int tooclose = 0;
+    float sensorval = 0;
+    int distanceconversion = 0;//Set change for distance to ultra
+    wait(2);
+    int counter = 1;
+    motspeed = .25;
+    manspeed = .35;
+    m.speed(.25);
+    pc.printf("starting2");
+
+
+    while(tooclose<2) { //MAINPROGRAM
+
+
+        volt = photoin;
+        volt2= photoin2;
+        volt3= photoin3;
+
+        LB = PWMbrightness; // set brightness of sensor LED
+        rgb_sensor.getAllColors( rgb_data ); // read the sensor to get red, green, and blue color data along with overall brightness
+        //pc.printf( "red: %d, green: %d, blue: %d \n", rgb_data[1], rgb_data[2], rgb_data[3]);
+        s1 = straight;
+        m.speed(motspeed);
+        while(counter<=1) {
+            distanceinches=20;
+            counter = 2;
+        }
+
+        if(distanceinches>19) {
+            pc.printf("%4.3f     %d\n", distanceinches, tooclose);
+            if(rgb_data[1]>rgb_data[2] && rgb_data[1]>rgb_data[3]) {
+                m.speed(-.25);
+                s1 = bleft;
+                wait(.25);
+                while(rgb_data[1]>rgb_data[2] && rgb_data[1]>rgb_data[3]) {
+                    rgb_sensor.getAllColors( rgb_data );
+                    m.speed(-manspeed);
+                }
+                //AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
+                wait(.4);
+                //AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAa
+                s1 = straight;
+                m.speed(manspeed);
+            }//end red if
+            else if(rgb_data[2]>rgb_data[1] && rgb_data[2]>rgb_data[3]) {
+                m.speed(-.25);
+                s1 = bright;
+                wait(.25);
+                while(rgb_data[2]>rgb_data[1] && rgb_data[2]>rgb_data[3]) {
+                    rgb_sensor.getAllColors( rgb_data );
+                    m.speed(-manspeed);
+                }
+                //AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
+                wait(.4);
+                //AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAa
+                s1 = straight;
+                m.speed(manspeed);
+            }//end green if
+            else if(rgb_data[3] >= rgb_data[1] && rgb_data[3]>rgb_data[2]) {
+                //pc.printf("go straight\n");
+                m.speed(manspeed);
+                wait(.1);
+            }//end blue if
+            else {
+                //pc.printf("back up\n");
+                m.speed(-manspeed);
+                wait(.1);
+            }
+
+            //pc.printf("go straight");
+
+
+
+            sensorval = sensorin.read();//Read Sensor
+            distanceinches = (160750 * (sensorval*sensorval*sensorval) - 16325 * (sensorval*sensorval) + 1039 * sensorval - 4.6243) - distanceconversion;//Convert to inches
+            pc.printf("%4.3f     %d\n", distanceinches, tooclose);
+        } else {
+
+            if(rgb_data[1]>rgb_data[2] && rgb_data[1]>rgb_data[3]) {
+                m.speed(-.25);
+                s1 = bleft;
+                wait(.25);
+                while(rgb_data[1]>rgb_data[2] && rgb_data[1]>rgb_data[3]) {
+                    rgb_sensor.getAllColors( rgb_data );
+                    m.speed(-manspeed);
+                }
+                //AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
+                wait(.4);
+                //AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAa
+                s1 = straight;
+                m.speed(manspeed);
+                wait(.2);
+            }//end red if
+            else if(rgb_data[1]>rgb_data[2] && rgb_data[1]>rgb_data[3]) {
+                m.speed(-.25);
+                s1 = bright;
+                wait(.25);
+                while(rgb_data[1]>rgb_data[2] && rgb_data[1]>rgb_data[3]) {
+                    rgb_sensor.getAllColors( rgb_data );
+                    m.speed(-manspeed);
+                }
+                //AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
+                wait(.4);
+                //AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAa
+                s1 = straight;
+                m.speed(manspeed);
+            }//end red if
+            else if(rgb_data[3] >= rgb_data[1] && rgb_data[3]>rgb_data[2]) {
+                if(volt>volt2 && volt>volt3 && volt>.025) {
+                    pc.printf("turn left");
+                    m.speed(-.5);
+                    s1 = bleft;
+                    wait(.25);
+                    m.speed(0);
+                    wait(.2);
+                    s1 = straight;
+                    wait(.2);
+                    sensorval = sensorin.read();//Read Sensor
+                    distanceinches = (160750 * (sensorval*sensorval*sensorval) - 16325 * (sensorval*sensorval) + 1039 * sensorval - 4.6243) - distanceconversion;//Convert to inches
+                    pc.printf("%4.3f     %d\n", distanceinches, tooclose);
+
+                } else if(volt2>volt && volt2>volt3 && volt2>.0002) {
+
+                    pc.printf("turn right");
+                    m.speed(-.5);
+                    s1 = bright;
+                    wait(.25);
+                    m.speed(0);
+                    wait(.2);
+                    s1 = straight;
+                    wait(.2);
+                    sensorval = sensorin.read();//Read Sensor
+                    distanceinches = (160750 * (sensorval*sensorval*sensorval) - 16325 * (sensorval*sensorval) + 1039 * sensorval - 4.6243) - distanceconversion;//Convert to inches
+                    pc.printf("%4.3f     %d\n", distanceinches, tooclose);
+
+
+                } else {
+
+                    pc.printf("go straight");
+                    m.speed(manspeed);
+                    wait(.2);
+                    sensorval = sensorin.read();//Read Sensor
+                    distanceinches = (160750 * (sensorval*sensorval*sensorval) - 16325 * (sensorval*sensorval) + 1039 * sensorval - 4.6243) - distanceconversion;//Convert to inches
+                    pc.printf("%4.3f     %d\n", distanceinches, tooclose);
+
+                }
+
+
+                sensorval = sensorin.read();//Read Sensor
+                distanceinches = (160750 * (sensorval*sensorval*sensorval) - 16325 * (sensorval*sensorval) + 1039 * sensorval - 4.6243) - distanceconversion;//Convert to inches
+                pc.printf("%4.3f     %d\n", distanceinches, tooclose);
+
+
+            }
+        }
+        if(distanceinches>10) { //If car far enough away
+            tooclose = 0;
+        } else {
+            tooclose = tooclose+1;
+        }
+    }
+
+    m.speed(0);
+}