Robert Stroup / Mbed 2 deprecated ES202_Final_Entire

Final Project

Dependencies:   Motor Servo TCS3472_I2C mbed

main.cpp@0:267ae73c744e, 2015-04-28 (annotated)

Committer:
mon3ywolf
Date:
Tue Apr 28 01:24:23 2015 +0000
Revision:
0:267ae73c744e
Child:
1:db5986287d13
Code for project

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mon3ywolf 0:267ae73c744e 1 #include "mbed.h" //includes this library for all mbed functions
mon3ywolf 0:267ae73c744e 2 #include "TCS3472_I2C.h" //includes the library for the RGB sensor
mon3ywolf 0:267ae73c744e 3 #include "Servo.h" //includes the library for the servo motor
mon3ywolf 0:267ae73c744e 4 #include "Motor.h" //includes library for the dc motor
mon3ywolf 0:267ae73c744e 5
mon3ywolf 0:267ae73c744e 6 PwmOut RGBsensorLED(p22); //established pin to power the LED on the sensor
mon3ywolf 0:267ae73c744e 7 TCS3472_I2C RGBsensor(p9, p10); //establishes pins for the RGB sensor.
mon3ywolf 0:267ae73c744e 8 Servo s1(p24); //establishes pin for servo motor
mon3ywolf 0:267ae73c744e 9 Motor m1(p26,p29,p30); //establishes pins for the dc motor
mon3ywolf 0:267ae73c744e 10
mon3ywolf 0:267ae73c744e 11
mon3ywolf 0:267ae73c744e 12 AnalogIn range(p19);
mon3ywolf 0:267ae73c744e 13 Serial pc(USBTX, USBRX);
mon3ywolf 0:267ae73c744e 14 float distance;
mon3ywolf 0:267ae73c744e 15 float new_distance;
mon3ywolf 0:267ae73c744e 16
mon3ywolf 0:267ae73c744e 17 int main() //starts the program
mon3ywolf 0:267ae73c744e 18 {
mon3ywolf 0:267ae73c744e 19 int RGB_data[4]; //declares an array to store the data read from the RGB sensor
mon3ywolf 0:267ae73c744e 20 float PWMbrightness = 1.0; //declares float for LED brightness
mon3ywolf 0:267ae73c744e 21
mon3ywolf 0:267ae73c744e 22 RGBsensor.enablePowerAndRGBC(); //enables RGB sensor
mon3ywolf 0:267ae73c744e 23 RGBsensor.setIntegrationTime(100); //sets the intergration time of the RGB sensor
mon3ywolf 0:267ae73c744e 24 RGBsensorLED = PWMbrightness; //set brightness of sensor LED
mon3ywolf 0:267ae73c744e 25 pc.baud(921600); //sets the baud rate of the serial connection
mon3ywolf 0:267ae73c744e 26 //pc.format(7,SerialBase::None,1);
mon3ywolf 0:267ae73c744e 27
mon3ywolf 0:267ae73c744e 28 s1 = .5; //starting position
mon3ywolf 0:267ae73c744e 29 wait(5.0);
mon3ywolf 0:267ae73c744e 30
mon3ywolf 0:267ae73c744e 31 while (1) { //loops the program until is broken
mon3ywolf 0:267ae73c744e 32
mon3ywolf 0:267ae73c744e 33 int region[5] = {0,0,0,0,0}; //establishes an array for the possible regions the car is in
mon3ywolf 0:267ae73c744e 34 int scan = 0; //variable for for loop
mon3ywolf 0:267ae73c744e 35 distance = 565.9273577*(range.read()) -.63993889;
mon3ywolf 0:267ae73c744e 36 for (scan=0; scan<5; scan+=1) {
mon3ywolf 0:267ae73c744e 37
mon3ywolf 0:267ae73c744e 38 RGBsensor.getAllColors(RGB_data); //read data from sensor for red, green, and blue along with magnitude
mon3ywolf 0:267ae73c744e 39
mon3ywolf 0:267ae73c744e 40 //pc.printf("%d,%d,%d,%d\n", RGB_data[0], RGB_data[1], RGB_data[2], RGB_data[3]);
mon3ywolf 0:267ae73c744e 41 //pc.printf("unfiltered: %d, red: %d, green: %d, blue: %d \n", RGB_data[0], RGB_data[1], RGB_data[2], RGB_data[3]);
mon3ywolf 0:267ae73c744e 42
mon3ywolf 0:267ae73c744e 43 int Unfil = RGB_data[0]; //sets a variable for the light data so I don't have to type that every time
mon3ywolf 0:267ae73c744e 44 int Red = RGB_data[1]; //sets a variable for the red data so I don't have to type that every time
mon3ywolf 0:267ae73c744e 45 int Green = RGB_data[2]; //sets a variable for the blue data so I don't have to type that every time
mon3ywolf 0:267ae73c744e 46 int Blue = RGB_data[3]; //sets a variable for the green data so I don't have to type that every time
mon3ywolf 0:267ae73c744e 47
mon3ywolf 0:267ae73c744e 48 if ( (abs((1.7574*Red + 3.9239)-Unfil) < 13) && (abs((4.0508*Green - 32.749)-Unfil) < 13) && (abs((5.959*Blue - 21.767 )-Unfil) < 13) ) {
mon3ywolf 0:267ae73c744e 49 region[0] += 1; //car is in red
mon3ywolf 0:267ae73c744e 50 }
mon3ywolf 0:267ae73c744e 51 if ( (abs((2.3592*Red - 40.53 )-Unfil) < 13) && (abs((3.6436*Green - 17.036 )-Unfil) < 13) && (abs((3.6735*Blue + 40.292)-Unfil) < 13) ) {
mon3ywolf 0:267ae73c744e 52 region[1] += 1; //car is in red and blue
mon3ywolf 0:267ae73c744e 53 }
mon3ywolf 0:267ae73c744e 54 if ( (abs((3.6835*Red - 130.71)-Unfil) < 13) && (abs((3.4125*Green - 11.762 )-Unfil) < 13) && (abs((2.542*Blue + 64.922)-Unfil) < 13) ) {
mon3ywolf 0:267ae73c744e 55 region[2] += 1; //car is in blue
mon3ywolf 0:267ae73c744e 56 }
mon3ywolf 0:267ae73c744e 57 if ( (abs((3.5816*Red - 120.33 )-Unfil) < 13) && (abs((2.4781*Green + 22.624)-Unfil) < 13) && (abs((3.6996*Blue + 37.829)-Unfil) < 13) ) {
mon3ywolf 0:267ae73c744e 58 region[3] += 1; //car is in blue and green
mon3ywolf 0:267ae73c744e 59 }
mon3ywolf 0:267ae73c744e 60 if ( (abs((3.6619*Red - 126.41)-Unfil) < 13) && (abs((2.109*Green + 40.777)-Unfil) < 13) && (abs((5.0975*Blue - 1.0619)-Unfil) < 13) ) {
mon3ywolf 0:267ae73c744e 61 region[4] += 1; //car is in green
mon3ywolf 0:267ae73c744e 62 }
mon3ywolf 0:267ae73c744e 63 }
mon3ywolf 0:267ae73c744e 64
mon3ywolf 0:267ae73c744e 65 if (region[0] >= 4) { //data reads in the red
mon3ywolf 0:267ae73c744e 66 pc.printf("Red Region. Make Right Turn. \n"); //print to tera term
mon3ywolf 0:267ae73c744e 67 s1 = .75; //make right turn
mon3ywolf 0:267ae73c744e 68 // m1.speed(.22);
mon3ywolf 0:267ae73c744e 69 // wait(.25);
mon3ywolf 0:267ae73c744e 70 // m1.speed(0.0);
mon3ywolf 0:267ae73c744e 71 // s1 = .5; //straighten it out
mon3ywolf 0:267ae73c744e 72 }
mon3ywolf 0:267ae73c744e 73 if (region[1] >= 4) { //data reads on the red/blue line
mon3ywolf 0:267ae73c744e 74 pc.printf("Red/Blue Region. Make Slight Right Turn. \n"); //print to tera term
mon3ywolf 0:267ae73c744e 75 s1 = .625; //make slight right turn
mon3ywolf 0:267ae73c744e 76 m1.speed(.22);
mon3ywolf 0:267ae73c744e 77 //wait(.25);
mon3ywolf 0:267ae73c744e 78 // m1.speed(0.0);
mon3ywolf 0:267ae73c744e 79 // s1 = .5; //straighten it out
mon3ywolf 0:267ae73c744e 80 }
mon3ywolf 0:267ae73c744e 81 if (region[2] >= 4) { //data reads in the blue
mon3ywolf 0:267ae73c744e 82 pc.printf("Blue Region. Drive Straight. We Cruising. \n"); //print to tera term
mon3ywolf 0:267ae73c744e 83 s1 = .5; //make no turn
mon3ywolf 0:267ae73c744e 84 //m1.speed(.22);
mon3ywolf 0:267ae73c744e 85 // wait(.25);
mon3ywolf 0:267ae73c744e 86 // m1.speed(0.0);
mon3ywolf 0:267ae73c744e 87 // s1 = .5; //straighten it out
mon3ywolf 0:267ae73c744e 88 }
mon3ywolf 0:267ae73c744e 89 if (region[3] >= 4) { //data reads on the green/blue line
mon3ywolf 0:267ae73c744e 90 pc.printf("Green/Blue Region. Make Slight Left Turn. \n"); //print to tera term
mon3ywolf 0:267ae73c744e 91 s1 = .375; //make slight left turn
mon3ywolf 0:267ae73c744e 92 // m1.speed(.22);
mon3ywolf 0:267ae73c744e 93 // wait(.25);
mon3ywolf 0:267ae73c744e 94 // m1.speed(0.0);
mon3ywolf 0:267ae73c744e 95 // s1 = .5; //straighten it out
mon3ywolf 0:267ae73c744e 96 }
mon3ywolf 0:267ae73c744e 97 if (region[4] >= 4) { //data reads in the green
mon3ywolf 0:267ae73c744e 98 pc.printf("Green Region. Make Left Turn. \n"); //print to tera term
mon3ywolf 0:267ae73c744e 99 s1 = .25; //make left turn
mon3ywolf 0:267ae73c744e 100 // m1.speed(.22);
mon3ywolf 0:267ae73c744e 101 // wait(.25);
mon3ywolf 0:267ae73c744e 102 //m1.speed(0.0);
mon3ywolf 0:267ae73c744e 103 // s1 = .5; //straighten it out
mon3ywolf 0:267ae73c744e 104 }
mon3ywolf 0:267ae73c744e 105 if ((region[0] < 4)&&(region[1] < 4)&&(region[2] < 4)&&(region[3] < 4)&&(region[4] < 4)) {
mon3ywolf 0:267ae73c744e 106 //pc.printf("No region. Bad data. \n"); //prints to tera term
mon3ywolf 0:267ae73c744e 107 // m1.speed(0.0);
mon3ywolf 0:267ae73c744e 108 }
mon3ywolf 0:267ae73c744e 109
mon3ywolf 0:267ae73c744e 110
mon3ywolf 0:267ae73c744e 111 }
mon3ywolf 0:267ae73c744e 112 }