Diff: main.cpp

Revision:

40:c40bd1a84e7c

Parent:

39:c15f84af49fd

Child:

41:08d41bb622bb

--- a/main.cpp	Fri Nov 03 02:32:43 2017 +0000
+++ b/main.cpp	Fri Nov 03 03:00:30 2017 +0000
@@ -521,6 +521,7 @@
             double beta = inversekinematics2(x,y);
             setpoint1 = alpha;
             setpoint2 = beta;
+            wait(0.1f);
             
         }            
         // This part checks for left biceps contractions only  
@@ -535,7 +536,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 +550,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 +564,7 @@
             double beta = inversekinematics2(x,y);
             setpoint1 = alpha;
             setpoint2 = beta;
-            
+            wait(0.1f);
         }           
 /*        // This part checks for both lower arm contractions only 
         else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4<MOVAVG_RL){
@@ -665,27 +666,27 @@
 // check to see if motor1 needs to be activated
 void SetMotor1() {
     //PIDcalculation1();
-    filter();
-    while (angle1<setpoint1 || angle1>setpoint1 || angle2<setpoint1 || angle2>setpoint2) {
-        count1 = Encoder1.getPulses();
-        count2 = Encoder2.getPulses();
-        angle1 += (0.0981 * count1);
-        angle2 += (0.0981 * count2);
+    //filter();
+    while (angle1<setpoint1 || angle1>setpoint1 || angle2<setpoint2 || angle2>setpoint2) {
         if (angle1<setpoint1 && angle2<setpoint2) {
-        motor1direction = 0; // counterclockwise rotation
-        motor2direction = 0;
+        motor1direction = 1; // counterclockwise rotation
+        motor2direction = 1;
+        
         }
         else if (angle1>setpoint1 && angle2<setpoint2) {
-        motor1direction = 1; // clockwise rotation
-        motor2direction = 0;
+        motor1direction = 0; // clockwise rotation
+        motor2direction = 1;
+        
         }
         else if (angle1<setpoint1 && angle2>setpoint2) {
-        motor1direction = 0;
-        motor2direction = 1;
+        motor1direction = 1;
+        motor2direction = 0;
+        
         }
         else if (angle1>setpoint1 && angle2>setpoint2) {
-        motor1direction = 1;
-        motor2direction = 1;
+        motor1direction = 0;
+        motor2direction = 0;
+        
         }
         if ((angle1<setpoint1 || angle1>setpoint1) && (angle2<setpoint2 || angle2>setpoint2)) {
         motor1pwm = 0.2;
@@ -703,6 +704,10 @@
         motor1pwm = 0;
         motor2pwm = 0;
         }
+        double count1;
+        double count2;
+        angle1 += (0.0981 * count1);
+        angle2 += (0.0981 * count2);
     }
 }
 /*
@@ -739,8 +744,10 @@
     main_timer.attach(&filter, 0.001); // set frequency for the filters at 1000Hz
     max_read1.attach(&get_max1, 2); // set the frequency of the calibration loop at 0.5Hz
     max_read3.attach(&get_max3, 2);
-    Motorcontrol.attach(&MeasureAndControl,0.5);
+    Motorcontrol.attach(&MeasureAndControl,0.1);
     //PIDtimer.attach(&PIDcalculation1, 0.005);
     //PIDtimer.attach(&PIDcalculation2, 0.005);
+    count1 = Encoder1.getPulses();
+    count2 = Encoder2.getPulses();
     while(1) {}
 }
\ No newline at end of file