Diff: main.cpp

Revision:

7:1640572360de

Parent:

6:3e133819d792

Child:

8:a6080c27f8c5

--- a/main.cpp	Sat Mar 31 19:28:35 2018 +0000
+++ b/main.cpp	Sat Mar 31 19:42:16 2018 +0000
@@ -36,6 +36,8 @@
 const float stepSize = 0.001212; //in meters
 const float FREQUENCY = 500; //steps per second
 int sensor_addr = 41 << 1;
+float leg; 
+int color; 
 
 
 //    NOTES
@@ -87,18 +89,19 @@
     //   RGB Sensor Settings     
      pc.baud(115200);
      green = 1; // off  
-    i2c.frequency(200000);
-    char id_regval[1] = {146};
-    char data[1] = {0};
-    i2c.write(sensor_addr,id_regval,1, true);
-    i2c.read(sensor_addr,data,1,false);
-    if (data[0]==68) {
+     i2c.frequency(200000);
+     char id_regval[1] = {146};
+     char data[1] = {0};
+     i2c.write(sensor_addr,id_regval,1, true);
+     i2c.read(sensor_addr,data,1,false);
+     if (data[0]==68) 
+     {
         green = 0;
         wait (2); 
         green = 1;
         } else {
         green = 1; 
-    }
+     }
     
     //    Initialize color sensor
     char timing_register[2] = {129,0};
@@ -117,6 +120,22 @@
     
     while(true)
      { 
+        for(int i = 0; i <=8; i++)
+        {   
+            leg = 0.762; //2.5 feet in meters 
+            grabToken(); 
+            color = findColor(); 
+            if (color = 9)
+            {
+                break;
+            }
+            else 
+            {
+                findPath(color)
+            }
+            move(leg, FORWARD); 
+               
+        }
         grabToken(); 
         move(1,FORWARD);
         turnLeft();
@@ -261,7 +280,7 @@
         
         
         
-        //1=red,2=green,3=blue,4=cyan,5=magenta,6=yellow,7=gray,8=error
+        //1=red,2=green,3=blue,4=cyan,5=magenta,6=yellow,7=gray,8=error, 9=nothing
         if(blue_value<10000 && red_value>10000){
             return(1);
         }
@@ -286,7 +305,9 @@
         else if(red_value==0){
             return(8);
         }
-        
+        else{
+            return(9);
+        }
         // print sensor readings
         
         //pc.printf("Clear (%d), Red (%d), Green (%d), Blue (%d)\n", clear_value, red_value, green_value, blue_value);