Final Project ES202 Code / Mbed 2 deprecated FINALPROJECT_ES202

Our final project code

Dependencies:   Motor Servo TCS3472_I2C mbed

main.cpp@0:0a03e4314cb7, 2015-04-27 (annotated)

Committer:
westobrien
Date:
Mon Apr 27 21:12:28 2015 +0000
Revision:
0:0a03e4314cb7
Final Project code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
westobrien 0:0a03e4314cb7 1 #include "mbed.h"
westobrien 0:0a03e4314cb7 2 #include "TCS3472_I2C.h"
westobrien 0:0a03e4314cb7 3 #include "Servo.h"
westobrien 0:0a03e4314cb7 4 #include "Motor.h"
westobrien 0:0a03e4314cb7 5
westobrien 0:0a03e4314cb7 6 PwmOut LB(p22); // PWM out signal to power LED on the sensor
westobrien 0:0a03e4314cb7 7 TCS3472_I2C rgb_sensor( p9, p10 ); // Establish RGB sensor object
westobrien 0:0a03e4314cb7 8 Serial pc(USBTX,USBRX); // Establish serial connection with computer
westobrien 0:0a03e4314cb7 9 Motor rmv(p26,p29,p30); //sets up DC motor
westobrien 0:0a03e4314cb7 10 Servo myservo(p21); //sets up the servo motor
westobrien 0:0a03e4314cb7 11 DigitalOut led1(LED1);
westobrien 0:0a03e4314cb7 12
westobrien 0:0a03e4314cb7 13 //This is all for the photo diodes
westobrien 0:0a03e4314cb7 14 AnalogIn pd1(p17);
westobrien 0:0a03e4314cb7 15 AnalogIn pd2(p16);
westobrien 0:0a03e4314cb7 16 AnalogIn pd3(p15);
westobrien 0:0a03e4314cb7 17
westobrien 0:0a03e4314cb7 18 //This is all for the ultrasound sensor
westobrien 0:0a03e4314cb7 19 AnalogIn ted(p18); //sets up the ultrasound sensor
westobrien 0:0a03e4314cb7 20 float x;
westobrien 0:0a03e4314cb7 21 Timer t;
westobrien 0:0a03e4314cb7 22 float data = x;
westobrien 0:0a03e4314cb7 23 float distance;
westobrien 0:0a03e4314cb7 24
westobrien 0:0a03e4314cb7 25
westobrien 0:0a03e4314cb7 26 int main()
westobrien 0:0a03e4314cb7 27 {
westobrien 0:0a03e4314cb7 28 rgb_sensor.enablePowerAndRGBC(); // Enable RGB sensor
westobrien 0:0a03e4314cb7 29 rgb_sensor.setIntegrationTime(100); // Set integration time of sensor
westobrien 0:0a03e4314cb7 30
westobrien 0:0a03e4314cb7 31 pc.baud(9600); // Set Baud rate of serial connection
westobrien 0:0a03e4314cb7 32 pc.format(8, SerialBase::None,1);
westobrien 0:0a03e4314cb7 33
westobrien 0:0a03e4314cb7 34 int rgb_data[4]; // declare a 4 element array to store RGB sensor readings
westobrien 0:0a03e4314cb7 35 float PWMbrightness = 1.0; // declare a float specifying brightness of LED (between 0.0 and 1.0)
westobrien 0:0a03e4314cb7 36 int red;
westobrien 0:0a03e4314cb7 37 int blue;
westobrien 0:0a03e4314cb7 38 int green;
westobrien 0:0a03e4314cb7 39
westobrien 0:0a03e4314cb7 40 //FOR INFRARED PHOTODIODES
westobrien 0:0a03e4314cb7 41 float in1;
westobrien 0:0a03e4314cb7 42 float in2;
westobrien 0:0a03e4314cb7 43 float in3;
westobrien 0:0a03e4314cb7 44 int turns=1; //main loop while vehicles is navigating through red/green snake
westobrien 0:0a03e4314cb7 45 while(turns==1) {
westobrien 0:0a03e4314cb7 46
westobrien 0:0a03e4314cb7 47 LB = PWMbrightness; // set brightness of sensor LED
westobrien 0:0a03e4314cb7 48 rgb_sensor.getAllColors( rgb_data ); // read the sensor to get red, green, and blue color data along with overall brightness
westobrien 0:0a03e4314cb7 49
westobrien 0:0a03e4314cb7 50 red=rgb_data[1]; //makes it easier to interpret the data
westobrien 0:0a03e4314cb7 51 green=rgb_data[2];
westobrien 0:0a03e4314cb7 52 blue=rgb_data[3];
westobrien 0:0a03e4314cb7 53
westobrien 0:0a03e4314cb7 54 // myservo=.35; //this is straight for the servo
westobrien 0:0a03e4314cb7 55 rmv.speed(.2);//makes our vehicle go forward
westobrien 0:0a03e4314cb7 56 wait(.1);
westobrien 0:0a03e4314cb7 57 rmv.speed(0);
westobrien 0:0a03e4314cb7 58 wait(.1);
westobrien 0:0a03e4314cb7 59
westobrien 0:0a03e4314cb7 60
westobrien 0:0a03e4314cb7 61
westobrien 0:0a03e4314cb7 62 // Print data to serial port CALIBRATION
westobrien 0:0a03e4314cb7 63 //.printf( "unfiltered: %d, red: %d, green: %d, blue: %d \n\r", rgb_data[0], rgb_data[1], rgb_data[2], rgb_data[3]);
westobrien 0:0a03e4314cb7 64 // pc.printf("%d,%d,%d,%d\n", rgb_data[0], rgb_data[1], rgb_data[2], rgb_data[3]);
westobrien 0:0a03e4314cb7 65 //wait(0.1);
westobrien 0:0a03e4314cb7 66
westobrien 0:0a03e4314cb7 67 //RGB SENSOR COLOR TURN
westobrien 0:0a03e4314cb7 68 if((blue>red) && (blue>green)) { //if the blue is on
westobrien 0:0a03e4314cb7 69 //pc.printf("Maintain current bearing\n\r");
westobrien 0:0a03e4314cb7 70 myservo=.35; //goes straight
westobrien 0:0a03e4314cb7 71 wait(0.1);
westobrien 0:0a03e4314cb7 72
westobrien 0:0a03e4314cb7 73 }
westobrien 0:0a03e4314cb7 74 if((green>red) && (green>blue)) { //if the green is on
westobrien 0:0a03e4314cb7 75 //pc.printf("Turn right\n\r");
westobrien 0:0a03e4314cb7 76 myservo=.01;//turns right (if motor is going backward)
westobrien 0:0a03e4314cb7 77 rmv.speed(-.2);//makes our vehicle go backward
westobrien 0:0a03e4314cb7 78 wait(.2);
westobrien 0:0a03e4314cb7 79 rmv.speed(0);
westobrien 0:0a03e4314cb7 80 wait(.1);
westobrien 0:0a03e4314cb7 81
westobrien 0:0a03e4314cb7 82 }
westobrien 0:0a03e4314cb7 83 if((red>green) && (red>blue)) { //if the red is on
westobrien 0:0a03e4314cb7 84 //pc.printf("Turn left\n\r");
westobrien 0:0a03e4314cb7 85 myservo=.99; //turns left (if motor is going backwar)
westobrien 0:0a03e4314cb7 86 rmv.speed(-.2);//makes our vehicle go backward
westobrien 0:0a03e4314cb7 87 wait(.2);
westobrien 0:0a03e4314cb7 88 rmv.speed(0);
westobrien 0:0a03e4314cb7 89 wait(.1);
westobrien 0:0a03e4314cb7 90
westobrien 0:0a03e4314cb7 91 }
westobrien 0:0a03e4314cb7 92
westobrien 0:0a03e4314cb7 93
westobrien 0:0a03e4314cb7 94 //lower numbers go right, higher numbers go left. .35=straight.
westobrien 0:0a03e4314cb7 95
westobrien 0:0a03e4314cb7 96 in1 = pd1.read();
westobrien 0:0a03e4314cb7 97 in2 = pd2.read();
westobrien 0:0a03e4314cb7 98 in3 = pd3.read();
westobrien 0:0a03e4314cb7 99 //pc.printf("sensor 1: %d,sensor 2: %d,sensor 3: %d\n\r",in1,in2,in3);
westobrien 0:0a03e4314cb7 100
westobrien 0:0a03e4314cb7 101 if(in1 > .6|| in2 > .6|| in3 > .6){
westobrien 0:0a03e4314cb7 102 turns=0; //exits outs of main while loop, starts a new one in the turns=0 while loop (where IR sensor takes over)
westobrien 0:0a03e4314cb7 103 led1=1;
westobrien 0:0a03e4314cb7 104 }
westobrien 0:0a03e4314cb7 105
westobrien 0:0a03e4314cb7 106 }//ends turns = 1 loop
westobrien 0:0a03e4314cb7 107
westobrien 0:0a03e4314cb7 108 while(turns==0) {
westobrien 0:0a03e4314cb7 109
westobrien 0:0a03e4314cb7 110
westobrien 0:0a03e4314cb7 111
westobrien 0:0a03e4314cb7 112 //color sensor stuff
westobrien 0:0a03e4314cb7 113 LB = PWMbrightness; // set brightness of sensor LED
westobrien 0:0a03e4314cb7 114 rgb_sensor.getAllColors( rgb_data ); // read the sensor to get red, green, and blue color data along with overall brightness
westobrien 0:0a03e4314cb7 115
westobrien 0:0a03e4314cb7 116 red=rgb_data[1]; //makes it easier to interpret the data
westobrien 0:0a03e4314cb7 117 green=rgb_data[2];
westobrien 0:0a03e4314cb7 118 blue=rgb_data[3];
westobrien 0:0a03e4314cb7 119
westobrien 0:0a03e4314cb7 120
westobrien 0:0a03e4314cb7 121
westobrien 0:0a03e4314cb7 122 //for infrared photodiodes
westobrien 0:0a03e4314cb7 123 in1 = pd1.read();
westobrien 0:0a03e4314cb7 124 in2 = pd2.read();
westobrien 0:0a03e4314cb7 125 in3 = pd3.read();
westobrien 0:0a03e4314cb7 126
westobrien 0:0a03e4314cb7 127 //ULTRASOUND ACTION
westobrien 0:0a03e4314cb7 128 x = ted.read();
westobrien 0:0a03e4314cb7 129 distance = 689.8006*pow(x,1) - 2.6437; //ultrasound distance calibration equation
westobrien 0:0a03e4314cb7 130 // pc.printf("%f\n\r",distance);
westobrien 0:0a03e4314cb7 131
westobrien 0:0a03e4314cb7 132
westobrien 0:0a03e4314cb7 133 //INFRARED TURN ACTION
westobrien 0:0a03e4314cb7 134 if(in1 >in2 && in1 > in3) { //if the right infrared sensor is reading high
westobrien 0:0a03e4314cb7 135 //pc.printf("Turn Right\n");
westobrien 0:0a03e4314cb7 136 myservo=.1; //turns right if motor is going backwards
westobrien 0:0a03e4314cb7 137 rmv.speed(-.2);
westobrien 0:0a03e4314cb7 138 wait(.1);
westobrien 0:0a03e4314cb7 139 rmv.speed(0);
westobrien 0:0a03e4314cb7 140 wait(.1);
westobrien 0:0a03e4314cb7 141 }
westobrien 0:0a03e4314cb7 142
westobrien 0:0a03e4314cb7 143 if(in2 >in1 && in2 > in3 && blue>red &&blue>green) { //if the middle infrared sensor is reading high
westobrien 0:0a03e4314cb7 144 //pc.printf("Go Straight\n");
westobrien 0:0a03e4314cb7 145 myservo=.35; //goes straight
westobrien 0:0a03e4314cb7 146 rmv.speed(.2);
westobrien 0:0a03e4314cb7 147 wait(.1);
westobrien 0:0a03e4314cb7 148 rmv.speed(0);
westobrien 0:0a03e4314cb7 149 wait(.1);
westobrien 0:0a03e4314cb7 150
westobrien 0:0a03e4314cb7 151 }
westobrien 0:0a03e4314cb7 152
westobrien 0:0a03e4314cb7 153 if(in3 >in1 && in3 > in2) { //if the left PD is reading high
westobrien 0:0a03e4314cb7 154 //pc.printf("Turn Left\n");
westobrien 0:0a03e4314cb7 155 myservo=.99; //turns left if the motor is going backwards
westobrien 0:0a03e4314cb7 156 rmv.speed(-.2);
westobrien 0:0a03e4314cb7 157 wait(.1);
westobrien 0:0a03e4314cb7 158 rmv.speed(0);
westobrien 0:0a03e4314cb7 159 wait(.1);
westobrien 0:0a03e4314cb7 160 }
westobrien 0:0a03e4314cb7 161
westobrien 0:0a03e4314cb7 162 if((green>red) && (green>blue)) { //if the green is on
westobrien 0:0a03e4314cb7 163 //pc.printf("Turn right\n\r");
westobrien 0:0a03e4314cb7 164 myservo=.01;//turns right (if motor is going backward)
westobrien 0:0a03e4314cb7 165 rmv.speed(-.2);//makes our vehicle go backward
westobrien 0:0a03e4314cb7 166 wait(.2);
westobrien 0:0a03e4314cb7 167 rmv.speed(0);
westobrien 0:0a03e4314cb7 168 wait(.1);
westobrien 0:0a03e4314cb7 169
westobrien 0:0a03e4314cb7 170 }
westobrien 0:0a03e4314cb7 171 // if((red>green) && (red>blue)) { //if the red is on
westobrien 0:0a03e4314cb7 172 // //pc.printf("Turn left\n\r");
westobrien 0:0a03e4314cb7 173 // myservo=.99; //turns left (if motor is going backwar)
westobrien 0:0a03e4314cb7 174 // rmv.speed(-.2);//makes our vehicle go backward
westobrien 0:0a03e4314cb7 175 // wait(.2);
westobrien 0:0a03e4314cb7 176 // rmv.speed(0);
westobrien 0:0a03e4314cb7 177 // wait(.1);
westobrien 0:0a03e4314cb7 178
westobrien 0:0a03e4314cb7 179 // }
westobrien 0:0a03e4314cb7 180
westobrien 0:0a03e4314cb7 181 //ultrasound stop action
westobrien 0:0a03e4314cb7 182 if(distance <11) {
westobrien 0:0a03e4314cb7 183 rmv.speed(0);
westobrien 0:0a03e4314cb7 184 wait(10);
westobrien 0:0a03e4314cb7 185 }
westobrien 0:0a03e4314cb7 186 } //end turn = 0 loop
westobrien 0:0a03e4314cb7 187 }// end main
westobrien 0:0a03e4314cb7 188
westobrien 0:0a03e4314cb7 189