Diff: main.cpp

Revision:

0:a4ce7d166c62

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Apr 28 01:46:49 2015 +0000
@@ -0,0 +1,118 @@
+//Ben Keegan, Mitch Bond, Nick Dilleumber, Rixon Fletcher
+//ES202 Principles of Mechatronics
+//Autonomous Vehicle Project
+//26 APR 2015
+
+#include "mbed.h"
+#include "Servo.h"
+#include "Motor.h"
+#include "TCS3472_I2C.h"
+
+//Actuators and Ultrasonic Sensor Pins
+Motor M(p26, p29, p30);
+Servo S(p23);
+AnalogIn coke(p20); // sets analog in to pin 20
+//Color Sensor Pins
+PwmOut LB(p22); // PWM out signal to power LED on the sensor
+TCS3472_I2C rgb_sensor( p9, p10 ); // Establish RGB sensor object
+//IR Sensor Pins
+AnalogIn photod1(p15);
+AnalogIn photod2(p16);
+AnalogIn photod3(p17);
+
+//Serial pc(USBTX,USBRX); // Establish serial connection with computer
+
+float volt = 0.0; // float volt
+int distance = 100.0; //float distance set at a value that will allow for initial entry into loop
+float cof[2] = {489.1001, 2.0247}; // coefficients from matlab calibration
+int count = 0;
+int there = 0;
+
+int main()
+{
+    rgb_sensor.enablePowerAndRGBC(); // Enable RGB sensor
+    rgb_sensor.setIntegrationTime(100); // Set integration time of sensor
+
+    //pc.baud(921600); // 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 = 0.75; // declare a float specifying brightness of LED (between 0.0 and 1.0)
+    float r; //photodiode 1
+    float m; //photodiode 2
+    float l; //photodiode 3
+    int mult=1000; //multiplier to amplify signal
+
+    S= 0.63;
+    wait(0.1);
+    M.speed(0);
+    while(!there) {// While the distance to the front is sufficient
+        volt = coke;
+        distance = floor(cof[0]*pow(volt,1) + cof[1]); // determines distance based on calibration coefficients and rounds it down
+        
+        r=mult*photod1.read();
+        m=mult*photod2.read();
+        l=mult*photod3.read();
+        
+        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
+        //int unf = rgb_data[0];
+        int red = rgb_data[1];
+        int green = rgb_data[2];
+        int blue = rgb_data[3];
+        //pc.printf("%d %d %d\n",red, green, blue);
+        // Print data to serial port
+        //pc.printf( "unfiltered: %d, red: %d, green: %d, blue: %d \n", rgb_data[0], rgb_data[1], rgb_data[2], rgb_data[3]);
+        M.speed(-0.2);
+        
+        if (red > 150 && green < 100){
+            S = 0.34;
+            M.speed(0.25);
+            wait(.5);
+            S= 0.84;
+            //M.speed(-0.2);
+            M.speed(-0.25);
+            //wait(0.1);
+            //M.speed(0);
+            //wait(0.1);
+            //S =0.63;
+            //M.speed(-0.2);
+            //pc.printf("Turn Left\n");
+        }
+        else if(green > 200){ //&& green < 200
+            S = 0.84;
+            M.speed(0.25);
+            wait(0.5);
+            S= 0.34;
+            M.speed(-0.25);
+            //m.speed(0.1);
+            //wait(2);
+            //S =0.63;
+            //m.speed(-0.05);
+            //pc.printf("Turn Right\n");
+            }
+        else { /*(green-red)<50 && (blue-green)<50)*/
+            //pc.printf("Go Straight\n");
+            S= 0.63;
+            //M.speed(-0.175);
+        }
+        
+        if (count > 10000){ //Ensure vehicle won't stop short due to people in way when it starts
+            if (distance < 17){
+                M.speed(0);
+                if (r > 200){ //Adjust to face lighthouse if right photodiode reads high
+                    S = 0.84;
+                    M.speed(0.25);
+                    wait(0.5);
+                    S= 0.34;
+                    M.speed(-0.25);
+                    wait(0.05);
+                    M.speed(0); 
+                }
+                there = 1;
+            }
+        }
+        count +=1;
+    }// end while
+    M.speed(0);
+    //pc.printf("Stop/n");
+} // end main
\ No newline at end of file