Diff: main.cpp

Revision:

47:4f418a4b0051

Parent:

46:49f3da891e24

--- a/main.cpp	Tue Jun 21 11:41:18 2016 +0000
+++ b/main.cpp	Tue Jun 21 14:43:44 2016 +0000
@@ -56,10 +56,12 @@
 EncoderMotor* ms[MOTORS] = {&m1, &m2, &m3, &m4, &m5, &m6};
 DigitalIn ss[MOTORS] = {s1, s2, s3, s4, s5, s6};
 
+float volatile rxSpeed;
+int volatile rxDir;
+int volatile rxTurn;
+
 float volatile speed;
-    
 int volatile dir;
-    
 int volatile turn;
 
 Ticker ticker;
@@ -95,8 +97,8 @@
         for (int i = 0; i < MOTORS; i++)
             sum += abs(ms[i]->errorTurn);
         
-        pc.printf("");
-        //pc.printf("%f\n",sum);
+        //pc.printf("");
+        pc.printf("%f\n",sum);
     }
     while(sum > 0.05f);
     
@@ -107,7 +109,7 @@
 {
     for (int i = 0; i < MOTORS; i++)
     {
-        ms[i]->drive(0.2f);
+        ms[i]->drive(0.60f);
     }
     
     bool done;
@@ -141,40 +143,42 @@
     waitAllRotate();
 }
 
+float groundDivide = 4;
+
 void moveOne()
 {
     
     ms[0]->rotate(dir*0.875f,speed);
-    ms[1]->rotate(dir*0.125f,speed/3);
+    ms[1]->rotate(dir*0.125f,speed/groundDivide);
     ms[2]->rotate(dir*0.875f,speed);
-    ms[3]->rotate(dir*0.125f,speed/3);
+    ms[3]->rotate(dir*0.125f,speed/groundDivide);
     ms[4]->rotate(dir*0.875f,speed);
-    ms[5]->rotate(dir*0.125f,speed/3);
+    ms[5]->rotate(dir*0.125f,speed/groundDivide);
     waitAllRotate();
 }
 
 void moveTwo(){
-    ms[0]->rotate(dir*0.125f,speed/3);
+    ms[0]->rotate(dir*0.125f,speed/groundDivide);
     ms[1]->rotate(dir*0.875f,speed);
-    ms[2]->rotate(dir*0.125f,speed/3);
+    ms[2]->rotate(dir*0.125f,speed/groundDivide);
     ms[3]->rotate(dir*0.875f,speed);
-    ms[4]->rotate(dir*0.125f,speed/3);
+    ms[4]->rotate(dir*0.125f,speed/groundDivide);
     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[1]->rotate(turn*dir*0.125f,speed/groundDivide);
+    ms[2]->rotate(-turn*dir*0.125f,speed/groundDivide);
     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);
+    ms[4]->rotate(-turn*dir*0.125f,speed/groundDivide);
+    ms[5]->rotate(turn*dir*0.125f,speed/groundDivide);
     waitAllRotate();
 }
 
 void turnTwo(){
-    ms[0]->rotate(turn*dir*0.125f,speed/3);
+    ms[0]->rotate(turn*dir*0.125f,speed/groundDivide);
     ms[1]->rotate(0.0f, 1.0f);
     ms[2]->rotate(-turn*dir*0.875f,speed);
     ms[3]->rotate(0.0f, 1.0f);
@@ -187,15 +191,47 @@
     ms[0]->rotate(0.0f, 1.0f);
     ms[1]->rotate(turn*dir*0.875f,speed);
     ms[2]->rotate(0.0f, 1.0f);
-    ms[3]->rotate(-turn*dir*0.125f,speed/3);
+    ms[3]->rotate(-turn*dir*0.125f,speed/groundDivide);
     ms[4]->rotate(0.0f, 1.0f);
     ms[5]->rotate(turn*dir*0.875f,speed);
     waitAllRotate();
 }
 
+void rfRx()
+{
+    int b = pc.getc();
+    int rfSpeed = b & 0x3F;
+    
+    rxSpeed = rfSpeed / 10.f * 2;
+    
+    switch ((b >> 6) & 0x3)
+    {
+        case 0: // fwd
+            rxDir = 1;
+            rxTurn = 0;
+            break;
+        
+        case 1: // bwd
+            rxDir = -1;
+            rxTurn = 0;
+            break;
+        
+        case 2: // left
+            rxDir = 1;
+            rxTurn = -1;
+            break;
+        
+        case 3: // right
+            rxDir = 1;
+            rxTurn = 1;
+            break;
+    }
+}
 
 int main()
-{   
+{
+    pc.attach(&rfRx);
+    
     pc.printf("MAIN\n");
     bt.rise(&rise);
     bt.fall(&fall);
@@ -211,27 +247,18 @@
     
     //float volatile sum;
     
-    speed = 0.0f;
-    dir = 1;
-    turn = 0;
+    rxSpeed = 0.0f;
+    rxDir = 1;
+    rxTurn = 0;
     
     pc.printf("start\n");
     while(1)
     {
-      
-        //speed = 1.5f;
-        //speed = 0;
-        //int oldactive = active;
-        //if (pc.readable()){
-        int rfSpeed = 0;
-        pc.scanf("%d %d %d", &rfSpeed, &dir, &turn);
-        if (rfSpeed > 50){
-            rfSpeed = rfSpeed-(rfSpeed - rfSpeed/10);
-            }
-        speed = rfSpeed/10.0f;
-        if (speed <= 0.5f)
-            speed = float(0);
-        pc.printf("%d %.2f\n",rfSpeed,speed);
+        speed = rxSpeed;
+        dir = rxDir;
+        turn = rxTurn;
+        
+        //pc.printf("%.2f %d %d\n", speed, dir, turn);
                 
         if (speed != 0){
             //for (int i = 0; i < MOTORS; i++)