Diff: main.cpp

Revision:

3:adc3052039e7

Parent:

2:0cb899f2800a

Child:

4:69540b9c0646

--- a/main.cpp	Thu Oct 16 11:53:26 2014 +0000
+++ b/main.cpp	Thu Oct 16 12:19:15 2014 +0000
@@ -14,12 +14,16 @@
 PwmOut pwm_motor_arm1(PTA5);            //PWM naar motor arm 1
 DigitalOut dir_motor_arm1(PTD1);        //Richting van motor arm 1
 Encoder puls_motor_arm1(PTD0, PTC9);    //Encoder pulsen tellen van motor arm 1
+PwmOut pwm_motor_arm2(PTA12);
+DigitalOut dir_motor_arm2(PTD3);
+Encoder puls_motor_arm2(PTD4, PTC8);
 
 Ticker ticker_regelaar;                  
 
 //Gedefinieerde datatypen en naamgeving
 bool rust = false;                      //Bool voor controleren volgorde in programma
-bool kalibratie_positie = false;        //Bool voor controleren volgorde in programma
+bool kalibratie_positie_arm1 = false;   //Bool voor controleren volgorde in programma
+bool kalibratie_positie_arm2 = false;   
 bool kalibratie_EMG = false;            //Bool voor controleren volgorde in programma
 
 char *lcd_r1 = new char[16];             //Char voor tekst op eerste regel LCD scherm
@@ -44,7 +48,9 @@
     
 
 int puls_arm1_home = 363;
+int puls_arm2_home = 787;
 float pwm_to_motor_arm1;
+float pwm_to_motor_arm2;
 
 //Beginwaarden voor variabelen
 
@@ -55,7 +61,7 @@
     pc.baud(38400);             //PC baud rate is 38400 bits/seconde
     
     //Aanzetten
-    if (rust == false && kalibratie_positie == false && kalibratie_EMG == false){
+    if (rust == false && kalibratie_positie_arm1 == false && kalibratie_positie_arm2 == false && kalibratie_EMG == false){
         lcd_r1 = " BMT M9   GR. 4 ";        //Tekst op eerste regel van LCD scherm
         lcd_r2 = "Hoi! Ik ben PIPO";        //Tekst op tweede regel van LCD scherm
         wait(2);                            //Twee seconden wachten
@@ -66,12 +72,12 @@
     
     
     //Arm 1 naar thuispositie
-    if (rust == true && kalibratie_positie == false && kalibratie_EMG == false){
+    if (rust == true && kalibratie_positie_arm1 == false && kalibratie_positie_arm2 == false && kalibratie_EMG == false){
         wait(1);                            //Een seconde wachten
         
         ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR);       //Ticker iedere zoveel seconde de flag op laten steken
         
-        while(rust == true && kalibratie_positie == false && kalibratie_EMG == false) {
+        while(rust == true && kalibratie_positie_arm1 == false && kalibratie_positie_arm2 == false && kalibratie_EMG == false) {
             while(regelaar_ticker_flag != true) ;
             regelaar_ticker_flag = false;
             
@@ -87,11 +93,40 @@
             pwm_motor_arm1.write(abs(pwm_to_motor_arm1));
             
             if (pwm_to_motor_arm1 == 0) {
-                 kalibratie_positie = true;
+                 kalibratie_positie_arm1 = true;
                  pc.printf("Arm 1 naar thuispositie compleet");
             }        
             wait(1);
             
         }   
-    }    
+    }
+    
+    //Arm 2 naar thuispositie
+        if (rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == false && kalibratie_EMG == false){
+        wait(1);                            //Een seconde wachten
+        
+        //ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR);       //Ticker iedere zoveel seconde de flag op laten steken
+        
+        while(rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == false && kalibratie_EMG == false) {
+            while(regelaar_ticker_flag != true) ;
+            regelaar_ticker_flag = false;
+            
+            pwm_to_motor_arm2 = (puls_arm2_home - puls_motor_arm2.getPosition())*KP;
+            keep_in_range(&pwm_to_motor_arm2, -1, 1);
+            
+            if (pwm_to_motor_arm2 > 0){
+                dir_motor_arm2.write(1);
+            }
+            else {
+                dir_motor_arm2.write(0);
+            }
+            pwm_motor_arm2.write(abs(pwm_to_motor_arm2));
+            
+            if (pwm_to_motor_arm2 == 0) {
+                 kalibratie_positie_arm2 = true;
+                 pc.printf("Arm 2 naar thuispositie compleet");
+            }        
+            wait(1); 
+        }
+    }
 }
\ No newline at end of file