Diff: main.cpp

Revision:

35:82b432f5aef7

Parent:

34:6e74f6629a0e

Child:

36:2fbc53a2747c

--- a/main.cpp	Fri Nov 03 01:21:32 2017 +0000
+++ b/main.cpp	Fri Nov 03 01:33:27 2017 +0000
@@ -516,12 +516,12 @@
             green = 0;
             //MV1 = 0.5;
             //MV2 = 0;
-            x = x + 0.2;
+            x = x + 0.1;
             double alpha = inversekinematics1(x,y); //calculate alpha and beta for current x,y
             double beta = inversekinematics2(x,y);
             setpoint1 = alpha;
             setpoint2 = beta;
-            wait(0.2);
+            wait(0.1f);
         }            
         // This part checks for left biceps contractions only  
         else if (maxpart1>MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
@@ -530,12 +530,12 @@
             green = 1;
             //MV1 = -0.5;
             //MV2 = 0;
-            x = x - 0.2;
+            x = x - 0.1;
             double alpha = inversekinematics1(x,y);
             double beta = inversekinematics2(x,y);
             setpoint1 = alpha;
             setpoint2 = beta;
-            wait(0.2);
+            wait(0.1f);
         }
         // This part checks for left lower arm contractions only  
         else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4>MOVAVG_RL){
@@ -544,12 +544,12 @@
             green = 1;
             //MV1 = 0;
             //MV2 = 0.5;
-            y = y + 0.2;
+            y = y + 0.1;
             double alpha = inversekinematics1(x,y);
             double beta = inversekinematics2(x,y);
             setpoint1 = alpha;
             setpoint2 = beta;
-            wait(0.2);
+            wait(0.1f);
         }
         // This part checks for right lower arm contractions only
         else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4<MOVAVG_RL){
@@ -558,12 +558,12 @@
             green = 0;
             //MV1 = 0;
             //MV2 = -0.5;
-            y = y - 0.2;
+            y = y - 0.1;
             double alpha = inversekinematics1(x,y);
             double beta = inversekinematics2(x,y);
             setpoint1 = alpha;
             setpoint2 = beta;
-            wait(0.2);
+            wait(0.2f);
         }           
 /*        // This part checks for both lower arm contractions only 
         else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4<MOVAVG_RL){
@@ -673,11 +673,13 @@
     else {
     motor1direction = 1; // clockwise rotation
     }
-    if (angle1<setpoint1 || angle1>setpoint1) {
-    motor1pwm = 0.5;
-    }
-    else if (angle1 == setpoint1){
-    motor1pwm = 0;
+    while (angle1<setpoint1 || angle1>setpoint1) {
+        if (angle2<setpoint2 || angle2>setpoint2) {
+        motor1pwm = 0.5;
+        }
+        else if (angle1 == setpoint1){
+        motor1pwm = 0;
+        }
     }
 }
 // check if motor1 needs to be activated
@@ -691,11 +693,13 @@
     else {
     motor2direction = 1; // clockwise rotation
     }
-    if (angle2<setpoint2 || angle2>setpoint2) {
-    motor2pwm = 0.5;
-    }
-    else if (angle2 == setpoint2){
-    motor2pwm = 0;
+    while (angle2<setpoint2 || angle2>setpoint2) {
+        if (angle2<setpoint2 || angle2>setpoint2) {
+        motor2pwm = 0.5;
+        }
+        else if (angle2 == setpoint2){
+        motor2pwm = 0;
+        }
     }
 }