Diff: PROJECT_main.cpp

Revision:

3:2b2710b8b02e

Parent:

2:10dd6a88dfd7

Child:

4:697d5a806cc4

--- a/PROJECT_main.cpp	Fri Oct 31 16:17:18 2014 +0000
+++ b/PROJECT_main.cpp	Fri Oct 31 19:38:07 2014 +0000
@@ -327,10 +327,60 @@
     wait (1);
     for1 = for2 = for3 = 0;
 
-directionchoice:
+    
+    while(1) { //Loop van directiekeuze, naar slag en naar nulpositie
     log_timer.attach(looper, TSAMP_EMG);
+    if(direction==0) {
+        ledtimer.attach(directionchoice, 1);
+    }
+    while(direction==0) { //Loop keuze DIRECTION
+        if(bi_result>FACTOR*bi_max) {
+            switch(ledburning){
+                case 1:
+                direction = 1;
+                break;
+                case 2:
+                direction = 2;
+                break;
+                case 3:                
+                direction = 3;
+                break;    
+            }            
+        } else {
+            wait(0.01);            
+        }
+    }
+    
+    ledtimer.detach();
+    
+    if(force==0) {
+        ledtimer.attach(forcechoice, 1);
+    }
+    while(force==0||balhit!=0) {//Loop keuze FORCE
+        if(bi_result>FACTOR*bi_max) {
+            switch(ledburning){
+                case 1:
+                force = 1;
+                break;
+                case 2:
+                force = 2;
+                break;
+                case 3:                
+                force = 3;
+                break;    
+            }            
+        } else {
+            if(tri_result>FACTOR*tri_max) {
+                balhit = 3;        
+            } else    
+                wait(0.01); 
+            }           
+        }
+    }
+    
+    ledtimer.detach();
 
-    while(1) { //Loop keuze DIRECTION
+
         for(int i=1; i<4; i++) {
             if(i==1) {           //red
                 dir1=1;
@@ -376,8 +426,9 @@
             }
         }
     }
-forcechoice:
-    while(1) { //Loop keuze FORCE
+
+    while(force==0) { //Loop keuze FORCE
+    //dit kan ook een while(force==0) {} zijn, maar dan moet er een ticker gemaakt worden die de lampjes aan en uit laat gaan
         for(int j=1; j<4; j++) {
             if(j==1) {           //red
                 for1=1;
@@ -441,8 +492,8 @@
 
 choicesmade:
     blink.attach(okay, 0.2);
-    while(1) {
-        if(tri_result>FACTOR*tri_max) {
+    while(balhit==0) { //balhit is gewoon een variabele die gebruikt wordt om uit de while te komen, naam kan verwarring veroorzaken
+        if(tri_result>FACTOR*tri_max) { 
             blink.detach();
             switch (direction) {
                 case 1:
@@ -461,22 +512,23 @@
                     for2 = for3 = 0;
                     break;
             }
-
-            wait(1);                                // 1 sec wait, anders reset je meteen ook de biceps keuze
-            goto forcechoice;
+            balhit = 2; //zodat het de motorcontrol overslaat
+            force = 0;
+            wait(1);                                // 1 sec wait, anders reset je meteen ook de biceps keuze            
         } else {
             if(bi_result>FACTOR*bi_max && (dir1==1||dir2==1||dir3==1)) {
-                blink.detach();
-                log_timer.detach();
-                goto motorcontrol;
+                balhit = 1; 
             } else {
                 wait(0.01);                          // not sure of de wait noodzakelijk is (nu toegevoegd zodat het niet teveel strain levert op bordje)
             }
         }
     }
-
-motorcontrol:
-
+    
+    log_timer.detach();
+    blink.detach();
+    wait(1);
+        
+    if(balhit!=2){
     /* Vanaf hier komt de aansturing van de motor */
 
     switch (direction) {
@@ -503,7 +555,7 @@
             break;
     }
 
-    while(1) {      // loop voor het goed plaatsen van motor2 (batje hoek)
+    while(batjeset<200) {      // loop voor het goed plaatsen van motor2 (batje hoek)
         while(!looptimerflag);
         looptimerflag = false; //clear flag
 
@@ -520,37 +572,27 @@
             motor2dir = 0;
         }
 
-        //controleert of batje positie heeft bepaald
-        if(batjeset < 200) {                                // dit is nog te bepalen, op dit moment als binnen marge van 1% voor 2 seconde, dan naar volgende motorcontrol
-            if (motor2.getPosition()*omrekenfactor2 > setpoint2*1.01 || motor2.getPosition()*omrekenfactor2 < setpoint2*0.99) {
-                batjeset = 0;
-            } else {
-                batjeset++;
-            }
+        //controleert of batje positie heeft bepaald                  
+        if (motor2.getPosition()*omrekenfactor2 > setpoint2*1.01 || motor2.getPosition()*omrekenfactor2 < setpoint2*0.99) {
+            // dit is nog te bepalen, op dit moment als binnen marge van 1% voor 2 seconde, dan naar volgende motorcontrol
+            batjeset = 0;
         } else {
-            pwm_motor2.write(0);
-            batjeset = integral = 0;
-            wait(1);
-            goto motor1control;
+            batjeset++;
         }
     }
-
-motor1control:
-    while(1) {      // loop voor het slaan mbv motor1 (batje snelheid)
+    pwm_motor2.write(0);
+    batjeset = integral = 0;
+    wait(1);
+    
+    while(balhit==0) {      // loop voor het slaan mbv motor1 (batje snelheid)
         while(!looptimerflag);
         looptimerflag = false; //clear flag
 
-        if (balhit == 0) {  //regelaar motor1, bepaalt snelheid
-            controlerror = setpoint1 - motor1.getSpeed()*omrekenfactor1;
-            integral = integral + controlerror*TSAMP;
-            pwm = Kp1*controlerror + Ki1*integral;
-        } else {            //regelaar motor1, bepaalt positie
-            pwm_motor1.write(0);
-            balhit = integral = 0;
-            wait(1); // wait voordat arm weer naar beginpositie terugkeert
-            goto resetpositionmotor1;
-        }
-
+        //regelaar motor1, bepaalt snelheid
+        controlerror = setpoint1 - motor1.getSpeed()*omrekenfactor1;
+        integral = integral + controlerror*TSAMP;
+        pwm = Kp1*controlerror + Ki1*integral;
+        
         keep_in_range(&pwm, -1,1);
         pwm_motor1.write(abs(pwm));
 
@@ -566,9 +608,11 @@
             balhit = 1;
         }
     }
-
-resetpositionmotor1:
-    while(1) {      // slagarm wordt weer in oorspronkelijke positie geplaatst
+pwm_motor1.write(0);     
+balhit = integral = 0;
+wait(1); // wait voordat arm weer naar beginpositie terugkeert
+            
+    while(batjeset < 200) {      // slagarm wordt weer in oorspronkelijke positie geplaatst
         while(!looptimerflag);
         looptimerflag = false; //clear flag
 
@@ -587,22 +631,17 @@
         pwm_motor1.write(abs(pwm));
 
         //controleert of arm terug in positie is
-        if(batjeset < 200) {
-            if (motor1.getPosition()*omrekenfactor1 > 0.03 || motor1.getPosition()*omrekenfactor1 < -0.03) {
-                batjeset = 0;
-            } else {
-                batjeset++;
-            }
+        if (motor1.getPosition()*omrekenfactor1 > 0.03 || motor1.getPosition()*omrekenfactor1 < -0.03) {
+            batjeset = 0;
         } else {
-            pwm_motor1.write(0);
-            batjeset = integral = 0;
-            wait(1);
-            goto resetpositionmotor2;
+            batjeset++;
         }
     }
-
-resetpositionmotor2:
-    while(1) {      // loop voor het goed plaatsen van motor2 (batje hoek)
+    pwm_motor1.write(0);
+    batjeset = integral = 0;
+    wait(1);
+    
+    while(batjeset < 200) {      // loop voor het goed plaatsen van motor2 (batje hoek)
         while(!looptimerflag);
         looptimerflag = false; //clear flag
 
@@ -622,18 +661,18 @@
         pwm_motor2.write(abs(pwm));
 
         //controleert of batje positie heeft bepaald
-        if(batjeset < 200) {                                // dit is nog te bepalen, op dit moment als binnen marge van 1% voor 2 seconde, dan naar volgende motorcontrol
-            if (motor2.getPosition()*omrekenfactor2 > 0.03 || motor2.getPosition()*omrekenfactor2 < -0.03) {
-                batjeset = 0;
-            } else {
-                batjeset++;
-            }
+        if (motor2.getPosition()*omrekenfactor2 > 0.03 || motor2.getPosition()*omrekenfactor2 < -0.03) {
+            // dit is nog te bepalen, op dit moment als binnen marge van 1% voor 2 seconde, dan naar volgende motorcontrol
+            batjeset = 0;
         } else {
-            pwm_motor2.write(0);
-            batjeset = integral = 0;
-            direction = force = 0;
-            wait(1);            
-            goto directionchoice;
-        }
+            batjeset++;
+        }            
     }
+    pwm_motor2.write(0);
+    batjeset = integral = 0;
+    direction = force = 0;
+    wait(1);    
+    } 
+                                                          
+    }         
 } // end main
\ No newline at end of file