ES202 Final Project {Gettinger ThompsonSevcik Blas Lee} / Mbed 2 deprecated ES202FinalProject

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   Motor Servo TCS3472_I2C mbed

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

Committer:
GettingerR
Date:
Mon Apr 27 20:24:55 2015 +0000
Revision:
0:dd23b27532c5
Final Project Final Build;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
GettingerR 0:dd23b27532c5 1 #include "mbed.h"
GettingerR 0:dd23b27532c5 2 #include "Motor.h"
GettingerR 0:dd23b27532c5 3 #include "Servo.h"
GettingerR 0:dd23b27532c5 4 #include "TCS3472_I2C.h"
GettingerR 0:dd23b27532c5 5
GettingerR 0:dd23b27532c5 6
GettingerR 0:dd23b27532c5 7 PwmOut LB(p22); // PWM out signal to power LED on the sensor
GettingerR 0:dd23b27532c5 8 TCS3472_I2C rgb_sensor( p9, p10 ); // Establish RGB sensor object
GettingerR 0:dd23b27532c5 9 Motor m(p25, p27, p28);
GettingerR 0:dd23b27532c5 10 Serial pc(USBTX, USBRX); //USB Port Declare
GettingerR 0:dd23b27532c5 11 AnalogIn sensorin(p20); //Analog Pin Declare
GettingerR 0:dd23b27532c5 12 AnalogIn photoin(p16); // photoresistor into pin 16
GettingerR 0:dd23b27532c5 13 AnalogIn photoin2(p18);
GettingerR 0:dd23b27532c5 14 AnalogIn photoin3(p17);
GettingerR 0:dd23b27532c5 15 Servo s1(p21);
GettingerR 0:dd23b27532c5 16 float distanceinches = 20;
GettingerR 0:dd23b27532c5 17
GettingerR 0:dd23b27532c5 18 int main()
GettingerR 0:dd23b27532c5 19 {
GettingerR 0:dd23b27532c5 20 pc.printf("starting...");
GettingerR 0:dd23b27532c5 21 rgb_sensor.enablePowerAndRGBC(); // Enable RGB sensor
GettingerR 0:dd23b27532c5 22 rgb_sensor.setIntegrationTime(100); // Set integration time of sensor
GettingerR 0:dd23b27532c5 23 pc.baud(9600);//Port Parameters
GettingerR 0:dd23b27532c5 24 pc.format(8, SerialBase::None, 1);
GettingerR 0:dd23b27532c5 25
GettingerR 0:dd23b27532c5 26 float volt = 0.0; // variable float
GettingerR 0:dd23b27532c5 27 float volt2= 0.0;
GettingerR 0:dd23b27532c5 28 float volt3= 0.0;
GettingerR 0:dd23b27532c5 29 float PWMbrightness = 1.0; // declare a float specifying brightness of LED
GettingerR 0:dd23b27532c5 30 int rgb_data[4]; // declare a 4 element array to store RGB sensor readings
GettingerR 0:dd23b27532c5 31 float straight = .605;
GettingerR 0:dd23b27532c5 32 float left = .755;
GettingerR 0:dd23b27532c5 33 float right = .455;
GettingerR 0:dd23b27532c5 34 float bleft = .455;
GettingerR 0:dd23b27532c5 35 float manspeed = 0;
GettingerR 0:dd23b27532c5 36 float bright = .755;
GettingerR 0:dd23b27532c5 37 m.speed(0);//Stop Motor
GettingerR 0:dd23b27532c5 38 s1 = straight;//Set Servo Straight Ahead
GettingerR 0:dd23b27532c5 39 float motspeed = 0;
GettingerR 0:dd23b27532c5 40 int tooclose = 0;
GettingerR 0:dd23b27532c5 41 float sensorval = 0;
GettingerR 0:dd23b27532c5 42 int distanceconversion = 0;//Set change for distance to ultra
GettingerR 0:dd23b27532c5 43 wait(2);
GettingerR 0:dd23b27532c5 44 int counter = 1;
GettingerR 0:dd23b27532c5 45 motspeed = .25;
GettingerR 0:dd23b27532c5 46 manspeed = .35;
GettingerR 0:dd23b27532c5 47 m.speed(.25);
GettingerR 0:dd23b27532c5 48 pc.printf("starting2");
GettingerR 0:dd23b27532c5 49
GettingerR 0:dd23b27532c5 50
GettingerR 0:dd23b27532c5 51 while(tooclose<2) { //MAINPROGRAM
GettingerR 0:dd23b27532c5 52
GettingerR 0:dd23b27532c5 53
GettingerR 0:dd23b27532c5 54 volt = photoin;
GettingerR 0:dd23b27532c5 55 volt2= photoin2;
GettingerR 0:dd23b27532c5 56 volt3= photoin3;
GettingerR 0:dd23b27532c5 57
GettingerR 0:dd23b27532c5 58 LB = PWMbrightness; // set brightness of sensor LED
GettingerR 0:dd23b27532c5 59 rgb_sensor.getAllColors( rgb_data ); // read the sensor to get red, green, and blue color data along with overall brightness
GettingerR 0:dd23b27532c5 60 //pc.printf( "red: %d, green: %d, blue: %d \n", rgb_data[1], rgb_data[2], rgb_data[3]);
GettingerR 0:dd23b27532c5 61 s1 = straight;
GettingerR 0:dd23b27532c5 62 m.speed(motspeed);
GettingerR 0:dd23b27532c5 63 while(counter<=1) {
GettingerR 0:dd23b27532c5 64 distanceinches=20;
GettingerR 0:dd23b27532c5 65 counter = 2;
GettingerR 0:dd23b27532c5 66 }
GettingerR 0:dd23b27532c5 67
GettingerR 0:dd23b27532c5 68 if(distanceinches>19) {
GettingerR 0:dd23b27532c5 69 pc.printf("%4.3f %d\n", distanceinches, tooclose);
GettingerR 0:dd23b27532c5 70 if(rgb_data[1]>rgb_data[2] && rgb_data[1]>rgb_data[3]) {
GettingerR 0:dd23b27532c5 71 m.speed(-.25);
GettingerR 0:dd23b27532c5 72 s1 = bleft;
GettingerR 0:dd23b27532c5 73 wait(.25);
GettingerR 0:dd23b27532c5 74 while(rgb_data[1]>rgb_data[2] && rgb_data[1]>rgb_data[3]) {
GettingerR 0:dd23b27532c5 75 rgb_sensor.getAllColors( rgb_data );
GettingerR 0:dd23b27532c5 76 m.speed(-manspeed);
GettingerR 0:dd23b27532c5 77 }
GettingerR 0:dd23b27532c5 78 //AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
GettingerR 0:dd23b27532c5 79 wait(.4);
GettingerR 0:dd23b27532c5 80 //AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAa
GettingerR 0:dd23b27532c5 81 s1 = straight;
GettingerR 0:dd23b27532c5 82 m.speed(manspeed);
GettingerR 0:dd23b27532c5 83 }//end red if
GettingerR 0:dd23b27532c5 84 else if(rgb_data[2]>rgb_data[1] && rgb_data[2]>rgb_data[3]) {
GettingerR 0:dd23b27532c5 85 m.speed(-.25);
GettingerR 0:dd23b27532c5 86 s1 = bright;
GettingerR 0:dd23b27532c5 87 wait(.25);
GettingerR 0:dd23b27532c5 88 while(rgb_data[2]>rgb_data[1] && rgb_data[2]>rgb_data[3]) {
GettingerR 0:dd23b27532c5 89 rgb_sensor.getAllColors( rgb_data );
GettingerR 0:dd23b27532c5 90 m.speed(-manspeed);
GettingerR 0:dd23b27532c5 91 }
GettingerR 0:dd23b27532c5 92 //AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
GettingerR 0:dd23b27532c5 93 wait(.4);
GettingerR 0:dd23b27532c5 94 //AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAa
GettingerR 0:dd23b27532c5 95 s1 = straight;
GettingerR 0:dd23b27532c5 96 m.speed(manspeed);
GettingerR 0:dd23b27532c5 97 }//end green if
GettingerR 0:dd23b27532c5 98 else if(rgb_data[3] >= rgb_data[1] && rgb_data[3]>rgb_data[2]) {
GettingerR 0:dd23b27532c5 99 //pc.printf("go straight\n");
GettingerR 0:dd23b27532c5 100 m.speed(manspeed);
GettingerR 0:dd23b27532c5 101 wait(.1);
GettingerR 0:dd23b27532c5 102 }//end blue if
GettingerR 0:dd23b27532c5 103 else {
GettingerR 0:dd23b27532c5 104 //pc.printf("back up\n");
GettingerR 0:dd23b27532c5 105 m.speed(-manspeed);
GettingerR 0:dd23b27532c5 106 wait(.1);
GettingerR 0:dd23b27532c5 107 }
GettingerR 0:dd23b27532c5 108
GettingerR 0:dd23b27532c5 109 //pc.printf("go straight");
GettingerR 0:dd23b27532c5 110
GettingerR 0:dd23b27532c5 111
GettingerR 0:dd23b27532c5 112
GettingerR 0:dd23b27532c5 113 sensorval = sensorin.read();//Read Sensor
GettingerR 0:dd23b27532c5 114 distanceinches = (160750 * (sensorval*sensorval*sensorval) - 16325 * (sensorval*sensorval) + 1039 * sensorval - 4.6243) - distanceconversion;//Convert to inches
GettingerR 0:dd23b27532c5 115 pc.printf("%4.3f %d\n", distanceinches, tooclose);
GettingerR 0:dd23b27532c5 116 } else {
GettingerR 0:dd23b27532c5 117
GettingerR 0:dd23b27532c5 118 if(rgb_data[1]>rgb_data[2] && rgb_data[1]>rgb_data[3]) {
GettingerR 0:dd23b27532c5 119 m.speed(-.25);
GettingerR 0:dd23b27532c5 120 s1 = bleft;
GettingerR 0:dd23b27532c5 121 wait(.25);
GettingerR 0:dd23b27532c5 122 while(rgb_data[1]>rgb_data[2] && rgb_data[1]>rgb_data[3]) {
GettingerR 0:dd23b27532c5 123 rgb_sensor.getAllColors( rgb_data );
GettingerR 0:dd23b27532c5 124 m.speed(-manspeed);
GettingerR 0:dd23b27532c5 125 }
GettingerR 0:dd23b27532c5 126 //AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
GettingerR 0:dd23b27532c5 127 wait(.4);
GettingerR 0:dd23b27532c5 128 //AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAa
GettingerR 0:dd23b27532c5 129 s1 = straight;
GettingerR 0:dd23b27532c5 130 m.speed(manspeed);
GettingerR 0:dd23b27532c5 131 wait(.2);
GettingerR 0:dd23b27532c5 132 }//end red if
GettingerR 0:dd23b27532c5 133 else if(rgb_data[1]>rgb_data[2] && rgb_data[1]>rgb_data[3]) {
GettingerR 0:dd23b27532c5 134 m.speed(-.25);
GettingerR 0:dd23b27532c5 135 s1 = bright;
GettingerR 0:dd23b27532c5 136 wait(.25);
GettingerR 0:dd23b27532c5 137 while(rgb_data[1]>rgb_data[2] && rgb_data[1]>rgb_data[3]) {
GettingerR 0:dd23b27532c5 138 rgb_sensor.getAllColors( rgb_data );
GettingerR 0:dd23b27532c5 139 m.speed(-manspeed);
GettingerR 0:dd23b27532c5 140 }
GettingerR 0:dd23b27532c5 141 //AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
GettingerR 0:dd23b27532c5 142 wait(.4);
GettingerR 0:dd23b27532c5 143 //AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAa
GettingerR 0:dd23b27532c5 144 s1 = straight;
GettingerR 0:dd23b27532c5 145 m.speed(manspeed);
GettingerR 0:dd23b27532c5 146 }//end red if
GettingerR 0:dd23b27532c5 147 else if(rgb_data[3] >= rgb_data[1] && rgb_data[3]>rgb_data[2]) {
GettingerR 0:dd23b27532c5 148 if(volt>volt2 && volt>volt3 && volt>.025) {
GettingerR 0:dd23b27532c5 149 pc.printf("turn left");
GettingerR 0:dd23b27532c5 150 m.speed(-.5);
GettingerR 0:dd23b27532c5 151 s1 = bleft;
GettingerR 0:dd23b27532c5 152 wait(.25);
GettingerR 0:dd23b27532c5 153 m.speed(0);
GettingerR 0:dd23b27532c5 154 wait(.2);
GettingerR 0:dd23b27532c5 155 s1 = straight;
GettingerR 0:dd23b27532c5 156 wait(.2);
GettingerR 0:dd23b27532c5 157 sensorval = sensorin.read();//Read Sensor
GettingerR 0:dd23b27532c5 158 distanceinches = (160750 * (sensorval*sensorval*sensorval) - 16325 * (sensorval*sensorval) + 1039 * sensorval - 4.6243) - distanceconversion;//Convert to inches
GettingerR 0:dd23b27532c5 159 pc.printf("%4.3f %d\n", distanceinches, tooclose);
GettingerR 0:dd23b27532c5 160
GettingerR 0:dd23b27532c5 161 } else if(volt2>volt && volt2>volt3 && volt2>.0002) {
GettingerR 0:dd23b27532c5 162
GettingerR 0:dd23b27532c5 163 pc.printf("turn right");
GettingerR 0:dd23b27532c5 164 m.speed(-.5);
GettingerR 0:dd23b27532c5 165 s1 = bright;
GettingerR 0:dd23b27532c5 166 wait(.25);
GettingerR 0:dd23b27532c5 167 m.speed(0);
GettingerR 0:dd23b27532c5 168 wait(.2);
GettingerR 0:dd23b27532c5 169 s1 = straight;
GettingerR 0:dd23b27532c5 170 wait(.2);
GettingerR 0:dd23b27532c5 171 sensorval = sensorin.read();//Read Sensor
GettingerR 0:dd23b27532c5 172 distanceinches = (160750 * (sensorval*sensorval*sensorval) - 16325 * (sensorval*sensorval) + 1039 * sensorval - 4.6243) - distanceconversion;//Convert to inches
GettingerR 0:dd23b27532c5 173 pc.printf("%4.3f %d\n", distanceinches, tooclose);
GettingerR 0:dd23b27532c5 174
GettingerR 0:dd23b27532c5 175
GettingerR 0:dd23b27532c5 176 } else {
GettingerR 0:dd23b27532c5 177
GettingerR 0:dd23b27532c5 178 pc.printf("go straight");
GettingerR 0:dd23b27532c5 179 m.speed(manspeed);
GettingerR 0:dd23b27532c5 180 wait(.2);
GettingerR 0:dd23b27532c5 181 sensorval = sensorin.read();//Read Sensor
GettingerR 0:dd23b27532c5 182 distanceinches = (160750 * (sensorval*sensorval*sensorval) - 16325 * (sensorval*sensorval) + 1039 * sensorval - 4.6243) - distanceconversion;//Convert to inches
GettingerR 0:dd23b27532c5 183 pc.printf("%4.3f %d\n", distanceinches, tooclose);
GettingerR 0:dd23b27532c5 184
GettingerR 0:dd23b27532c5 185 }
GettingerR 0:dd23b27532c5 186
GettingerR 0:dd23b27532c5 187
GettingerR 0:dd23b27532c5 188 sensorval = sensorin.read();//Read Sensor
GettingerR 0:dd23b27532c5 189 distanceinches = (160750 * (sensorval*sensorval*sensorval) - 16325 * (sensorval*sensorval) + 1039 * sensorval - 4.6243) - distanceconversion;//Convert to inches
GettingerR 0:dd23b27532c5 190 pc.printf("%4.3f %d\n", distanceinches, tooclose);
GettingerR 0:dd23b27532c5 191
GettingerR 0:dd23b27532c5 192
GettingerR 0:dd23b27532c5 193 }
GettingerR 0:dd23b27532c5 194 }
GettingerR 0:dd23b27532c5 195 if(distanceinches>10) { //If car far enough away
GettingerR 0:dd23b27532c5 196 tooclose = 0;
GettingerR 0:dd23b27532c5 197 } else {
GettingerR 0:dd23b27532c5 198 tooclose = tooclose+1;
GettingerR 0:dd23b27532c5 199 }
GettingerR 0:dd23b27532c5 200 }
GettingerR 0:dd23b27532c5 201
GettingerR 0:dd23b27532c5 202 m.speed(0);
GettingerR 0:dd23b27532c5 203 }