zzzzzzzzzz / Mbed 2 deprecated LAB_10_control

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?

UserRevisionLine numberNew 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 };