Diff: main.cpp

Revision:

39:e35a99801ec1

Parent:

38:b03a5bf9ac7b

Child:

40:01a97bc4ef7a

--- a/main.cpp	Wed Jun 15 14:20:32 2016 +0000
+++ b/main.cpp	Wed Jun 15 14:50:08 2016 +0000
@@ -24,13 +24,13 @@
 */
     
 // 1
-MotorData m1Data = {PB_0, PC_0, PA_4}; //PWM, Dir1, Dir2
-EncoderData enc1Data = {PC_1, PC_3, 102.083 * 64}; //EncA, encB  // https://www.pololu.com/product/2826
+MotorData m1Data = {PB_0, PA_4, PC_0}; //PWM, Dir1, Dir2
+EncoderData enc1Data = {PC_3, PC_1, 102.083 * 64}; //EncA, encB  // https://www.pololu.com/product/2826
 EncoderMotor m1(m1Data, enc1Data, speedPIDData, turnPIDData, NULL);
 DigitalIn s1(PA_8);
 // 2
-MotorData m2Data = {PB_7, PB_9, PA_6};  //PB7 = fault dir2 oli enne PC13
-EncoderData enc2Data = {PC_15, PC_14, 102.083 * 64};
+MotorData m2Data = {PB_7, PA_6, PB_9};  //PB7 = fault dir2 oli enne PC13
+EncoderData enc2Data = {PC_14, PC_15, 102.083 * 64};
 EncoderMotor m2(m2Data, enc2Data, speedPIDData, turnPIDData, NULL);
 DigitalIn s2(PH_1);
 // 3
@@ -49,8 +49,8 @@
 EncoderMotor m5(m5Data, enc5Data, speedPIDData, turnPIDData, NULL);
 DigitalIn s5(PC_4);
 // 6
-MotorData m6Data = {PB_3, PB_5, PA_7};  //PA_2 = TX; PA_3 (m6-fault) = RX DIR2 oli enne PA2
-EncoderData enc6Data = {PA_10, PB_4, 102.083 * 64};
+MotorData m6Data = {PB_3, PA_7, PB_5};  //PA_2 = TX; PA_3 (m6-fault) = RX DIR2 oli enne PA2
+EncoderData enc6Data = {PB_4, PA_10, 102.083 * 64};
 EncoderMotor m6(m6Data, enc6Data, speedPIDData, turnPIDData, NULL); 
 DigitalIn s6(PB_10);
 
@@ -115,7 +115,7 @@
         if (!(i == 2 || i == 3))
         {        
             pc.printf("calib %d\n", i);
-            ms[i]->drive(i == 0 || i == 1 || i == 5 ? -0.5f : 0.5f);
+            ms[i]->drive(0.2f);
             while (ss[i].read());
         }
         ms[i]->drive(0.f);
@@ -127,19 +127,12 @@
         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.75f);
-    ms[1]->rotate(-0.5f + 0.125f, 0.75f);
+    ms[0]->rotate(0.5f + 0.125f, 0.75f);
+    ms[1]->rotate(0.5f - 0.125f, 0.75f);
     ms[2]->rotate(0.5f + 0.125f, 0.75f);
     ms[3]->rotate(0.5f - 0.125f, 0.75f);
     ms[4]->rotate(0.5f + 0.125f, 0.75f);
-    ms[5]->rotate(-0.5f + 0.125f, 0.75f);
+    ms[5]->rotate(0.5f - 0.125f, 0.75f);
     waitAllRotate();
     
     float volatile speed;
@@ -162,20 +155,20 @@
             
             speed = (speed*0.3f); //+ ((speed < 0) ? -1 : 1) * mod;
         
-            ms[0]->rotate(-0.75f,speed*3);
-            ms[1]->rotate(-0.25f,speed);
+            ms[0]->rotate(0.75f,speed*3);
+            ms[1]->rotate(0.25f,speed);
             ms[2]->rotate(0.75f,speed*3);
             ms[3]->rotate(0.25f,speed);
             ms[4]->rotate(0.75f,speed*3);
-            ms[5]->rotate(-0.25f,speed);
+            ms[5]->rotate(0.25f,speed);
             waitAllRotate();
             
-            ms[0]->rotate(-0.25f,speed);
-            ms[1]->rotate(-0.75f,speed*3);
+            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);
+            ms[5]->rotate(0.75f,speed*3);
             waitAllRotate();
         }
         else {