Diff: main.cpp

Revision:

36:2fbc53a2747c

Parent:

35:82b432f5aef7

Child:

37:6ac3e70787d1

--- a/main.cpp	Fri Nov 03 01:33:27 2017 +0000
+++ b/main.cpp	Fri Nov 03 01:50:10 2017 +0000
@@ -521,7 +521,7 @@
             double beta = inversekinematics2(x,y);
             setpoint1 = alpha;
             setpoint2 = beta;
-            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){
@@ -535,7 +535,7 @@
             double beta = inversekinematics2(x,y);
             setpoint1 = alpha;
             setpoint2 = beta;
-            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){
@@ -549,7 +549,7 @@
             double beta = inversekinematics2(x,y);
             setpoint1 = alpha;
             setpoint2 = beta;
-            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){
@@ -563,7 +563,7 @@
             double beta = inversekinematics2(x,y);
             setpoint1 = alpha;
             setpoint2 = beta;
-            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){
@@ -658,7 +658,7 @@
     
     last_error2 = new_error2;
 }
-*/  
+ */ 
 // Motor control
 //
 //
@@ -666,15 +666,16 @@
 void SetMotor1() {
     //PIDcalculation1();
     filter();
+    count1 = Encoder1.getPulses();
     angle1 += (0.0981 * count1);
     if (angle1<setpoint1){
-    motor1direction = 0; // counterclockwise rotation
+    motor1direction = 1; // counterclockwise rotation
     }
     else {
-    motor1direction = 1; // clockwise rotation
+    motor1direction = 0; // clockwise rotation
     }
-    while (angle1<setpoint1 || angle1>setpoint1) {
-        if (angle2<setpoint2 || angle2>setpoint2) {
+    while (angle1<setpoint1 /*|| angle1>setpoint1*/) {
+        if (angle1<setpoint1 /*|| angle1>setpoint1*/) {
         motor1pwm = 0.5;
         }
         else if (angle1 == setpoint1){
@@ -686,15 +687,16 @@
 void SetMotor2() {
     filter();
     //PIDcalculation2();
+    count2 = Encoder2.getPulses();
     angle2 += (0.0981 * count2);
     if (angle2<setpoint2){
-    motor2direction = 0; // counterclockwise rotation
+    motor2direction = 1; // counterclockwise rotation
     }
     else {
-    motor2direction = 1; // clockwise rotation
+    motor2direction = 0; // clockwise rotation
     }
-    while (angle2<setpoint2 || angle2>setpoint2) {
-        if (angle2<setpoint2 || angle2>setpoint2) {
+    while (angle2<setpoint2 /*|| angle2>setpoint2*/) {
+        if (angle2<setpoint2 /*|| angle2>setpoint2*/) {
         motor2pwm = 0.5;
         }
         else if (angle2 == setpoint2){