Diff: main.cpp

Revision:

44:c2acf8d5e191

Parent:

43:0627a2245b9d

Child:

45:409988ab53e4

--- a/main.cpp	Thu Jun 16 16:56:25 2016 +0000
+++ b/main.cpp	Mon Jun 20 15:47:07 2016 +0000
@@ -135,33 +135,64 @@
 
 void standUp()
 {
-    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[0]->rotate(0.5f + 0.0625f, 0.75f);
+    ms[1]->rotate(0.5f - 0.0625f, 0.75f);
+    ms[2]->rotate(0.5f + 0.0625f, 0.75f);
+    ms[3]->rotate(0.5f - 0.0625f, 0.75f);
+    ms[4]->rotate(0.5f + 0.0625f, 0.75f);
+    ms[5]->rotate(0.5f - 0.0625f, 0.75f);
     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);
+    
+    ms[0]->rotate(dir*0.875f,speed);
+    ms[1]->rotate(dir*0.125f,speed/3);
+    ms[2]->rotate(dir*0.875f,speed);
+    ms[3]->rotate(dir*0.125f,speed/3);
+    ms[4]->rotate(dir*0.875f,speed);
+    ms[5]->rotate(dir*0.125f,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);
+    ms[0]->rotate(dir*0.125f,speed/3);
+    ms[1]->rotate(dir*0.875f,speed);
+    ms[2]->rotate(dir*0.125f,speed/3);
+    ms[3]->rotate(dir*0.875f,speed);
+    ms[4]->rotate(dir*0.125f,speed/3);
+    ms[5]->rotate(dir*0.875f,speed);
+    waitAllRotate();
+}
+
+void turnOne(){
+    ms[0]->rotate(turn*dir*0.875f,speed);
+    ms[1]->rotate(turn*dir*0.125f,speed/3);
+    ms[2]->rotate(-turn*dir*0.125f,speed/3);
+    ms[3]->rotate(-turn*dir*0.875f,speed);
+    ms[4]->rotate(-turn*dir*0.125f,speed/3);
+    ms[5]->rotate(turn*dir*0.125f,speed/3);
+    waitAllRotate();
+}
+
+void turnTwo(){
+    ms[0]->rotate(turn*dir*0.125f,speed/3);
+    ms[1]->drive(0.0);
+    ms[2]->rotate(-turn*dir*0.875f,speed);
+    ms[3]->drive(0.0);
+    ms[4]->rotate(-turn*dir*0.875f,speed);
+    ms[5]->drive(0.0);
+    waitAllRotate();
+    }
+    
+void turnThree(){    
+    ms[0]->drive(0.0);
+    ms[1]->rotate(turn*dir*0.875f,speed);
+    ms[2]->drive(0.0);
+    ms[3]->rotate(-turn*dir*0.125f,speed/3);
+    ms[4]->drive(0.0);
+    ms[5]->rotate(turn*dir*0.875f,speed);
     waitAllRotate();
 }
 
@@ -196,10 +227,11 @@
         //int oldactive = active;
         if (pc.readable()){
             pc.scanf("%f %d %d", &speed, &dir, &turn);
-            if (speed >= 10){
+            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);
                 
         if (speed != 0){
@@ -208,15 +240,28 @@
             //pc.printf("\n");
             
             //speed = (rfget*0.3f); //+ ((speed < 0) ? -1 : 1) * mod;
-            
-            if (dir == 1){
-                moveOne();
-                moveTwo();
+            if (turn == 0) {
+                if (dir == 1){
+                    moveOne();
+                    moveTwo();
+                }
+                else {
+                    moveTwo();
+                    moveOne();
                 }
+            }
             else {
-                moveTwo();
-                moveOne();
+                if ((turn*dir) == 1){
+                    turnOne();
+                    turnTwo();
+                    turnThree();
                 }
+                else {
+                    turnThree();
+                    turnTwo();
+                    turnOne();
+                }    
+            }
             
             /*ms[0]->rotate(dir*0.75f,speed);
             ms[1]->rotate(dir*0.25f,speed/3);
@@ -235,12 +280,12 @@
             waitAllRotate();*/
         }
         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);
+            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);
         }
         
         //pc.printf("%f", speed);