Final Project ES202 / Mbed 2 deprecated ES202_Final_Entire

Final Project

Dependencies:   Motor Servo TCS3472_I2C mbed

Fork of ES202_Final_Entire by Robert Stroup

main.cpp@1:db5986287d13, 2015-04-28 (annotated)

Committer:
mon3ywolf
Date:
Tue Apr 28 01:50:18 2015 +0000
Revision:
1:db5986287d13
Parent:
0:267ae73c744e 
final project final edits;

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
mon3ywolf 0:267ae73c744e 16 int main() //starts the program
mon3ywolf 0:267ae73c744e 17 {
mon3ywolf 0:267ae73c744e 18 int RGB_data[4]; //declares an array to store the data read from the RGB sensor
mon3ywolf 0:267ae73c744e 19 float PWMbrightness = 1.0; //declares float for LED brightness
mon3ywolf 0:267ae73c744e 20
mon3ywolf 0:267ae73c744e 21 RGBsensor.enablePowerAndRGBC(); //enables RGB sensor
mon3ywolf 0:267ae73c744e 22 RGBsensor.setIntegrationTime(100); //sets the intergration time of the RGB sensor
mon3ywolf 0:267ae73c744e 23 RGBsensorLED = PWMbrightness; //set brightness of sensor LED
mon3ywolf 0:267ae73c744e 24 pc.baud(921600); //sets the baud rate of the serial connection
mon3ywolf 0:267ae73c744e 25 //pc.format(7,SerialBase::None,1);
mon3ywolf 0:267ae73c744e 26
mon3ywolf 0:267ae73c744e 27 s1 = .5; //starting position
mon3ywolf 0:267ae73c744e 28 wait(5.0);
mon3ywolf 0:267ae73c744e 29
mon3ywolf 0:267ae73c744e 30 while (1) { //loops the program until is broken
mon3ywolf 0:267ae73c744e 31
mon3ywolf 0:267ae73c744e 32 int region[5] = {0,0,0,0,0}; //establishes an array for the possible regions the car is in
mon3ywolf 0:267ae73c744e 33 int scan = 0; //variable for for loop
mon3ywolf 0:267ae73c744e 34 distance = 565.9273577*(range.read()) -.63993889;
mon3ywolf 0:267ae73c744e 35 for (scan=0; scan<5; scan+=1) {
mon3ywolf 0:267ae73c744e 36
mon3ywolf 0:267ae73c744e 37 RGBsensor.getAllColors(RGB_data); //read data from sensor for red, green, and blue along with magnitude
mon3ywolf 0:267ae73c744e 38
mon3ywolf 0:267ae73c744e 39 //pc.printf("%d,%d,%d,%d\n", RGB_data[0], RGB_data[1], RGB_data[2], RGB_data[3]);
mon3ywolf 0:267ae73c744e 40 //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 41
mon3ywolf 0:267ae73c744e 42 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 43 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 44 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 45 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 46
mon3ywolf 0:267ae73c744e 47 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 48 region[0] += 1; //car is in red
mon3ywolf 0:267ae73c744e 49 }
mon3ywolf 0:267ae73c744e 50 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 51 region[1] += 1; //car is in red and blue
mon3ywolf 0:267ae73c744e 52 }
mon3ywolf 0:267ae73c744e 53 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 54 region[2] += 1; //car is in blue
mon3ywolf 0:267ae73c744e 55 }
mon3ywolf 0:267ae73c744e 56 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 57 region[3] += 1; //car is in blue and green
mon3ywolf 0:267ae73c744e 58 }
mon3ywolf 0:267ae73c744e 59 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 60 region[4] += 1; //car is in green
mon3ywolf 0:267ae73c744e 61 }
mon3ywolf 0:267ae73c744e 62 }
mon3ywolf 0:267ae73c744e 63
mon3ywolf 0:267ae73c744e 64 if (region[0] >= 4) { //data reads in the red
mon3ywolf 0:267ae73c744e 65 pc.printf("Red Region. Make Right Turn. \n"); //print to tera term
mon3ywolf 0:267ae73c744e 66 s1 = .75; //make right turn
mon3ywolf 0:267ae73c744e 67 // m1.speed(.22);
mon3ywolf 0:267ae73c744e 68 // wait(.25);
mon3ywolf 0:267ae73c744e 69 // m1.speed(0.0);
mon3ywolf 0:267ae73c744e 70 // s1 = .5; //straighten it out
mon3ywolf 0:267ae73c744e 71 }
mon3ywolf 0:267ae73c744e 72 if (region[1] >= 4) { //data reads on the red/blue line
mon3ywolf 0:267ae73c744e 73 pc.printf("Red/Blue Region. Make Slight Right Turn. \n"); //print to tera term
mon3ywolf 0:267ae73c744e 74 s1 = .625; //make slight right turn
mon3ywolf 0:267ae73c744e 75 m1.speed(.22);
mon3ywolf 0:267ae73c744e 76 //wait(.25);
mon3ywolf 0:267ae73c744e 77 // m1.speed(0.0);
mon3ywolf 0:267ae73c744e 78 // s1 = .5; //straighten it out
mon3ywolf 0:267ae73c744e 79 }
mon3ywolf 0:267ae73c744e 80 if (region[2] >= 4) { //data reads in the blue
mon3ywolf 0:267ae73c744e 81 pc.printf("Blue Region. Drive Straight. We Cruising. \n"); //print to tera term
mon3ywolf 0:267ae73c744e 82 s1 = .5; //make no turn
mon3ywolf 0:267ae73c744e 83 //m1.speed(.22);
mon3ywolf 0:267ae73c744e 84 // wait(.25);
mon3ywolf 0:267ae73c744e 85 // m1.speed(0.0);
mon3ywolf 0:267ae73c744e 86 // s1 = .5; //straighten it out
mon3ywolf 0:267ae73c744e 87 }
mon3ywolf 0:267ae73c744e 88 if (region[3] >= 4) { //data reads on the green/blue line
mon3ywolf 0:267ae73c744e 89 pc.printf("Green/Blue Region. Make Slight Left Turn. \n"); //print to tera term
mon3ywolf 0:267ae73c744e 90 s1 = .375; //make slight left turn
mon3ywolf 0:267ae73c744e 91 // m1.speed(.22);
mon3ywolf 0:267ae73c744e 92 // wait(.25);
mon3ywolf 0:267ae73c744e 93 // m1.speed(0.0);
mon3ywolf 0:267ae73c744e 94 // s1 = .5; //straighten it out
mon3ywolf 0:267ae73c744e 95 }
mon3ywolf 0:267ae73c744e 96 if (region[4] >= 4) { //data reads in the green
mon3ywolf 0:267ae73c744e 97 pc.printf("Green Region. Make Left Turn. \n"); //print to tera term
mon3ywolf 0:267ae73c744e 98 s1 = .25; //make left turn
mon3ywolf 0:267ae73c744e 99 // m1.speed(.22);
mon3ywolf 0:267ae73c744e 100 // wait(.25);
mon3ywolf 0:267ae73c744e 101 //m1.speed(0.0);
mon3ywolf 0:267ae73c744e 102 // s1 = .5; //straighten it out
mon3ywolf 0:267ae73c744e 103 }
mon3ywolf 0:267ae73c744e 104 if ((region[0] < 4)&&(region[1] < 4)&&(region[2] < 4)&&(region[3] < 4)&&(region[4] < 4)) {
mon3ywolf 0:267ae73c744e 105 //pc.printf("No region. Bad data. \n"); //prints to tera term
mon3ywolf 0:267ae73c744e 106 // m1.speed(0.0);
mon3ywolf 0:267ae73c744e 107 }
mon3ywolf 1:db5986287d13 108 distance = 565.9273577*(range.read()) -.63993889; //calculate the distance with the range sensor using a calibrated curve
mon3ywolf 1:db5986287d13 109 if(distance>15) { //if the obstacle is still very far away
mon3ywolf 1:db5986287d13 110 m1.speed(0.5); //move the motor forward (It will turn according to the RGB servo motor command above)
mon3ywolf 1:db5986287d13 111 wait(0.1);
mon3ywolf 1:db5986287d13 112 m1.speed(0); //stop the motor and go back to the RGB sensor
mon3ywolf 1:db5986287d13 113 } else {
mon3ywolf 1:db5986287d13 114 while(distance>8) { //slow the movement down once you start to get close and no longer use the RGB sensor for directtion
mon3ywolf 1:db5986287d13 115 s1 = 0.5; //make it go straight towards the obstacle.
mon3ywolf 1:db5986287d13 116 m1.speed(0.5);
mon3ywolf 1:db5986287d13 117 wait(0.05);
mon3ywolf 1:db5986287d13 118 m1.speed(0);
mon3ywolf 1:db5986287d13 119 }
mon3ywolf 1:db5986287d13 120 while(1) {
mon3ywolf 1:db5986287d13 121 printf("Complete!\n");//entire an infinite loop so it doesn't move again after reaching the objective
mon3ywolf 1:db5986287d13 122 wait(1);
mon3ywolf 1:db5986287d13 123 }
mon3ywolf 0:267ae73c744e 124
mon3ywolf 1:db5986287d13 125 }
mon3ywolf 0:267ae73c744e 126 }
mon3ywolf 1:db5986287d13 127 }