Diff: main.cpp

Revision:

8:7814ba9f801e

Parent:

7:aad7d5cdecd3

Child:

9:70cab4bf38a5

--- a/main.cpp	Wed Oct 21 13:46:28 2015 +0000
+++ b/main.cpp	Thu Oct 22 14:46:51 2015 +0000
@@ -9,6 +9,7 @@
 Serial      pc2(USBTX, USBRX);
 Ticker      sample_timer;
 Ticker      movement_timer;
+Ticker      calc_timer;
 PinDetect   cali(SW2);
 PinDetect   start(SW3);
 int         counter = 0;
@@ -16,6 +17,9 @@
 bool        calibrating = false;
 bool        go_motor = true;
 bool        go_tick = false;
+bool        go_sample = false;
+
+double      emg_out0, emg_out1, emg_out2;
 
 double generateSample()
 {
@@ -39,19 +43,18 @@
 {
     if(!started) return;
     // EMG Reader
-    double output0 = generateSample();
-    double output1 = 0.0;
-    double output2 = 0.0;
-
+    
+    double a,b;
+    
     // In Between Controller
-    double lastA, lastB, nextA, nextB;
-    getCurrent(lastA, lastB);
-
-    newPos(output0, output1, output2, lastA, lastB, nextA, nextB);
-
-    pc2.printf("CURRENT: %d deg | %d deg\r\nNEW: %d deg | %d deg\r\n",rad2deg(lastA), rad2deg(lastB), rad2deg(nextA), rad2deg(nextB));
-    // Rotate the motors
-    rotate(nextA,nextB);
+    getCurrent(a,b);
+    double x,y;
+    a = getOffset(1) - a;
+    b = getOffset(2) - b;
+    //if(output2 == 1) newPos(0,0,1,a,b,x,y);
+    newPos(emg_out0, emg_out1, 0, a, b, x, y);
+    pc2.printf("O0: %f | O1: %f | O2: %f\r\n",emg_out0, emg_out1, emg_out2);
+    rotate(x,y);
 }
 
 void goMotor()
@@ -64,6 +67,11 @@
     go_tick = true;
 }
 
+void goSample() 
+{
+    go_sample = true;
+}
+
 void startAll()
 {
     started = !started;
@@ -76,34 +84,37 @@
     }
 }
 
-int ding = 0;
 void calibrate()
 {
-    ding++;
-    double a,b;
-    if(ding == 1) {
-        //sample_timer.attach(&goTick, 0.002);
-        Point2Angles(0,20,a,b);
-        double radA, radB;
-        float pX, pY;
-        //pc2.printf("Moving to (0,20) A: %d | B: %d\r\n",rad2deg(a),rad2deg(b));
-        rotate(a,b);
-        getCurrent(radA, radB);
-        Angles2Point(-radA + getOffset(1),-radB + getOffset(2),pX,pY);
-        pc2.printf("Px1: %f | Py1: %f\r\n",pX,pY);
+    sample_timer.attach(&goSample, 0.002);
+    calc_timer.attach(&goTick, 0.5);
+    
+    /*if(ding >= 1 && ding <= 2) {
+        getCurrent(a,b);
+        pc2.printf("CUR A: %d (%f) | CUR B: %d (%f)\r\n",rad2deg(a),a,rad2deg(b),b);
+        double x,y;
+        a = getOffset(1) - a;
+        b = getOffset(2) - b;
+        newPos(0,1,0,a,b,x,y);
+        pc2.printf("CUR A: %d (%f) | CUR B: %d (%f)\r\n",rad2deg(a),a,rad2deg(b),b);
+        pc2.printf("NEW A: %d (%f) | NEW B: %d (%f)\r\n",rad2deg(x),x,rad2deg(y),y);
+        //Point2Angles(-10,18,a,b);
+        rotate(x,y);
+        if(ding == 6) ding = 0;
     }
-
-    if(ding == 2) {
-        Point2Angles(-20,20,a,b);
-        double radA, radB;
-        float pX, pY;
-        //pc2.printf("Moving to (-20,20) A: %d | B: %d\r\n",rad2deg(a),rad2deg(b));
-        rotate(a,b);
-        getCurrent(radA, radB);
-        Angles2Point(-radA + getOffset(1),-radB + getOffset(2),pX,pY);
-        pc2.printf("Px2: %f | Py2: %f\r\n",pX,pY);
+    if(ding == 3) {  
+        getCurrent(a,b);
+        pc2.printf("CUR A: %d (%f) | CUR B: %d (%f)\r\n",rad2deg(a),a,rad2deg(b),b);
+        double x,y;
+        a = getOffset(1) - a;
+        b = getOffset(2) - b;
+        newPos(0,0,1,a,b,x,y);
+        pc2.printf("CUR A: %d (%f) | CUR B: %d (%f)\r\n",rad2deg(a),a,rad2deg(b),b);
+        pc2.printf("NEW A: %d (%f) | NEW B: %d (%f)\r\n",rad2deg(x),x,rad2deg(y),y);
+        //Point2Angles(-10,18,a,b);
+        rotate(x,y);
         ding = 0;
-    }
+    }*/
 }
 
 void init()
@@ -134,6 +145,7 @@
     pc2.printf("A: %f | B: %f\r\n",a,b);
 
     movement_timer.attach(&goMotor, 0.01f);
+    //sample_timer.attach(&goTick, 1f);
 
     rotate(a,b);
 
@@ -142,6 +154,10 @@
             go_motor = false;
             moveTick();
         }
+        if(go_sample) {
+            go_sample = false;
+            sample(emg_out0, emg_out1, emg_out2);   
+        }
         if(go_tick) {
             go_tick = false;
             tick();