Diff: main.cpp

Revision:

23:08255478f6cd

Parent:

22:02a9b5914cc7

--- a/main.cpp	Fri Nov 03 04:28:04 2017 +0000
+++ b/main.cpp	Fri Nov 03 05:09:15 2017 +0000
@@ -216,7 +216,7 @@
     emgLPRT = LPFRT.step(emgAbsHPRT);       // Low-pass filter: creates envelope
     emgMEANSUBRT = emgLPRT - RESTMEANRT;    //substract the restmean value
     RTF = emgLPRT/MVCRT;       // Scale to maximum signal: useful for motor
-        
+        pc.printf("LBF = %f, RBF = %f, LTF = %f, RTF = %f \r\n", LBF, RBF, LTF, RTF);
    //if (emgFiltered >1)
     //{
     //    emgFiltered=1.00;
@@ -386,7 +386,7 @@
     if(Timescalibration>6000)
     {
          pc.printf("calibratie afgelopen");
-         State = Demo; 
+         State = SelectDevice; 
     }
    // pc.printf("maxi waarde = %f emgfinal = %f \r\n",maxi,emgfinal);
    //}
@@ -553,9 +553,9 @@
     if (automode == 0)
     {
         Treecko.detach();
-        for (int n = 0; n < 100; n++)
+        for (int n = 0; n < 50; n++)
         {
-            Rsy -= 0.0002;
+            Rsy -= 0.002;
             RKI();
             
             // hier the control of the control system
@@ -569,12 +569,13 @@
             double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
             SetMotor2(motorValue2);
             //pc.printf("encoder 2 = %f\r\n",Huidigepositie2);
-            pc.printf("refP2 = %f, Huidigepositie2 = %f, error = %f, motorValue2 = %f \r\n", refP2, Huidigepositie2, error2, motorValue2);
+            pc.printf("Rsy= %f, RefP = %f, RefP2 = %f\r\n", Rsy, refP, refP2);
+            wait(0.05);
         }
         
-        for (int n = 0; n < 100; n++)
+        for (int n = 0; n < 50; n++)
         {
-            Rsy += 0.0002;
+            Rsy += 0.002;
             RKI();
             // hier the control of the control system
             double Huidigepositie = motor1.getPosition(); 
@@ -587,12 +588,13 @@
             double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
             SetMotor2(motorValue2);
             //pc.printf("encoder 2 = %f\r\n",Huidigepositie2);
-            pc.printf("refP2 = %f, Huidigepositie2 = %f, error = %f, motorValue2 = %f \r\n", refP2, Huidigepositie2, error2, motorValue2);
+            pc.printf("Rsy= %f, RefP = %f, RefP2 = %f\r\n", Rsy, refP, refP2);
+            wait(0.05);
         }
         
-        for (int n = 0; n < 100; n++)
+        for (int n = 0; n < 50; n++)
         {
-            Rsx += 0.0002;
+            Rsx += 0.002;
             RKI();
             // hier the control of the control system
             double Huidigepositie = motor1.getPosition(); 
@@ -605,12 +607,13 @@
             double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
             SetMotor2(motorValue2);
             //pc.printf("encoder 2 = %f\r\n",Huidigepositie2);
-            pc.printf("refP2 = %f, Huidigepositie2 = %f, error = %f, motorValue2 = %f \r\n", refP2, Huidigepositie2, error2, motorValue2);
+            pc.printf("Rsx= %f, RefP = %f, RefP2 = %f\r\n", Rsx, refP, refP2);
+            wait(0.05);
         }
     
-        for (int n = 0; n < 100; n++)
+        for (int n = 0; n < 50; n++)
         {
-            Rsx -= 0.0002;
+            Rsx -= 0.002;
             RKI();
             // hier the control of the control system
             double Huidigepositie = motor1.getPosition(); 
@@ -623,7 +626,8 @@
             double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
             SetMotor2(motorValue2);
             //pc.printf("Motor2.getposition() 2 = %f\r\n",Huidigepositie2);
-            pc.printf("refP2 = %f, Huidigepositie2 = %f, error = %f, motorValue2 = %f \r\n", refP2, Huidigepositie2, error2, motorValue2);
+            pc.printf("Rsx= %f, RefP = %f, RefP2 = %f\r\n", Rsx, refP, refP2);
+            wait(0.05);
         }
     
         automode = 1;