Diff: main.cpp

Revision:

43:0627a2245b9d

Parent:

42:7fa713d5d1af

Child:

44:c2acf8d5e191

--- a/main.cpp	Wed Jun 15 18:52:43 2016 +0000
+++ b/main.cpp	Thu Jun 16 16:56:25 2016 +0000
@@ -54,9 +54,14 @@
 
 const int MOTORS = 6;
 EncoderMotor* ms[MOTORS] = {&m1, &m2, &m3, &m4, &m5, &m6};
-volatile float mod = 0.25;
 DigitalIn ss[MOTORS] = {s1, s2, s3, s4, s5, s6};
 
+float volatile speed;
+    
+int volatile dir;
+    
+int volatile turn;
+
 Ticker ticker;
 
 void rise()
@@ -89,7 +94,8 @@
         sum = 0;
         for (int i = 0; i < MOTORS; i++)
             sum += abs(ms[i]->errorTurn);
-        pc.printf("%f\n",sum);
+        pc.printf("");
+        //pc.printf("%f\n",sum);
     }
     while(sum > 0.05f);
     
@@ -124,7 +130,7 @@
     for (int i = 0; i < MOTORS; i++)
         ms[i]->getEncoder().reset();
     
-    pc.printf("done");
+    pc.printf("done\n");
 }
 
 void standUp()
@@ -138,6 +144,28 @@
     waitAllRotate();
 }
 
+void moveOne()
+{
+    ms[0]->rotate(dir*0.75f,speed);
+    ms[1]->rotate(dir*0.25f,speed/3);
+    ms[2]->rotate(dir*0.75f,speed);
+    ms[3]->rotate(dir*0.25f,speed/3);
+    ms[4]->rotate(dir*0.75f,speed);
+    ms[5]->rotate(dir*0.25f,speed/3);
+    waitAllRotate();
+}
+
+void moveTwo(){
+    ms[0]->rotate(dir*0.25f,speed/3);
+    ms[1]->rotate(dir*0.75f,speed);
+    ms[2]->rotate(dir*0.25f,speed/3);
+    ms[3]->rotate(dir*0.75f,speed);
+    ms[4]->rotate(dir*0.25f,speed/3);
+    ms[5]->rotate(dir*0.75f,speed);
+    waitAllRotate();
+}
+
+
 int main()
 {   
     pc.printf("MAIN\n");
@@ -153,40 +181,58 @@
     calibrateLegs();
     standUp();
     
-    float volatile speed;
+    //float volatile sum;
     
-    float volatile sum;
+    speed = 0.0f;
+    dir = 1;
+    turn = 0;
     
+    pc.printf("start\n");
     while(1)
     {
       
-        speed = 1.5f;
+        //speed = 1.5f;
         //speed = 0;
         //int oldactive = active;
-        //pc.scanf("%f", &speed);
-        
+        if (pc.readable()){
+            pc.scanf("%f %d %d", &speed, &dir, &turn);
+            if (speed >= 10){
+                speed = speed - (((int)(speed/10))*10);
+                }
+            }
+        pc.printf("%f %d%d\n",speed,dir,turn);
+                
         if (speed != 0){
-            for (int i = 0; i < MOTORS; i++)
-                pc.printf("%ld ", ms[i]->encoder.getCount());
-            pc.printf("\n");
+            //for (int i = 0; i < MOTORS; i++)
+            //    pc.printf("%ld ", ms[i]->encoder.getCount());
+            //pc.printf("\n");
+            
+            //speed = (rfget*0.3f); //+ ((speed < 0) ? -1 : 1) * mod;
             
-            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);
+            if (dir == 1){
+                moveOne();
+                moveTwo();
+                }
+            else {
+                moveTwo();
+                moveOne();
+                }
+            
+            /*ms[0]->rotate(dir*0.75f,speed);
+            ms[1]->rotate(dir*0.25f,speed/3);
+            ms[2]->rotate(dir*0.75f,speed);
+            ms[3]->rotate(dir*0.25f,speed/3);
+            ms[4]->rotate(dir*0.75f,speed);
+            ms[5]->rotate(dir*0.25f,speed/3);
             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);
-            waitAllRotate();
+            ms[0]->rotate(dir*0.25f,speed/3);
+            ms[1]->rotate(dir*0.75f,speed);
+            ms[2]->rotate(dir*0.25f,speed/3);
+            ms[3]->rotate(dir*0.75f,speed);
+            ms[4]->rotate(dir*0.25f,speed/3);
+            ms[5]->rotate(dir*0.75f,speed);
+            waitAllRotate();*/
         }
         else {
             ms[0]->drive(0.0);