Diff: main.cpp

Revision:

3:59b504840b95

Parent:

2:2c4ee76dc830

Child:

4:fddab1c875a9

--- a/main.cpp	Wed Oct 25 13:32:29 2017 +0000
+++ b/main.cpp	Thu Oct 26 09:55:24 2017 +0000
@@ -44,6 +44,8 @@
 int             delta1;
 int             delta2; 
 
+bool   Move_done = false;
+
 /* Defining all the different BiQuad filters, which contain a Notch filter,
 High-pass filter and Low-pass filter. The Notch filter cancels all frequencies
 between 49 and 51 Hz, the High-pass filter cancels all frequencies below 20 Hz
@@ -146,7 +148,8 @@
 }
 
 // Initial input value for X                                                            
-int Xin=0;                                                                              //<- Hoe zit het met deze als we de robot daadwerkelijk gebruiken
+int Xin=0;  
+int Xin_new;                                                                           //<- Hoe zit het met deze als we de robot daadwerkelijk gebruiken
 double huidigetijdX;
 
 // Feedback system for counting values of X
@@ -165,28 +168,37 @@
 
 // Couting system for values of X
 int tellerX(){
-    led_G=1; 
-    led_B=1;
-    led_R=0;
+    if (Move_done == true) {
+        t.reset();
+        led_G=1; 
+        led_B=1;
+        led_R=0;
         while(true){
-    //button.fall(ledtX);          //This has to be replaced by EMG   
-    if (emgBRcomplete > thresholdBR){
+        button.fall(ledtX);          //This has to be replaced by EMG   
+        if (emgBRcomplete > thresholdBR){
         ledtX();                 // dit is wat je uiteindelijk wil dat er staat
-    }
-    t.start();
-    huidigetijdX=t.read();
-    if (huidigetijdX>2){
-        led_R=1;                //Go to the next program (couting values for Y)
-        if (emgBRcomplete > thresholdBR){
-        0;                 // dit is wat je uiteindelijk wil dat er staat
         }
-        return 0;
-        }
-    }  
+        
+        t.start();
+        huidigetijdX=t.read();
+        if (huidigetijdX>2){
+            led_R=1;                //Go to the next program (couting values for Y)
+            Xin_new = Xin;
+            Xin = 0;
+            //button.fall(0);   // Wat is deze?
+          
+            return Xin_new;
+            }
+            
+            }
+            
+    } 
+    return 0; 
 }  
 
 // Initial values needed for Y (see comments at X function) 
 int Yin=0;
+int Yin_new;
 double huidigetijdY;
 
 //Feedback system for couting values of Y
@@ -204,6 +216,7 @@
 
 // Couting system for values of Y
 int tellerY(){
+    if (Move_done == true) {
     t.reset();
     led_G=1;
     led_B=0; 
@@ -212,22 +225,27 @@
     //button.fall(ledtY);         //See comments at X   
     if (emgBRcomplete > thresholdBR){
         ledtY();                 // dit is wat je uiteindelijk wil dat er staat
-    }
+        }
     t.start();
     huidigetijdY=t.read();
     if (huidigetijdY>2){
         led_B=1; 
-        if (emgBRcomplete > thresholdBR){
-        0;                 // dit is wat je uiteindelijk wil dat er staat
-        }
+        Yin_new = Yin;
+        Yin = 0;
+        //if (emgBRcomplete > thresholdBR){
+        //0;                 // dit is wat je uiteindelijk wil dat er staat
+        //}
+        //button.fall(0);   // Wat is deze?
+        Move_done = false;
+        return Yin_new;
         
-        //button.fall(0);   // Wat is deze?
-        return 0;      // ga door naar het volgende programma 
         }
     }
+    }
+    return 0;      // ga door naar het volgende programma 
 }
 
-bool   B = false;
+
 
 // Declaring all variables needed for calculating rope lengths, 
 double Pox = 0;
@@ -456,8 +474,8 @@
 
 //Calculating desired end position based on the EMG input                       <- Waarom maar voor een paal?
 double Ps(){
-    Psx=(Xin)*30+121;
-    Psy=(Yin)*30+308;     
+    Psx=(Xin_new)*30+121;
+    Psy=(Yin_new)*30+308;     
     //pc.printf("x=%.2f \n\r y=%.2f \n\r",Psx,Psy);
     return 0;
 }
@@ -474,7 +492,7 @@
     Pst();
     pc.printf("Vex=%.2f \r\n Vey=%.2f \r\n",Vex,Vey);
     if((abs(Vex)<0.01f)&&(abs(Vey)<0.01f)){
-        B=true;
+        Move_done=true;
         loop.detach();
         }
 }
@@ -490,7 +508,7 @@
 void zakker(){
     while(1){
         wait(1);
-    if(B==true){    //misschien moet je hier als voorwaarden een delta is 1 zetten                               // hierdoor wacht dit programma totdat de beweging klaar is  
+    if(Move_done==true){    //misschien moet je hier als voorwaarden een delta is 1 zetten                               // hierdoor wacht dit programma totdat de beweging klaar is  
     dLod=sqrt(pow(Lou,2)+pow(397.85,2))-Lou;    //dit is wat je motoren moeten doen om te zakken
     dLbd=sqrt(pow(Lbu,2)+pow(397.85,2))-Lbu;
     dLrd=sqrt(pow(Lru,2)+pow(397.85,2))-Lru;
@@ -518,12 +536,11 @@
         calculator();
         controlmotor1.attach(&MeasureAndControl1, 0.01);
         calculatedelta1.attach(&calcdelta1, 0.01);
-        printdata1.attach(&readdata1, 0.5); 
+        //printdata1.attach(&readdata1, 0.5); 
         controlmotor2.attach(&MeasureAndControl2, 0.01);
         calculatedelta2.attach(&calcdelta2, 0.01);
-        printdata2.attach(&readdata2, 0.5); 
+        //printdata2.attach(&readdata2, 0.5); 
         //zakker();
-        wait(5.0f);
         }
     return 0;
 }
\ No newline at end of file