Diff: main.cpp

Revision:

36:906323851366

Parent:

35:bce9ac485178

Child:

37:8021b3ce241a

--- a/main.cpp	Thu Jun 02 15:12:01 2016 +0000
+++ b/main.cpp	Wed Jun 15 11:52:08 2016 +0000
@@ -29,8 +29,8 @@
 EncoderMotor m1(m1Data, enc1Data, speedPIDData, turnPIDData, NULL);
 DigitalIn s1(PA_8);
 // 2
-MotorData m2Data = {PB_7, PC_14, PA_6};  //PB7 = fault dir2 oli enne PC13
-EncoderData enc2Data = {PC_15, PH_0, 102.083 * 64};
+MotorData m2Data = {PB_7, PB_9, PA_6};  //PB7 = fault dir2 oli enne PC13
+EncoderData enc2Data = {PC_15, PC_14, 102.083 * 64};
 EncoderMotor m2(m2Data, enc2Data, speedPIDData, turnPIDData, NULL);
 DigitalIn s2(PH_1);
 // 3
@@ -56,7 +56,7 @@
 
 const int MOTORS = 6;
 EncoderMotor* ms[MOTORS] = {&m1, &m2, &m3, &m4, &m5, &m6};
-volatile int active = 0;
+volatile float mod = 0.25;
 DigitalIn ss[MOTORS] = {s1, s2, s3, s4, s5, s6};
 
 Ticker ticker;
@@ -65,6 +65,7 @@
 {
     //pc.printf("rise\n");
     //m1.drive(0);
+    //mod = mod*(-1);
 }
 
 void fall()
@@ -91,33 +92,77 @@
     
     ticker.attach(&tick, tickTime);
     
-    float volatile rot, speed;
+    //unsigned char volatile rfget;
     
-    speed = 0.0;
+    float volatile speed;
+    
+    float volatile sum;
+    
     
     while(1)
-    {/*
-        int oldactive = active;
-        pc.scanf("%d %f", &active, &speed);
-        //active--;
-        if (active != oldactive){
-            ms[oldactive]->drive(0);
-            }
-        ms[active]->drive(speed);
-        */
+    {
+      
+        speed = 0.925f;
+        //int oldactive = active;
+        //pc.scanf("%f", &speed);
+        
+        if (speed != 0){
+            speed = (speed*0.3f); //+ ((speed < 0) ? -1 : 1) * mod;
+        
+            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);
+            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 );
+            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);
+        }
+        else {
+            ms[0]->drive(0.0);
+            ms[1]->drive(0.0);
+            ms[2]->drive(0.0);
+            ms[3]->drive(0.0);
+            ms[4]->drive(0.0);
+            ms[5]->drive(0.0);
+        }
+        
+        //pc.printf("%f", speed);
         //info enkoodritelt
-        
+        /*
         for (int i = 0; i < MOTORS; i++)
             pc.printf("%ld ", ms[i]->encoder.getCount());
         pc.printf("\n");
-        
+        */
         /*
         //info IRidelt
         for (int i = 0; i < MOTORS; i++)
             pc.printf("%d ", ss[i].read());
         pc.printf("\n");
+        wait(1.0f);
         */
-        
         //pc.printf("%d", active);
         //pc.scanf("%f", &turn);
         //m.rotate(turn, 0.2);
@@ -128,6 +173,6 @@
         //pc.printf("%f %f\n", m.s, m.getSetSpeed());
         //pc.printf("%ld %f %f\n", m1.getEncoder().getCount(), m1.getEncoder().getTurn(), m1.getSetTurn());
         //pc.printf("%f %f\n", m.getEncoder().getTurn(), m.getSetTurn());
-        wait(1.f / 10);
+        
     }
 }   
\ No newline at end of file