Diff: proto_code.cpp

Revision:

32:64e5d7340d82

Parent:

31:a6110950f385

Child:

33:0fa936c5a098

--- a/proto_code.cpp	Tue Jan 05 13:05:48 2016 +0000
+++ b/proto_code.cpp	Wed Jan 06 16:39:59 2016 +0000
@@ -1056,41 +1056,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);  /// DEBUG-only message
 
     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");