Diff: main.cpp

Revision:

21:9307dc9be4f7

Parent:

20:14edaecd7413

Child:

22:02a9b5914cc7

--- a/main.cpp	Fri Nov 03 02:54:09 2017 +0000
+++ b/main.cpp	Fri Nov 03 03:14:33 2017 +0000
@@ -278,7 +278,7 @@
         RESTMEANRT = emgSUMRT/Timescalibration; //determine the mean rest value
         RESTMEANLT = emgSUMLT/Timescalibration; //determine the mean rest value
     }
-    if(Timescalibration>2000 && Timescalibration<6000)
+    if(Timescalibration>2000 && Timescalibration<3000)
     {
         pc.printf("maximum linker biceps \r\n");
         led = 1;
@@ -304,7 +304,7 @@
     }    
     }
     
-    if(Timescalibration>6000 && Timescalibration<10000)
+    if(Timescalibration>3000 && Timescalibration<4000)
     {
         pc.printf(" maximum rechter biceps \r\n");
         /*led = 1;
@@ -334,7 +334,7 @@
     }    
     }
     
-    if(Timescalibration>10000 && Timescalibration<14000)
+    if(Timescalibration>4000 && Timescalibration<5000)
     {
         pc.printf("maximum linker triceps \r\n");
         led = 1;
@@ -368,7 +368,7 @@
     }    
     }
     
-    if(Timescalibration>14000 && Timescalibration<18000)
+    if(Timescalibration>5000 && Timescalibration<6000)
     {
         pc.printf("maximum rechter triceps");
     emgNotchRT = NFRT.step(emgRT.read() );  
@@ -382,7 +382,7 @@
     }    
     }
     
-    if(Timescalibration>18000)
+    if(Timescalibration>6000)
     {
          pc.printf("calibratie afgelopen");
          State = SelectDevice; 
@@ -413,6 +413,29 @@
     refP2 = (((-pi) + q1 - q2)/(2*pi))*maxwaarde;    //Get reference positions was eerst 0.5*pi
 }
 
+void changePosition ()    // DIT MOET NOG HEEL ERG GETUNED WORDEN !!!
+{
+    if (RBF>0.5) {
+        //goalx++;    // hoe veel verder gaat hij? 1 cm? 10 cm?
+        Rsx += 0.001;
+    }
+    else if (RTF>0.5) {
+        //goalx--;
+        Rsx -= 0.001;
+    }
+    else {}
+    if (LBF>0.5) {
+        //goaly++;
+        Rsy +=0.001;
+    }
+    else if (LTF>0.5) {
+        //goaly--;
+        Rsy -= 0.001;
+    }
+    else {}
+    pc.printf("goalx = %f, goaly = %f\r\n",Rsx, Rsy);
+} 
+
 void SetpointRobot()
 {    
     double Potmeterwaarde2 = potMeter2.read();
@@ -528,7 +551,8 @@
 
 void MeasureAndControl(void)
 {
-    SetpointRobot(); 
+    //SetpointRobot(); 
+    changePosition();
     // RKI aanroepen
     RKI();
     // hier the control of the 1st control system
@@ -546,7 +570,7 @@
     pc.printf("refP = %f, huidigepos = %f, motorvalue = %f, refP2 = %f, huidigepos2 = %f, motorvalue2 = %f \r\n", refP, Huidigepositie, motorValue, refP2, Huidigepositie2, Huidigepositie2);
 }
      
-void changePosition ()    // DIT MOET NOG HEEL ERG GETUNED WORDEN !!!
+/*void changePosition ()    // DIT MOET NOG HEEL ERG GETUNED WORDEN !!!
 {
     if (RBF>0.3) {
         goalx++;    // hoe veel verder gaat hij? 1 cm? 10 cm?
@@ -562,7 +586,7 @@
     }
     pc.printf("goalx = %i, goaly = %i\r\n",goalx, goaly);
 } 
-
+*/
 void Loop_funtion()
 {   
     pc.printf("state machine begint \r\n");
@@ -614,7 +638,7 @@
         case EMG: //Aansturen met EMG
         pc.printf("EMG begint met aansturen \r\n");
             Filteren();
-            changePosition();
+            //changePosition();
             //RKI --> output refP van motor
             MeasureAndControl();
             break;