Diff: main.cpp

Revision:

45:d257047912c1

Parent:

44:8872b20bc1bd

--- a/main.cpp	Fri Nov 03 04:26:04 2017 +0000
+++ b/main.cpp	Fri Nov 03 05:29:36 2017 +0000
@@ -30,6 +30,10 @@
     DigitalOut  green(LED_GREEN);
     Serial      pc(USBTX, USBRX);
 
+
+double original_angle1 =  2.35;
+double original_angle2 = 0.785;
+
 // EMG Variables & Constants
     //Right Biceps
     double emg1;
@@ -142,8 +146,8 @@
     //float MV2 = 0;
     double count1 = 0;
     double count2 = 0;
-    double angle1 = 2.35;
-    double angle2 = 0.785;
+    double angle1;
+    double angle2;
    
 // BiQuad Filter Settings
     // Right Biceps
@@ -692,8 +696,9 @@
         encoders();
         double count1;
         double count2;
-        angle1 += (0.0981 * count1);
-        angle2 += (0.0981 * count2);
+        pc.printf("\n\rcount1=%i count2=%i\r",count1,count2);
+        angle1 = original_angle1 + (0.000747998251 * count1);
+        angle2 = original_angle2 + (0.000747998251 * count2);
         if (angle1<setpoint1 && angle2<setpoint2) {
         pc.printf("\n\rangle1<setpoint1 && angle2<setpoint2\r");
         motor1direction = 1; // counterclockwise rotation
@@ -718,6 +723,7 @@
         pc.printf("\n\rdit noem ik maar 1\r");
         motor1pwm = 0.2;
         motor2pwm = 0.2;
+        pc.printf("\n\rangle1=%d angle2=%d setpoint1=%d setpoint2=%d\r",angle1,angle2,setpoint1,setpoint2);
         }
         else if ((angle1<setpoint1 || angle1>setpoint1) && (angle2 == setpoint2)) {
         pc.printf("\n\rdit noem ik maar 2\r");