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