gradient
Dependencies: mbed TCS3472_I2C Tach ContinuousServo
Diff: main.cpp
- Revision:
- 0:46d75bfc2492
diff -r 000000000000 -r 46d75bfc2492 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sat Apr 27 00:47:12 2019 +0000 @@ -0,0 +1,123 @@ +//Satre Gagnon, Ewing, Batten +//Drive Straight + +#include "mbed.h" +#include "ContinuousServo.h" +#include "Tach.h" +#include "TCS3472_I2C.h" + +// color sensor +DigitalOut hallpwr(p22); //PWM out signal to power LED on the sensor +DigitalIn hall(p21); +TCS3472_I2C rgb_sensor(p9, p10); // Establish RGB sensor object +Serial pc(USBTX,USBRX); //Establish serial connection +int rgb_data[4]; //store sensor readings + +//lights for colors +DigitalOut red(LED1); +DigitalOut green(LED2); +DigitalOut blue(LED3); +DigitalOut lightmag(LED4); + +//speed +float wL, wR; +float x = 0.3, y = -0.3, dist, realdist; + +ContinuousServo left(p23); +ContinuousServo right(p26); + +//encoders +Tach tLeft(p17,64); +Tach tRight(p13,64); + +int main() +{ + rgb_sensor.enablePowerAndRGBC(); //Enable RGB sensor + rgb_sensor.setIntegrationTime(100); //Set integration time of sensor + + wL = tLeft.getSpeed(); //speed of left wheel (rev/sec) + wR = tRight.getSpeed(); //speed of right wheel (rev/sec) + + pc.baud(9600); //set baud rate + float PWMbrightness = 1.0; //float specifying brightness of LED (between 0.0 and 1.0) + + while(1) { + + left.speed(x); + right.speed(y); //right negative to go forward + + hallpwr = PWMbrightness; //set brightness of sensor LED + rgb_sensor.getAllColors(rgb_data); + + if ((rgb_data[0] > 10000)) + { + blue = 0; + green = 0; + red = 0; + } + + else if ((rgb_data[1] > rgb_data[2]) && (rgb_data[1] > rgb_data[3]) ) + { + red = 1; + green = 0; + blue = 0; + } + else if ((rgb_data[2] > rgb_data[1]) && (rgb_data[2] > rgb_data[3]) ) + { + if(rgb_data[2] > 1100) + { + green = 1; + red = 0; + blue = 0; + } + else + { + green = 0; + red = 0; + blue = 0; + } + } + + else if ((rgb_data[3] > rgb_data[2]) && (rgb_data[3] > rgb_data[1]) ) + { + blue = 1; + green = 0; + red = 0; + } + if (hall == 1){ + lightmag = 1; + printf("BOMB DETECTED"); + } + else if (hall == 0){ + lightmag = 0; + } + if (wL<wR) { + x = x+0.01; + y = y; } + else if (wR<wL) { + x = x-0.01; + y = y;} + else if (wR == wL) { + x = x; + y = y; + } + if (realdist <= 8) { + left.stop(); + right.stop(); + } + if ((rgb_data[0] > 9000)) + { + x = x; + y = 0; + } + if ((rgb_data[0] < 800)) + { + x = x; + y = 0; + } + + + printf("unfiltered: %d, red: %d, green: %d, blue: %d \n", rgb_data[0], rgb_data[1], rgb_data[2], rgb_data[3]); + +} //while +} //main \ No newline at end of file