Diff: proto_code.cpp

Revision:

29:ae208b014987

Parent:

28:8e74ddc4f70f

Child:

33:0fa936c5a098

--- a/proto_code.cpp	Sun Jan 03 20:05:15 2016 +0000
+++ b/proto_code.cpp	Wed Jan 06 12:44:23 2016 +0000
@@ -1053,41 +1053,54 @@
   
   lonely = true;
   
-  writeToPhone("Finding cones\r\n");
+  writeToPhone("FC\r\n");
+  
+  int retry_count = 0;
   
   for(i = 2; i < NUM_CONES + 1; ++i)
   {
+      retry_count = 0;
+go_again:
     active_cones[i] = false;
     m->cone = i;
     ta.send_immediate(m);
 
     unsigned long st = millis();
-    unsigned long current = 0;
-    unsigned long delta = 0;
+    unsigned long current = 0L;
+    unsigned long delta = 0L;
     
-    writeToPhone("start: %d\r\n", st);
+    writeToPhone("S: %lu F: %d\r\n", st, i);
 
     while(1)
     {
       current = millis();
       delta = current - st;
       
-      if(delta > 5000L) 
+      if(delta > 3000L) 
       {
-        writeToPhone("Find timeout for: %d\r\n", i);
-        break;
+        writeToPhone("FT: %d %lu\r\n", i, current);
+        
+        if (++retry_count < 3)
+        {
+            goto go_again;
+        }
+        else
+        {
+            break;
+        }
       }
-        
-      //ta.spin();
       
       if(ta.recieve(m_in))
       {
         lonely = false;
         active_cones[m_in->cone] = true;
         
+        writeToPhone("FS: %d\r\n", m_in->cone);
         break;
       }
     }
+    
+    wait_us(50);
   }
   
   writeToPhone("available cones are: (1");