![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
not sure on the if statement at the bottom
Dependencies: ContinuousServo Tach mbed TCS3472_I2C
Revision 2:b5a85ba07057, committed 2018-04-30
- Comitter:
- PlayaLarrea
- Date:
- Mon Apr 30 15:38:53 2018 +0000
- Parent:
- 1:1dd468e2ad40
- Commit message:
- gay
Changed in this revision
color.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 1dd468e2ad40 -r b5a85ba07057 color.cpp --- a/color.cpp Wed Apr 25 14:13:24 2018 +0000 +++ b/color.cpp Mon Apr 30 15:38:53 2018 +0000 @@ -3,63 +3,64 @@ #include "Tach.h" #include "TCS3472_I2C.h" -InterruptIn hall(p21); +//InterruptIn hall_sensor(p21); +DigitalOut hallpwr(p22); +DigitalIn hall(p21); BusOut flash(LED1, LED2, LED3, LED4); TCS3472_I2C rgb_sensor(p9, p10); ContinuousServo left(p23); //Set up left wheel driver ContinuousServo right(p26); //Set up right wheel driver -Tach tleft(p17,64); //Set up left tachometer -Tach tright(p13, 64); //Set up right tachometer - -float wl; //Left wheel velocity -float wr; //Right wheel velocity -float e; //Error - -float r = 1.3125; //radius of wheels -float l = 4.0625; //width of wheel base -float omega = 0.0; //angular velocity of wheels -float v = 0.5; //translational velocity of wheels -float kp = 1.0; //Proportional control gain - -float vl = ((omega*l)+(2*v))/(2*r); //Calculates left wheel velocity -float vr = ((omega*l)-(2*v))/(2*r); //Calculates right wheel velocity int rgb_data[4]; float PWMbrightness = 1.0; float LB = 0.0; +int x = 0.0; -void mine() { - flash = 8.0; -} +//void mine() { +// flash = 8.0; +// wait(1.0); +// flash = 0; +//} int main() { - hall.rise(&mine); - hall.fall(&mine); + hall.mode(PullUp); +// hall_sensor.rise(&mine); +// hall_sensor.fall(&mine); + hallpwr = 1; rgb_sensor.enablePowerAndRGBC(); rgb_sensor.setIntegrationTime(100); while(1) { // wait around, interrupts will interrupt this! - left.speed(vl); //Move the left wheel - right.speed(vr); //Move the right wheel - wl = tleft.getSpeed(); //Read the left tachometer - wr = tright.getSpeed(); //Read the right tachometer - - e = wl - (-wr); //Calculate the error - vr = vr - (kp*e); //Readjust the right wheel velocity + + x = hall.read(); + + if(x == 1){ + flash = 8.0; + } + else{ + flash = 0.0; + } + x = hall.read(); + + left.speed(0.1); //Move the left wheel + right.speed(-0.1); //Move the right wheel LB = PWMbrightness; rgb_sensor.getAllColors(rgb_data); - - if(rgb_data[1]>999 && rgb_data[2]<999 && rgb_data[3]<999){ - flash = 1.0; + + //if(rgb_data[1]>650){ + if(rgb_data[0]>1000 && rgb_data[1]>650 && rgb_data[2]<300 && rgb_data[3]<300){ + flash = 1.0; //red } - else if(rgb_data[1]<999 && rgb_data[2]>999 && rgb_data[3]<999){ - flash = 2.0; + //else if(rgb_data[2]>650){ + else if(rgb_data[0]>900 && rgb_data[1]<700 && rgb_data[2]>650 && rgb_data[3]<700){ + flash = 2.0; //green } - else if(rgb_data[1]<999 && rgb_data[2]<999 && rgb_data[3]>999){ - flash = 4.0; + //else if(rgb_data[3]>300){ + else if(rgb_data[0]>900 && rgb_data[1]<300 && rgb_data[2]<300 && rgb_data[3]>300){ + flash = 4.0; //blue } } } \ No newline at end of file