preliminary code
Dependencies: Motor TCS3472_I2C mbed
control.cpp@2:372ec56bb276, 2015-04-26 (annotated)
- Committer:
- ThatsAKnife
- Date:
- Sun Apr 26 19:45:50 2015 +0000
- Revision:
- 2:372ec56bb276
- Parent:
- 1:789224bfa1e4
minor edits
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ThatsAKnife | 0:c16de5d05dd7 | 1 | #include "mbed.h" |
ThatsAKnife | 0:c16de5d05dd7 | 2 | #include "Motor.h" |
ThatsAKnife | 0:c16de5d05dd7 | 3 | #include "TCS3472_I2C.h" |
ThatsAKnife | 0:c16de5d05dd7 | 4 | |
ThatsAKnife | 0:c16de5d05dd7 | 5 | PwmOut brightness(p22); |
ThatsAKnife | 0:c16de5d05dd7 | 6 | TCS3472_I2C rgb(p28, p27); |
ThatsAKnife | 0:c16de5d05dd7 | 7 | Timer time; |
ThatsAKnife | 1:789224bfa1e4 | 8 | AnalogIn photo1(p15); |
ThatsAKnife | 1:789224bfa1e4 | 9 | AnalogIn photo2(p16); |
ThatsAKnife | 1:789224bfa1e4 | 10 | AnalogIn photo3(p17); |
ThatsAKnife | 1:789224bfa1e4 | 11 | AnalogIn photo4(p18); |
ThatsAKnife | 1:789224bfa1e4 | 12 | AnalogIn photo5(p19); |
ThatsAKnife | 1:789224bfa1e4 | 13 | |
ThatsAKnife | 0:c16de5d05dd7 | 14 | int running=1; |
ThatsAKnife | 0:c16de5d05dd7 | 15 | int rgb_data[4]; |
ThatsAKnife | 0:c16de5d05dd7 | 16 | float set_brightness = 1.0; |
ThatsAKnife | 1:789224bfa1e4 | 17 | float left; |
ThatsAKnife | 1:789224bfa1e4 | 18 | float right; |
ThatsAKnife | 1:789224bfa1e4 | 19 | float center; |
ThatsAKnife | 0:c16de5d05dd7 | 20 | |
ThatsAKnife | 0:c16de5d05dd7 | 21 | int main(){ |
ThatsAKnife | 0:c16de5d05dd7 | 22 | rgb.enablePowerAndRGBC(); |
ThatsAKnife | 0:c16de5d05dd7 | 23 | rgb.setIntegrationTime(100); |
ThatsAKnife | 0:c16de5d05dd7 | 24 | brightness=set_brightness; |
ThatsAKnife | 0:c16de5d05dd7 | 25 | time.start(); |
ThatsAKnife | 0:c16de5d05dd7 | 26 | //dc left fwd |
ThatsAKnife | 0:c16de5d05dd7 | 27 | //dc rght fwd |
ThatsAKnife | 0:c16de5d05dd7 | 28 | while(running=1){ |
ThatsAKnife | 1:789224bfa1e4 | 29 | rgb.getAllColors(rgb_data); |
ThatsAKnife | 0:c16de5d05dd7 | 30 | if(rgb_data[1]>120 && rgb_data[2]<100){ //red;recalibrate |
ThatsAKnife | 0:c16de5d05dd7 | 31 | while(rgb_data[1]>100){ //recalibrate |
ThatsAKnife | 0:c16de5d05dd7 | 32 | //dc left rev |
ThatsAKnife | 0:c16de5d05dd7 | 33 | //dc rght fwd |
ThatsAKnife | 0:c16de5d05dd7 | 34 | time.reset(); |
ThatsAKnife | 1:789224bfa1e4 | 35 | rgb.getAllColors(rgb_data); |
ThatsAKnife | 0:c16de5d05dd7 | 36 | }; |
ThatsAKnife | 1:789224bfa1e4 | 37 | //dc left fwd |
ThatsAKnife | 1:789224bfa1e4 | 38 | //dc rght fwd |
ThatsAKnife | 1:789224bfa1e4 | 39 | |
ThatsAKnife | 0:c16de5d05dd7 | 40 | }; |
ThatsAKnife | 0:c16de5d05dd7 | 41 | if(rgb_data[2]>100){ //green;recalibrate |
ThatsAKnife | 0:c16de5d05dd7 | 42 | while(rgb_data[2]>90){ //recalibrate |
ThatsAKnife | 0:c16de5d05dd7 | 43 | //dc left fwd |
ThatsAKnife | 0:c16de5d05dd7 | 44 | //dc rght rev |
ThatsAKnife | 0:c16de5d05dd7 | 45 | time.reset(); |
ThatsAKnife | 1:789224bfa1e4 | 46 | rgb.getAllColors(rgb_data); |
ThatsAKnife | 1:789224bfa1e4 | 47 | }; |
ThatsAKnife | 1:789224bfa1e4 | 48 | //dc left fwd |
ThatsAKnife | 1:789224bfa1e4 | 49 | //dc rght fwd |
ThatsAKnife | 1:789224bfa1e4 | 50 | }; |
ThatsAKnife | 1:789224bfa1e4 | 51 | if (time>5){ //arbitrary number-->fix later |
ThatsAKnife | 1:789224bfa1e4 | 52 | left=photo1+photo2; |
ThatsAKnife | 1:789224bfa1e4 | 53 | center=2*photo3; |
ThatsAKnife | 1:789224bfa1e4 | 54 | while(left>(center+0.01)){ //recalibrate |
ThatsAKnife | 1:789224bfa1e4 | 55 | //dc left rev |
ThatsAKnife | 0:c16de5d05dd7 | 56 | //dc rght fwd |
ThatsAKnife | 1:789224bfa1e4 | 57 | left=photo1+photo2; |
ThatsAKnife | 1:789224bfa1e4 | 58 | center=2*photo3; |
ThatsAKnife | 1:789224bfa1e4 | 59 | right=photo4+photo5; |
ThatsAKnife | 0:c16de5d05dd7 | 60 | }; |
ThatsAKnife | 1:789224bfa1e4 | 61 | center=2*photo3; |
ThatsAKnife | 1:789224bfa1e4 | 62 | right=photo4+photo5; |
ThatsAKnife | 1:789224bfa1e4 | 63 | while(right>(center+0.01)){//recalibrate |
ThatsAKnife | 1:789224bfa1e4 | 64 | //dc left fwd |
ThatsAKnife | 1:789224bfa1e4 | 65 | //dc rght rev |
ThatsAKnife | 1:789224bfa1e4 | 66 | left=photo1+photo2; |
ThatsAKnife | 1:789224bfa1e4 | 67 | center=2*photo3; |
ThatsAKnife | 1:789224bfa1e4 | 68 | right=photo4+photo5; |
ThatsAKnife | 1:789224bfa1e4 | 69 | }; |
ThatsAKnife | 1:789224bfa1e4 | 70 | //dc left fwd |
ThatsAKnife | 1:789224bfa1e4 | 71 | //dc rght fwd |
ThatsAKnife | 1:789224bfa1e4 | 72 | time.reset(); |
ThatsAKnife | 1:789224bfa1e4 | 73 | }; |
ThatsAKnife | 1:789224bfa1e4 | 74 | left=photo1+photo2; |
ThatsAKnife | 1:789224bfa1e4 | 75 | center=2*photo3; |
ThatsAKnife | 1:789224bfa1e4 | 76 | right=photo4+photo5; |
ThatsAKnife | 2:372ec56bb276 | 77 | //collect sonar |
ThatsAKnife | 1:789224bfa1e4 | 78 | if (left<(center+0.01) && right<(center+0.01) && /* sonar is short */){ //recalibrate |
ThatsAKnife | 1:789224bfa1e4 | 79 | running=0; |
ThatsAKnife | 1:789224bfa1e4 | 80 | //dc left stop |
ThatsAKnife | 1:789224bfa1e4 | 81 | //dc rght stop |
ThatsAKnife | 1:789224bfa1e4 | 82 | }; |
ThatsAKnife | 1:789224bfa1e4 | 83 | }; |
ThatsAKnife | 1:789224bfa1e4 | 84 | }; |