Diff: main.cpp

Revision:

0:0a03e4314cb7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Apr 27 21:12:28 2015 +0000
@@ -0,0 +1,189 @@
+#include "mbed.h"
+#include "TCS3472_I2C.h"
+#include "Servo.h"
+#include "Motor.h"
+
+PwmOut LB(p22); // PWM out signal to power LED on the sensor
+TCS3472_I2C rgb_sensor( p9, p10 ); // Establish RGB sensor object
+Serial pc(USBTX,USBRX); // Establish serial connection with computer
+Motor rmv(p26,p29,p30); //sets up DC motor
+Servo myservo(p21); //sets up the servo motor
+DigitalOut led1(LED1);
+
+//This is all for the photo diodes
+AnalogIn pd1(p17);
+AnalogIn pd2(p16);
+AnalogIn pd3(p15);
+
+//This is all for the ultrasound sensor
+AnalogIn ted(p18); //sets up the ultrasound sensor
+float x;
+Timer t;
+float data = x;
+float distance;
+
+
+int main()
+{
+    rgb_sensor.enablePowerAndRGBC(); // Enable RGB sensor
+    rgb_sensor.setIntegrationTime(100); // Set integration time of sensor
+
+    pc.baud(9600); // Set Baud rate of serial connection
+    pc.format(8, SerialBase::None,1);
+
+    int rgb_data[4]; // declare a 4 element array to store RGB sensor readings
+    float PWMbrightness = 1.0; // declare a float specifying brightness of LED (between 0.0 and 1.0)
+    int red;
+    int blue;
+    int green;
+
+    //FOR INFRARED PHOTODIODES
+    float in1;
+    float in2;
+    float in3;
+    int turns=1; //main loop while vehicles is navigating through red/green snake
+    while(turns==1) {
+
+        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
+
+        red=rgb_data[1]; //makes it easier to interpret the data
+        green=rgb_data[2];
+        blue=rgb_data[3];
+
+        //    myservo=.35; //this is straight for the servo
+        rmv.speed(.2);//makes our vehicle go forward
+        wait(.1);
+        rmv.speed(0);
+        wait(.1);
+
+
+
+        // Print data to serial port CALIBRATION
+        //.printf( "unfiltered: %d, red: %d, green: %d, blue: %d \n\r", rgb_data[0], rgb_data[1], rgb_data[2], rgb_data[3]);
+        //  pc.printf("%d,%d,%d,%d\n", rgb_data[0], rgb_data[1], rgb_data[2], rgb_data[3]);
+        //wait(0.1);
+
+//RGB SENSOR COLOR TURN
+        if((blue>red) && (blue>green)) { //if the blue is on
+            //pc.printf("Maintain current bearing\n\r");
+            myservo=.35; //goes straight
+            wait(0.1);
+
+        }
+        if((green>red) && (green>blue)) { //if the green is on
+            //pc.printf("Turn right\n\r");
+            myservo=.01;//turns right (if motor is going backward)
+            rmv.speed(-.2);//makes our vehicle go backward
+            wait(.2);
+            rmv.speed(0);
+            wait(.1);
+
+        }
+        if((red>green) && (red>blue)) { //if the red is on
+            //pc.printf("Turn left\n\r");
+            myservo=.99; //turns left (if motor is going backwar)
+            rmv.speed(-.2);//makes our vehicle go backward
+            wait(.2);
+            rmv.speed(0);
+            wait(.1);
+
+        }
+
+        
+        //lower numbers go right, higher numbers go left. .35=straight.
+        
+        in1 = pd1.read();
+        in2 = pd2.read();
+        in3 = pd3.read();
+        //pc.printf("sensor 1: %d,sensor 2: %d,sensor 3: %d\n\r",in1,in2,in3);
+        
+        if(in1 > .6|| in2 > .6|| in3 > .6){
+            turns=0; //exits outs of main while loop, starts a new one in the turns=0 while loop (where IR sensor takes over)
+            led1=1;
+        }
+        
+    }//ends turns = 1 loop
+    
+    while(turns==0) {
+        
+        
+        
+        //color sensor stuff
+         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
+
+        red=rgb_data[1]; //makes it easier to interpret the data
+        green=rgb_data[2];
+        blue=rgb_data[3];
+        
+        
+        
+        //for infrared photodiodes
+        in1 = pd1.read();
+        in2 = pd2.read();
+        in3 = pd3.read();
+        
+        //ULTRASOUND ACTION
+        x = ted.read();
+        distance = 689.8006*pow(x,1) - 2.6437; //ultrasound distance calibration equation
+       // pc.printf("%f\n\r",distance);
+        
+        
+        //INFRARED TURN ACTION
+            if(in1 >in2 && in1 > in3) { //if the right infrared sensor is reading high
+                //pc.printf("Turn Right\n");
+                myservo=.1; //turns right if motor is going backwards
+                rmv.speed(-.2);
+                wait(.1);
+                rmv.speed(0);
+                wait(.1);
+            }
+
+            if(in2 >in1 && in2 > in3 && blue>red &&blue>green) { //if the middle infrared sensor is reading high
+                //pc.printf("Go Straight\n");
+                myservo=.35; //goes straight
+                rmv.speed(.2);
+                 wait(.1);
+                 rmv.speed(0);
+                 wait(.1);
+                
+            }
+
+            if(in3 >in1 && in3 > in2) { //if the left PD is reading high
+                //pc.printf("Turn Left\n");
+                myservo=.99; //turns left if the motor is going backwards
+                rmv.speed(-.2);
+                wait(.1);
+                rmv.speed(0);
+                wait(.1);
+            }
+        
+       if((green>red) && (green>blue)) { //if the green is on
+            //pc.printf("Turn right\n\r");
+            myservo=.01;//turns right (if motor is going backward)
+            rmv.speed(-.2);//makes our vehicle go backward
+            wait(.2);
+            rmv.speed(0);
+            wait(.1);
+
+        }
+      //  if((red>green) && (red>blue)) { //if the red is on
+      //      //pc.printf("Turn left\n\r");
+     //       myservo=.99; //turns left (if motor is going backwar)
+     //       rmv.speed(-.2);//makes our vehicle go backward
+     //       wait(.2);
+     //       rmv.speed(0);
+    //        wait(.1);
+
+    //    }
+        
+        //ultrasound stop action
+        if(distance <11) {
+            rmv.speed(0);
+            wait(10);
+        }
+    } //end turn = 0 loop
+}// end main
+
+