Diff: main.cpp

Revision:

45:409988ab53e4

Parent:

44:c2acf8d5e191

Child:

46:49f3da891e24

--- a/main.cpp	Mon Jun 20 15:47:07 2016 +0000
+++ b/main.cpp	Tue Jun 21 10:56:08 2016 +0000
@@ -94,7 +94,8 @@
         sum = 0;
         for (int i = 0; i < MOTORS; i++)
             sum += abs(ms[i]->errorTurn);
-        pc.printf("");
+        
+        pc.printf("%.2f %d%d\n",speed,dir,turn);
         //pc.printf("%f\n",sum);
     }
     while(sum > 0.05f);
@@ -178,20 +179,20 @@
 
 void turnTwo(){
     ms[0]->rotate(turn*dir*0.125f,speed/3);
-    ms[1]->drive(0.0);
+    ms[1]->rotate(0.0f, 1.0f);
     ms[2]->rotate(-turn*dir*0.875f,speed);
-    ms[3]->drive(0.0);
+    ms[3]->rotate(0.0f, 1.0f);
     ms[4]->rotate(-turn*dir*0.875f,speed);
-    ms[5]->drive(0.0);
+    ms[5]->rotate(0.0f, 1.0f);
     waitAllRotate();
     }
     
 void turnThree(){    
-    ms[0]->drive(0.0);
+    ms[0]->rotate(0.0f, 1.0f);
     ms[1]->rotate(turn*dir*0.875f,speed);
-    ms[2]->drive(0.0);
+    ms[2]->rotate(0.0f, 1.0f);
     ms[3]->rotate(-turn*dir*0.125f,speed/3);
-    ms[4]->drive(0.0);
+    ms[4]->rotate(0.0f, 1.0f);
     ms[5]->rotate(turn*dir*0.875f,speed);
     waitAllRotate();
 }
@@ -226,13 +227,15 @@
         //speed = 0;
         //int oldactive = active;
         if (pc.readable()){
-            pc.scanf("%f %d %d", &speed, &dir, &turn);
+            int rfSpeed = 0;
+            pc.scanf("%d %d %d", &rfSpeed, &dir, &turn);
+            speed = rfSpeed/10.0f;
             if (speed >= 10)
                 speed = speed - (((int)(speed/10))*10);
             else if (speed <= 0.25f)
                 speed = float(0);
         }
-        pc.printf("%f %d%d\n",speed,dir,turn);
+        pc.printf("%.2f %d%d\n",speed,dir,turn);
                 
         if (speed != 0){
             //for (int i = 0; i < MOTORS; i++)
@@ -262,30 +265,14 @@
                     turnOne();
                 }    
             }
-            
-            /*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(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]->rotate(0.0f, 0.0f);
-            ms[1]->rotate(0.0f, 0.0f);
-            ms[2]->rotate(0.0f, 0.0f);
-            ms[3]->rotate(0.0f, 0.0f);
-            ms[4]->rotate(0.0f, 0.0f);
-            ms[5]->rotate(0.0f, 0.0f);
+            ms[0]->rotate(0.0f, 1.0f);
+            ms[1]->rotate(0.0f, 1.0f);
+            ms[2]->rotate(0.0f, 1.0f);
+            ms[3]->rotate(0.0f, 1.0f);
+            ms[4]->rotate(0.0f, 1.0f);
+            ms[5]->rotate(0.0f, 1.0f);
         }
         
         //pc.printf("%f", speed);