preliminary code
Dependencies: Motor TCS3472_I2C mbed
Diff: control.cpp
- Revision:
- 1:789224bfa1e4
- Parent:
- 0:c16de5d05dd7
- Child:
- 2:372ec56bb276
--- a/control.cpp Sun Apr 26 19:09:55 2015 +0000 +++ b/control.cpp Sun Apr 26 19:44:10 2015 +0000 @@ -5,10 +5,18 @@ PwmOut brightness(p22); TCS3472_I2C rgb(p28, p27); Timer time; +AnalogIn photo1(p15); +AnalogIn photo2(p16); +AnalogIn photo3(p17); +AnalogIn photo4(p18); +AnalogIn photo5(p19); + int running=1; - int rgb_data[4]; float set_brightness = 1.0; +float left; +float right; +float center; int main(){ rgb.enablePowerAndRGBC(); @@ -18,21 +26,58 @@ //dc left fwd //dc rght fwd while(running=1){ + rgb.getAllColors(rgb_data); if(rgb_data[1]>120 && rgb_data[2]<100){ //red;recalibrate while(rgb_data[1]>100){ //recalibrate //dc left rev //dc rght fwd time.reset(); - //dc left fwd - //dc rght fwd + rgb.getAllColors(rgb_data); }; + //dc left fwd + //dc rght fwd + }; if(rgb_data[2]>100){ //green;recalibrate while(rgb_data[2]>90){ //recalibrate //dc left fwd //dc rght rev time.reset(); - //dc left fwd + rgb.getAllColors(rgb_data); + }; + //dc left fwd + //dc rght fwd + }; + if (time>5){ //arbitrary number-->fix later + left=photo1+photo2; + center=2*photo3; + while(left>(center+0.01)){ //recalibrate + //dc left rev //dc rght fwd + left=photo1+photo2; + center=2*photo3; + right=photo4+photo5; }; - }; \ No newline at end of file + center=2*photo3; + right=photo4+photo5; + while(right>(center+0.01)){//recalibrate + //dc left fwd + //dc rght rev + left=photo1+photo2; + center=2*photo3; + right=photo4+photo5; + }; + //dc left fwd + //dc rght fwd + time.reset(); + }; + left=photo1+photo2; + center=2*photo3; + right=photo4+photo5; + if (left<(center+0.01) && right<(center+0.01) && /* sonar is short */){ //recalibrate + running=0; + //dc left stop + //dc rght stop + }; + }; +}; \ No newline at end of file