Diff: main.cpp

Revision:

8:935abf8ecc27

Parent:

7:2f74dfd1d411

Child:

9:e4c34f5665a0

--- a/main.cpp	Wed Oct 19 13:28:38 2016 +0000
+++ b/main.cpp	Wed Oct 19 14:16:10 2016 +0000
@@ -91,30 +91,33 @@
     //get velocity vector v = (Pe*- Pe) = [0; dx; dy] from EMG  
     float biceps1 = button1.read();
     float biceps2 = button2.read();
-    while (biceps1 > 0){
-        if (biceps2 > 0){ //both arms activated: stamp moves down
-            dx = 0;
-            dy = dy_stampdown; //into stamping vertical position?? ~the stamp down action
-            wait(1);
-            dy = -(dy_stampdown);  //reset vertical position
-            }
-            else{           //left arm activated
-            dx = biceps1;
-            dy = 0;
-            }
-    while (biceps2 > 0){
-        if (biceps1 <= 0){  //right arm activated
-            dx = -biceps2;
-            dy = 0;
-            } 
-            }
-    
+    if (biceps1 > 0 && biceps2 > 0){
+        //both arms activated: stamp moves down
+        dx = 0;
+        dy = dy_stampdown; //into stamping vertical position?? ~the stamp down action
+         wait(1);
+        dy = -(dy_stampdown);  //reset vertical position
+        }
+    else if (biceps1 > 0 && biceps2 <= 0){
+        //arm 1 activated, move left
+        dx = -biceps1;
+        dy = 0;
+        }
+    else if (biceps1 <= 0 && biceps2 > 0){
+        //arm 1 activated, move left
+        dx = biceps2;
+        dy = 0;
+        }
+    else{
+        dx=0;
+        dy=0;
+        }
+            
     //get joint angles change q_dot = Jpseudo * TwistEndEff  (Matlab)
     float q1_dot = dy*(((x0 + L1*cos(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (x0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1))) - dx*(((L0 + L1*sin(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (L0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)));
 
     //update joint angles
     q1 = q1 + q1_dot;
-    }
     return q1_dot;
     }