Diff: main.cpp

Revision:

40:01a97bc4ef7a

Parent:

39:e35a99801ec1

Child:

41:8ce8a3a47a92

--- a/main.cpp	Wed Jun 15 14:50:08 2016 +0000
+++ b/main.cpp	Wed Jun 15 15:09:37 2016 +0000
@@ -112,15 +112,29 @@
     
     for (int i = 0; i < MOTORS; i++)
     {
-        if (!(i == 2 || i == 3))
-        {        
-            pc.printf("calib %d\n", i);
+        if (i != 3)
             ms[i]->drive(0.2f);
-            while (ss[i].read());
+    }
+    
+    bool done;
+    do
+    {
+        done = true;
+        for (int i = 0; i < MOTORS; i++)
+        {
+            if (i != 3)
+            {
+                if (ss[i].read())
+                    done = false;
+                else
+                    ms[i]->drive(0.f);
+            }
         }
-        ms[i]->drive(0.f);
+    }
+    while (!done);
+    
+    for (int i = 0; i < MOTORS; i++)
         ms[i]->getEncoder().reset();
-    }
     pc.printf("done");
     
     for (int i = 0; i < MOTORS; i++)