ES202 Final Project {Gettinger ThompsonSevcik Blas Lee}
/
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
- Committer:
- GettingerR
- Date:
- 2015-04-27
- Revision:
- 0:dd23b27532c5
File content as of revision 0:dd23b27532c5:
#include "mbed.h"
#include "Motor.h"
#include "Servo.h"
#include "TCS3472_I2C.h"
PwmOut LB(p22); // PWM out signal to power LED on the sensor
TCS3472_I2C rgb_sensor( p9, p10 ); // Establish RGB sensor object
Motor m(p25, p27, p28);
Serial pc(USBTX, USBRX); //USB Port Declare
AnalogIn sensorin(p20); //Analog Pin Declare
AnalogIn photoin(p16); // photoresistor into pin 16
AnalogIn photoin2(p18);
AnalogIn photoin3(p17);
Servo s1(p21);
float distanceinches = 20;
int main()
{
pc.printf("starting...");
rgb_sensor.enablePowerAndRGBC(); // Enable RGB sensor
rgb_sensor.setIntegrationTime(100); // Set integration time of sensor
pc.baud(9600);//Port Parameters
pc.format(8, SerialBase::None, 1);
float volt = 0.0; // variable float
float volt2= 0.0;
float volt3= 0.0;
float PWMbrightness = 1.0; // declare a float specifying brightness of LED
int rgb_data[4]; // declare a 4 element array to store RGB sensor readings
float straight = .605;
float left = .755;
float right = .455;
float bleft = .455;
float manspeed = 0;
float bright = .755;
m.speed(0);//Stop Motor
s1 = straight;//Set Servo Straight Ahead
float motspeed = 0;
int tooclose = 0;
float sensorval = 0;
int distanceconversion = 0;//Set change for distance to ultra
wait(2);
int counter = 1;
motspeed = .25;
manspeed = .35;
m.speed(.25);
pc.printf("starting2");
while(tooclose<2) { //MAINPROGRAM
volt = photoin;
volt2= photoin2;
volt3= photoin3;
LB = PWMbrightness; // set brightness of sensor LED
rgb_sensor.getAllColors( rgb_data ); // read the sensor to get red, green, and blue color data along with overall brightness
//pc.printf( "red: %d, green: %d, blue: %d \n", rgb_data[1], rgb_data[2], rgb_data[3]);
s1 = straight;
m.speed(motspeed);
while(counter<=1) {
distanceinches=20;
counter = 2;
}
if(distanceinches>19) {
pc.printf("%4.3f %d\n", distanceinches, tooclose);
if(rgb_data[1]>rgb_data[2] && rgb_data[1]>rgb_data[3]) {
m.speed(-.25);
s1 = bleft;
wait(.25);
while(rgb_data[1]>rgb_data[2] && rgb_data[1]>rgb_data[3]) {
rgb_sensor.getAllColors( rgb_data );
m.speed(-manspeed);
}
//AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
wait(.4);
//AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAa
s1 = straight;
m.speed(manspeed);
}//end red if
else if(rgb_data[2]>rgb_data[1] && rgb_data[2]>rgb_data[3]) {
m.speed(-.25);
s1 = bright;
wait(.25);
while(rgb_data[2]>rgb_data[1] && rgb_data[2]>rgb_data[3]) {
rgb_sensor.getAllColors( rgb_data );
m.speed(-manspeed);
}
//AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
wait(.4);
//AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAa
s1 = straight;
m.speed(manspeed);
}//end green if
else if(rgb_data[3] >= rgb_data[1] && rgb_data[3]>rgb_data[2]) {
//pc.printf("go straight\n");
m.speed(manspeed);
wait(.1);
}//end blue if
else {
//pc.printf("back up\n");
m.speed(-manspeed);
wait(.1);
}
//pc.printf("go straight");
sensorval = sensorin.read();//Read Sensor
distanceinches = (160750 * (sensorval*sensorval*sensorval) - 16325 * (sensorval*sensorval) + 1039 * sensorval - 4.6243) - distanceconversion;//Convert to inches
pc.printf("%4.3f %d\n", distanceinches, tooclose);
} else {
if(rgb_data[1]>rgb_data[2] && rgb_data[1]>rgb_data[3]) {
m.speed(-.25);
s1 = bleft;
wait(.25);
while(rgb_data[1]>rgb_data[2] && rgb_data[1]>rgb_data[3]) {
rgb_sensor.getAllColors( rgb_data );
m.speed(-manspeed);
}
//AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
wait(.4);
//AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAa
s1 = straight;
m.speed(manspeed);
wait(.2);
}//end red if
else if(rgb_data[1]>rgb_data[2] && rgb_data[1]>rgb_data[3]) {
m.speed(-.25);
s1 = bright;
wait(.25);
while(rgb_data[1]>rgb_data[2] && rgb_data[1]>rgb_data[3]) {
rgb_sensor.getAllColors( rgb_data );
m.speed(-manspeed);
}
//AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
wait(.4);
//AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAa
s1 = straight;
m.speed(manspeed);
}//end red if
else if(rgb_data[3] >= rgb_data[1] && rgb_data[3]>rgb_data[2]) {
if(volt>volt2 && volt>volt3 && volt>.025) {
pc.printf("turn left");
m.speed(-.5);
s1 = bleft;
wait(.25);
m.speed(0);
wait(.2);
s1 = straight;
wait(.2);
sensorval = sensorin.read();//Read Sensor
distanceinches = (160750 * (sensorval*sensorval*sensorval) - 16325 * (sensorval*sensorval) + 1039 * sensorval - 4.6243) - distanceconversion;//Convert to inches
pc.printf("%4.3f %d\n", distanceinches, tooclose);
} else if(volt2>volt && volt2>volt3 && volt2>.0002) {
pc.printf("turn right");
m.speed(-.5);
s1 = bright;
wait(.25);
m.speed(0);
wait(.2);
s1 = straight;
wait(.2);
sensorval = sensorin.read();//Read Sensor
distanceinches = (160750 * (sensorval*sensorval*sensorval) - 16325 * (sensorval*sensorval) + 1039 * sensorval - 4.6243) - distanceconversion;//Convert to inches
pc.printf("%4.3f %d\n", distanceinches, tooclose);
} else {
pc.printf("go straight");
m.speed(manspeed);
wait(.2);
sensorval = sensorin.read();//Read Sensor
distanceinches = (160750 * (sensorval*sensorval*sensorval) - 16325 * (sensorval*sensorval) + 1039 * sensorval - 4.6243) - distanceconversion;//Convert to inches
pc.printf("%4.3f %d\n", distanceinches, tooclose);
}
sensorval = sensorin.read();//Read Sensor
distanceinches = (160750 * (sensorval*sensorval*sensorval) - 16325 * (sensorval*sensorval) + 1039 * sensorval - 4.6243) - distanceconversion;//Convert to inches
pc.printf("%4.3f %d\n", distanceinches, tooclose);
}
}
if(distanceinches>10) { //If car far enough away
tooclose = 0;
} else {
tooclose = tooclose+1;
}
}
m.speed(0);
}