Diff: main.cpp

Revision:

37:8021b3ce241a

Parent:

36:906323851366

Child:

38:b03a5bf9ac7b

--- a/main.cpp	Wed Jun 15 11:52:08 2016 +0000
+++ b/main.cpp	Wed Jun 15 14:09:49 2016 +0000
@@ -82,6 +82,22 @@
 
 const float tickTime = 1.f / 60;
 
+void waitAllRotate()
+{
+    tick();
+    
+    float sum;
+    do {
+        sum = 0;
+        for (int i = 0; i < MOTORS; i++)
+            sum += abs(ms[i]->errorTurn);
+        pc.printf("%f\n",sum);
+    }
+    while(sum > 0.05f);
+    
+    wait(0.1f);
+}
+
 int main()
 {   
     pc.printf("MAIN\n");
@@ -94,6 +110,38 @@
     
     //unsigned char volatile rfget;
     
+    for (int i = 0; i < MOTORS; i++)
+    {
+        if (!(i == 2 || i == 3))
+        {        
+            pc.printf("calib %d\n", i);
+            ms[i]->drive(i == 0 || i == 1 || i == 5 ? -0.3f : 0.3f);
+            while (ss[i].read());
+        }
+        ms[i]->drive(0.f);
+        ms[i]->getEncoder().reset();
+    }
+    pc.printf("done");
+    
+    for (int i = 0; i < MOTORS; i++)
+        pc.printf("%ld ", ms[i]->encoder.getCount());
+    pc.printf("\n");
+    
+    /*ms[0]->rotate(-0.5f + 0.125f, 0.5f);
+    ms[1]->rotate(-0.5f - 0.125f, 0.5f);
+    ms[2]->rotate(0.5f - 0.125f, 0.5f);
+    ms[3]->rotate(0.5f + 0.125f, 0.5f);
+    ms[4]->rotate(0.5f - 0.125f, 0.5f);
+    ms[5]->rotate(-0.5f - 0.125f, 0.5f);
+    waitAllRotate();*/
+    ms[0]->rotate(-0.5f - 0.125f, 0.5f);
+    ms[1]->rotate(-0.5f + 0.125f, 0.5f);
+    ms[2]->rotate(0.5f + 0.125f, 0.5f);
+    ms[3]->rotate(0.5f - 0.125f, 0.5f);
+    ms[4]->rotate(0.5f + 0.125f, 0.5f);
+    ms[5]->rotate(-0.5f + 0.125f, 0.5f);
+    waitAllRotate();
+    
     float volatile speed;
     
     float volatile sum;
@@ -103,10 +151,15 @@
     {
       
         speed = 0.925f;
+        //speed = 0;
         //int oldactive = active;
         //pc.scanf("%f", &speed);
         
         if (speed != 0){
+            for (int i = 0; i < MOTORS; i++)
+                pc.printf("%ld ", ms[i]->encoder.getCount());
+            pc.printf("\n");
+            
             speed = (speed*0.3f); //+ ((speed < 0) ? -1 : 1) * mod;
         
             ms[0]->rotate(-0.75f,speed*3);
@@ -115,30 +168,15 @@
             ms[3]->rotate(0.25f,speed);
             ms[4]->rotate(0.75f,speed*3);
             ms[5]->rotate(-0.25f,speed);
-            tick();
-            do {
-                sum = 0;
-                for (int i = 0; i < MOTORS; i++){
-                    pc.printf("%d,%f\n",i,ms[i]->errorTurn);
-                    sum+=abs(ms[i]->errorTurn);
-                    }
-                pc.printf("1)%f\n",sum);
-            } while( sum > 0.05f );
+            waitAllRotate();
+            
             ms[0]->rotate(-0.25f,speed);
             ms[1]->rotate(-0.75f,speed*3);
             ms[2]->rotate(0.25f,speed);
             ms[3]->rotate(0.75f,speed*3);
             ms[4]->rotate(0.25f,speed);
             ms[5]->rotate(-0.75f,speed*3);
-            tick();
-            do {
-                sum = 0;
-                for (int i = 0; i < MOTORS; i++){
-                    sum+=abs(ms[i]->errorTurn);
-                    }
-                //pc.printf("2)%f\n",sum);
-            } while( sum > 0.05f );
-            wait(0.1f);
+            waitAllRotate();
         }
         else {
             ms[0]->drive(0.0);
@@ -156,13 +194,13 @@
             pc.printf("%ld ", ms[i]->encoder.getCount());
         pc.printf("\n");
         */
-        /*
+        
         //info IRidelt
-        for (int i = 0; i < MOTORS; i++)
+        /*for (int i = 0; i < MOTORS; i++)
             pc.printf("%d ", ss[i].read());
         pc.printf("\n");
-        wait(1.0f);
-        */
+        wait(1.0f);*/
+        
         //pc.printf("%d", active);
         //pc.scanf("%f", &turn);
         //m.rotate(turn, 0.2);