Diff: main.cpp

Revision:

7:dd3cba61b34b

Parent:

6:3b6fad529520

Child:

8:d4161e9be1da

Child:

9:454e7da8ab65

diff -r 3b6fad529520 -r dd3cba61b34b main.cpp
--- a/main.cpp	Thu Oct 16 15:09:46 2014 +0000
+++ b/main.cpp	Fri Oct 17 07:52:22 2014 +0000
@@ -34,6 +34,13 @@
 char *lcd_r1 = new char[16];            //Char voor tekst op eerste regel LCD scherm
 char *lcd_r2 = new char[16];            //Char voor tekst op tweede regel LCD scherm
 
+int puls_arm1_home = 363;           //Aantal pulsen die de encoder moet hebben geteld wanneer arm 1 op de thuispositie is
+int puls_arm2_home = 787;           //Aantal pulsen die de encoder moet hebben geteld wanneer arm 2 op de thuispositie is
+float pwm_to_motor_arm1;            //PWM output naar motor arm 1
+float pwm_to_motor_arm2;            //PWM output naar motor arm 2
+float xbk;                          //Gemeten EMG waarde biceps in de kalibratie
+int i;                              //
+
 volatile bool regelaar_ticker_flag;     //Definiëren flag als bool die verandert kan worden in programma
 void setregelaar_ticker_flag(){         //Setregelaar_ticker_flag zet de regelaar_ticker_flag op true
     regelaar_ticker_flag = true;        
@@ -55,14 +62,42 @@
         *in = *in;
     }
 }
-    
 
-int puls_arm1_home = 363;           //Aantal pulsen die de encoder moet hebben geteld wanneer arm 1 op de thuispositie is
-int puls_arm2_home = 787;           //Aantal pulsen die de encoder moet hebben geteld wanneer arm 2 op de thuispositie is
-float pwm_to_motor_arm1;            //PWM output naar motor arm 1
-float pwm_to_motor_arm2;            //PWM output naar motor arm 2
-float xbk;                          //Gemeten EMG waarde biceps in de kalibratie
-int i = 0;                          //
+void arm1_naar_thuispositie(){
+    pwm_to_motor_arm1 = (puls_arm1_home - puls_motor_arm1.getPosition())*KP;        //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
+    keep_in_range(&pwm_to_motor_arm1, -1, 1);                                       //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
+            
+    if (pwm_to_motor_arm1 > 0){                     //Als PWM is positief, dan richting 1
+       dir_motor_arm1.write(1);
+    }
+    else {                                          //Anders richting nul
+        dir_motor_arm1.write(0);
+    }
+    pwm_motor_arm1.write(abs(pwm_to_motor_arm1));   //Output PWM naar motor is de absolute waarde van de berekende PWM
+            
+    if (pwm_to_motor_arm1 == 0) {                           //Als het verschil tussen de voorgestelde en huidige positie nul is wordt de while loop gestopt (condities kloppen niet meer)
+        kalibratie_positie_arm1 = true;
+        pc.printf("Arm 1 naar thuispositie compleet");     //Tekst voor controle Arm 1 naar thuispositie
+    }  
+}   
+
+void arm2_naar_thuispositie(){
+    pwm_to_motor_arm2 = (puls_arm2_home - puls_motor_arm2.getPosition())*KP;        //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
+    keep_in_range(&pwm_to_motor_arm2, -1, 1);                                       //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
+            
+    if (pwm_to_motor_arm2 > 0){                     //Als PWM is positief, dan richting 1
+        dir_motor_arm2.write(1);
+    }
+    else {                                          //Anders richting nul
+        dir_motor_arm2.write(0);
+    }
+    pwm_motor_arm2.write(abs(pwm_to_motor_arm2));   //Output PWM naar motor is de absolute waarde van de berekende PWM
+            
+    if (pwm_to_motor_arm2 == 0) {                           //Als het verschil tussen de voorgestelde en huidige positie nul is wordt de while loop gestopt (condities kloppen niet meer)
+        kalibratie_positie_arm2 = true;
+        pc.printf("Arm 2 naar thuispositie compleet");     //Tekst voor controle Arm 2 naar thuispositie
+    }
+}
 
 
 int main() {
@@ -91,28 +126,13 @@
             while(regelaar_ticker_flag != true) ;           //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
             regelaar_ticker_flag = false;                   //Flag weer naar beneden, zodat deze straks weer omhoog kan
             
-            pwm_to_motor_arm1 = (puls_arm1_home - puls_motor_arm1.getPosition())*KP;        //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
-            keep_in_range(&pwm_to_motor_arm1, -1, 1);                                       //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
-            
-            if (pwm_to_motor_arm1 > 0){                     //Als PWM is positief, dan richting 1
-                dir_motor_arm1.write(1);
-            }
-            else {                                          //Anders richting nul
-                dir_motor_arm1.write(0);
-            }
-            pwm_motor_arm1.write(abs(pwm_to_motor_arm1));   //Output PWM naar motor is de absolute waarde van de berekende PWM
-            
-            if (pwm_to_motor_arm1 == 0) {                           //Als het verschil tussen de voorgestelde en huidige positie nul is wordt de while loop gestopt (condities kloppen niet meer)
-                 kalibratie_positie_arm1 = true;
-                 pc.printf("Arm 1 naar thuispositie compleet");     //Tekst voor controle Arm 1 naar thuispositie
-            }        
-            wait(1);        //Een seconde wachten
-            
+            arm1_naar_thuispositie();                       //Voer acties uit om arm 1 naar thuispositie te krijgen
         }   
+        wait(1);                    //Een seconde wachten
     }
     
     //Arm 2 naar thuispositie
-        if (rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){
+    if (rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){
         wait(1);                            //Een seconde wachten
         
         //ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR);       //Ticker iedere zoveel seconde de flag op laten steken
@@ -121,21 +141,7 @@
             while(regelaar_ticker_flag != true) ;           //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
             regelaar_ticker_flag = false;                   //Flag weer naar beneden, zodat deze straks weer omhoog kan
             
-            pwm_to_motor_arm2 = (puls_arm2_home - puls_motor_arm2.getPosition())*KP;        //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
-            keep_in_range(&pwm_to_motor_arm2, -1, 1);                                       //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
-            
-            if (pwm_to_motor_arm2 > 0){                     //Als PWM is positief, dan richting 1
-                dir_motor_arm2.write(1);
-            }
-            else {                                          //Anders richting nul
-                dir_motor_arm2.write(0);
-            }
-            pwm_motor_arm2.write(abs(pwm_to_motor_arm2));   //Output PWM naar motor is de absolute waarde van de berekende PWM
-            
-            if (pwm_to_motor_arm2 == 0) {                           //Als het verschil tussen de voorgestelde en huidige positie nul is wordt de while loop gestopt (condities kloppen niet meer)
-                 kalibratie_positie_arm2 = true;
-                 pc.printf("Arm 2 naar thuispositie compleet");     //Tekst voor controle Arm 2 naar thuispositie
-            }        
+            arm2_naar_thuispositie();                       //Voer acties uit om arm 2 naar thuispositie te krijgen
         }
         wait(1);            //Een seconde wachten
         ticker_regelaar.detach();       //Ticker detachten, ticker doet nu niks meer
@@ -154,8 +160,8 @@
         lcd_r2 = " Span biceps aan";        //Tekst op tweede regel van LCD scherm
         wait(2);                            //Twee seconden wachten
         
-        for (i=0 ,i<5000, i++){
-            while(setregelaar_EMG_flag != true) ;           //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
+        for (i=0; i<1000; i++){
+            while(regelaar_EMG_flag != true) ;              //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
             regelaar_EMG_flag = false;                      //Flag weer naar beneden, zodat deze straks weer omhoog kan
             
             xbk = EMG_bi.read();            //EMG signaal uitlezen