Final Project / Mbed 2 deprecated FINAL_PROJECT

HI

Dependencies:   Motor TCS3472_I2C mbed

main.cpp@2:a681e7e4bd72, 2015-04-27 (annotated)

Committer:
taylormooers
Date:
Mon Apr 27 01:17:48 2015 +0000
Revision:
2:a681e7e4bd72
Parent:
1:9eb8d4e6a719 
newest version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
taylormooers 0:1cbed7d36912 1 #include "mbed.h" //includes this library for all mbed functions
taylormooers 0:1cbed7d36912 2 #include "TCS3472_I2C.h" //includes the library for the RGB sensor
taylormooers 0:1cbed7d36912 3 #include "Motor.h"
taylormooers 1:9eb8d4e6a719 4 AnalogIn sonic(p15);
taylormooers 2:a681e7e4bd72 5 Motor R(p26, p30, p29);
taylormooers 2:a681e7e4bd72 6 Motor L(p25, p27, p28);
taylormooers 0:1cbed7d36912 7
taylormooers 1:9eb8d4e6a719 8 float data; //ULTRASONIC SENSOR
taylormooers 1:9eb8d4e6a719 9 float sum;
taylormooers 1:9eb8d4e6a719 10 float average;
taylormooers 1:9eb8d4e6a719 11 int p;
taylormooers 1:9eb8d4e6a719 12 int count;
taylormooers 1:9eb8d4e6a719 13
taylormooers 0:1cbed7d36912 14 PwmOut RGBsensorLED(p22); //established pin to power the LED on the sensor
taylormooers 0:1cbed7d36912 15 TCS3472_I2C RGBsensor(p9, p10); //establishes pins for the RGB sensor.
taylormooers 0:1cbed7d36912 16
taylormooers 0:1cbed7d36912 17
taylormooers 0:1cbed7d36912 18 Serial pc(USBTX, USBRX); //establishes a serial port in order to communicate with TeraTerm and LATLAB
taylormooers 0:1cbed7d36912 19
taylormooers 0:1cbed7d36912 20 int main() //starts the program
taylormooers 0:1cbed7d36912 21 {
taylormooers 1:9eb8d4e6a719 22
taylormooers 0:1cbed7d36912 23 int RGB_data[4]; //declares an array to store the data read from the RGB sensor
taylormooers 0:1cbed7d36912 24 float PWMbrightness = 1.0; //declares float for LED brightness
taylormooers 0:1cbed7d36912 25
taylormooers 0:1cbed7d36912 26 RGBsensor.enablePowerAndRGBC(); //enables RGB sensor
taylormooers 0:1cbed7d36912 27 RGBsensor.setIntegrationTime(100); //sets the intergration time of the RGB sensor
taylormooers 0:1cbed7d36912 28 RGBsensorLED = PWMbrightness; //set brightness of sensor LED
taylormooers 0:1cbed7d36912 29 pc.baud(921600); //sets the baud rate of the serial connection
taylormooers 0:1cbed7d36912 30
taylormooers 0:1cbed7d36912 31 while (1) { //loops the program until is broken
taylormooers 1:9eb8d4e6a719 32 data=sonic.read();
taylormooers 1:9eb8d4e6a719 33 sum=0;
taylormooers 1:9eb8d4e6a719 34 for(p=0; p<=50; p++) {
taylormooers 1:9eb8d4e6a719 35 sum =sum+data;
taylormooers 1:9eb8d4e6a719 36 }
taylormooers 1:9eb8d4e6a719 37 average= sum/50;
taylormooers 1:9eb8d4e6a719 38 printf(" average is %f \n",average);
taylormooers 0:1cbed7d36912 39
taylormooers 1:9eb8d4e6a719 40 if(average> 0.026) {
taylormooers 1:9eb8d4e6a719 41 int colorcount[3] = {0,0,0}; //establishes an array for the possible regions the car is in
taylormooers 1:9eb8d4e6a719 42 int scan = 0; //variable for for loop
taylormooers 1:9eb8d4e6a719 43
taylormooers 1:9eb8d4e6a719 44 for (scan=0; scan<10; scan+=1) {
taylormooers 1:9eb8d4e6a719 45
taylormooers 1:9eb8d4e6a719 46 RGBsensor.getAllColors(RGB_data); //read data from sensor for red, green, and blue along with magnitude
taylormooers 1:9eb8d4e6a719 47
taylormooers 1:9eb8d4e6a719 48 int light = RGB_data[0]; //sets a variable for the light data so I don't have to type that every time
taylormooers 1:9eb8d4e6a719 49 int red = RGB_data[1]; //sets a variable for the red data so I don't have to type that every time
taylormooers 1:9eb8d4e6a719 50 int green = RGB_data[2]; //sets a variable for the blue data so I don't have to type that every time
taylormooers 1:9eb8d4e6a719 51 int blue = RGB_data[3]; //sets a variable for the green data so I don't have to type that every time
taylormooers 0:1cbed7d36912 52
taylormooers 1:9eb8d4e6a719 53 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) ) {
taylormooers 1:9eb8d4e6a719 54 colorcount[0] += 1; //car is in red
taylormooers 1:9eb8d4e6a719 55 L.speed(0.0);
taylormooers 2:a681e7e4bd72 56 R.speed(0.37);
taylormooers 1:9eb8d4e6a719 57 }
taylormooers 1:9eb8d4e6a719 58
taylormooers 1:9eb8d4e6a719 59 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) ) {
taylormooers 1:9eb8d4e6a719 60 colorcount[1] += 1; //car is in blue
taylormooers 2:a681e7e4bd72 61 R.speed(0.4);
taylormooers 2:a681e7e4bd72 62 L.speed(-0.45);
taylormooers 1:9eb8d4e6a719 63 }
taylormooers 1:9eb8d4e6a719 64
taylormooers 1:9eb8d4e6a719 65 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) ) {
taylormooers 1:9eb8d4e6a719 66 colorcount[2] += 1; //car is in green
taylormooers 2:a681e7e4bd72 67 L.speed(-0.37);
taylormooers 1:9eb8d4e6a719 68 R.speed(0.0);
taylormooers 1:9eb8d4e6a719 69 }
taylormooers 0:1cbed7d36912 70 }
taylormooers 1:9eb8d4e6a719 71 if (colorcount[0] >= 7) { //data reads in the red
taylormooers 1:9eb8d4e6a719 72 pc.printf("Making a right turn. \n"); //print to tera term
taylormooers 1:9eb8d4e6a719 73
taylormooers 0:1cbed7d36912 74 }
taylormooers 1:9eb8d4e6a719 75 if (colorcount[1] >= 7) { //data reads in the blue
taylormooers 1:9eb8d4e6a719 76 pc.printf("Driving straight. \n"); //print to tera term
taylormooers 1:9eb8d4e6a719 77
taylormooers 1:9eb8d4e6a719 78 }
taylormooers 1:9eb8d4e6a719 79 if (colorcount[2] >= 7) { //data reads in the green
taylormooers 1:9eb8d4e6a719 80 pc.printf("Making a left turn. \n"); //print to tera term
taylormooers 1:9eb8d4e6a719 81
taylormooers 0:1cbed7d36912 82 }
taylormooers 0:1cbed7d36912 83 }
taylormooers 1:9eb8d4e6a719 84 if ((average> 0.014) && (average < 0.026)) {
taylormooers 1:9eb8d4e6a719 85
taylormooers 2:a681e7e4bd72 86 L.speed(-0.2);
taylormooers 1:9eb8d4e6a719 87 R.speed(0.2);
taylormooers 1:9eb8d4e6a719 88 printf("%f \n",average);
taylormooers 0:1cbed7d36912 89 }
taylormooers 1:9eb8d4e6a719 90 if(average< 0.014) {
taylormooers 1:9eb8d4e6a719 91 wait(0.1);
taylormooers 1:9eb8d4e6a719 92 L.speed(0.0);
taylormooers 1:9eb8d4e6a719 93 R.speed(0.0);
taylormooers 0:1cbed7d36912 94 }
taylormooers 0:1cbed7d36912 95 }
taylormooers 0:1cbed7d36912 96
taylormooers 0:1cbed7d36912 97 }