gradient
Dependencies: mbed TCS3472_I2C Tach ContinuousServo
main.cpp
- Committer:
- m215910
- Date:
- 2019-04-27
- Revision:
- 0:46d75bfc2492
File content as of revision 0:46d75bfc2492:
//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