Final Project
Dependencies: Motor Servo TCS3472_I2C mbed
Fork of ES202_Final_Entire by
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?
User | Revision | Line number | New 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 | } |