Diff: PROJECT_main.cpp

Revision:

4:697d5a806cc4

Parent:

3:2b2710b8b02e

Child:

5:e5ca53305b87

diff -r 2b2710b8b02e -r 697d5a806cc4 PROJECT_main.cpp
--- a/PROJECT_main.cpp	Fri Oct 31 19:38:07 2014 +0000
+++ b/PROJECT_main.cpp	Mon Nov 03 10:49:21 2014 +0000
@@ -10,7 +10,6 @@
 #define TSAMP_EMG 0.002 //sample frequency emg
 #define KALIBRATIONTIME 1000 // 10 sec voor bepalen van maximale biceps/triceps waarde
 #define FACTOR 0.6 //factor*max_waarde = threshold emg
-
 //Define objects
 AnalogIn    emg0(PTB1);         //Analog input biceps
 AnalogIn    emg1(PTB2);         //Analog input triceps
@@ -92,8 +91,8 @@
 PwmOut pwm_motor2(PTA5);
 
 float integral = 0;
-uint8_t batjeset = 0; //een if statement wordt "true" wanneer batje voor een bepaalde tijd de juiste hoek heeft behouden
-uint8_t balhit = 0;  //balhit wordt 1 wanneer arm een bepaalde hoek heeft afgelegd
+float batjeset = 0; //een if statement wordt "true" wanneer batje voor een bepaalde tijd de juiste hoek heeft behouden
+float balhit = 0;  //balhit wordt 1 wanneer arm een bepaalde hoek heeft afgelegd
 float controlerror = 0;
 float pwm = 0;
 
@@ -275,30 +274,35 @@
 
     //calibration motor 2
     pwm_motor2.write(0.6); //lage PWM
-    motor2dir = 1;
+    motor2dir = 0;
     wait(1);                // anders wordt de while(1) meteen onderbroken
-    while(motor2.getSpeed()*omrekenfactor2>0) {
-        if(motor2.getSpeed()*omrekenfactor2 < 0.70) { //0.70 is nog aan te passen
+    while(1) {
+        if(motor2.getSpeed()*omrekenfactor2 > -0.70 && motor2.getSpeed()*omrekenfactor2 < 0.70) { // ik weet niet of het rechtsom of linksom zal gaan en misschien is de speed bij de gekozen pwm wel binnen dit bereik
             pwm_motor2.write(0);
-            motor2.setPosition(0);            
+            motor2.setPosition(0);
+            goto motor1cal;
         }
         wait(0.01);
     }
+motor1cal:
     //calibration motor 1
     pwm_motor1.write(0.55); //lage PWM
-    motor1dir = 0;
+    motor1dir = 1;
     wait(1);                // anders wordt de while(1) meteen onderbroken
-    while(motor1.getSpeed()*omrekenfactor1<0) {
-        if(motor1.getSpeed()*omrekenfactor1 > -0.20 ) { // 0.20 is nog aan te passen
+    while(1) {
+        if(motor1.getSpeed()*omrekenfactor1 > -0.20 && motor1.getSpeed()*omrekenfactor1 < 0.20) { // ik weet niet of het rechtsom of linksom zal gaan en misschien is de speed bij de gekozen pwm wel binnen dit bereik
             pwm_motor1.write(0);
-            motor1.setPosition(0);            
+            motor1.setPosition(0);
+            goto emgcal;
         }
         wait(0.01);
     }
+emgcal:
     blink.detach();
     blink2.detach();
     dir1 = dir2 = dir3 = 1;
-    for1 = for2 = for3 = 1; 
+    for1 = for2 = for3 = 1;
+    pc.printf("kalmoarm ");
     wait (1);
     for1 = for2 = for3 = 0;
 
@@ -312,6 +316,7 @@
     }
     blink.detach();
     dir1 = dir2 = dir3 = 1;
+    pc.printf("kalbi ");
     wait (1);
 
     //triceps kalibratie
@@ -324,63 +329,14 @@
     }
     blink.detach();
     for1 = for2 = for3 = 1;
+    pc.printf("kaltri ");
     wait (1);
     for1 = for2 = for3 = 0;
 
-    
-    while(1) { //Loop van directiekeuze, naar slag en naar nulpositie
+directionchoice:
     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;
@@ -389,6 +345,7 @@
                 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
                     if(bi_result>FACTOR*bi_max) {
                         direction = 1;
+                        pc.printf("links ");
                         wait(TIMEB4NEXTCHOICE);                 // Tijdelijke wait om cyaan lampje aan te zetten ter controle selectie
                         goto forcechoice;                       // goes to second while(1) for the deciding the force
                     } else {
@@ -403,6 +360,7 @@
                 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
                     if(bi_result>FACTOR*bi_max) {
                         direction = 2;
+                        pc.printf("mid ");
                         wait(TIMEB4NEXTCHOICE);                            // Tijdelijke wait om paars lampje aan te zetten ter controle selectie
                         goto forcechoice;
                     } else {
@@ -417,6 +375,7 @@
                 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
                     if(bi_result>FACTOR*bi_max) {
                         direction = 3;
+                        pc.printf("rechts ");
                         wait(TIMEB4NEXTCHOICE);                            // Tijdelijke wait om oranje lampje aan te zetten ter controle selectie
                         goto forcechoice;
                     } else {
@@ -426,9 +385,8 @@
             }
         }
     }
-
-    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
+forcechoice:
+    while(1) { //Loop keuze FORCE
         for(int j=1; j<4; j++) {
             if(j==1) {           //red
                 for1=1;
@@ -437,10 +395,12 @@
                 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
                     if(tri_result>FACTOR*tri_max) {
                         for1 = for2 = for3 = 0;
+                        pc.printf("reset ");
                         goto directionchoice;
                     } else {
                         if(bi_result>FACTOR*bi_max) {
                             force = 1;
+                            pc.printf("zwak ");
                             wait(TIMEB4NEXTCHOICE);                            // Tijdelijke wait om cyaan lampje aan te zetten ter controle selectie
                             goto choicesmade;
                         } else {
@@ -456,10 +416,12 @@
                 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
                     if(tri_result>FACTOR*tri_max) {
                         for1 = for2 = for3 = 0;
+                        pc.printf("reset ");
                         goto directionchoice;
                     } else {
                         if(bi_result>FACTOR*bi_max) {
                             force = 2;
+                            pc.printf("normaal ");
                             wait(TIMEB4NEXTCHOICE);                            // Tijdelijke wait om paars lampje aan te zetten ter controle selectie
                             goto choicesmade;
                         } else {
@@ -475,10 +437,12 @@
                 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
                     if(tri_result>FACTOR*tri_max) {
                         for1 = for2 = for3 = 0;
+                        pc.printf("reset ");
                         goto directionchoice;
                     } else {
                         if(bi_result>FACTOR*bi_max) {
                             force = 3;
+                            pc.printf("sterk ");
                             wait(TIMEB4NEXTCHOICE);                            // Tijdelijke wait om oranje lampje aan te zetten ter controle selectie
                             goto choicesmade;
                         } else {
@@ -492,9 +456,10 @@
 
 choicesmade:
     blink.attach(okay, 0.2);
-    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) { 
+    while(1) {
+        if(tri_result>FACTOR*tri_max) {
             blink.detach();
+            pc.printf("reset ");
             switch (direction) {
                 case 1:
                     dir1 = 1;
@@ -512,31 +477,30 @@
                     for2 = for3 = 0;
                     break;
             }
-            balhit = 2; //zodat het de motorcontrol overslaat
-            force = 0;
-            wait(1);                                // 1 sec wait, anders reset je meteen ook de biceps keuze            
+
+            wait(1);                                // 1 sec wait, anders reset je meteen ook de biceps keuze
+            goto forcechoice;
         } else {
             if(bi_result>FACTOR*bi_max && (dir1==1||dir2==1||dir3==1)) {
-                balhit = 1; 
+                blink.detach();
+                log_timer.detach();
+                goto motorcontrol;
             } else {
                 wait(0.01);                          // not sure of de wait noodzakelijk is (nu toegevoegd zodat het niet teveel strain levert op bordje)
             }
         }
     }
-    
-    log_timer.detach();
-    blink.detach();
-    wait(1);
-        
-    if(balhit!=2){
+
+motorcontrol:
+
     /* Vanaf hier komt de aansturing van de motor */
 
     switch (direction) {
         case 1:
-            setpoint2 = -2*0.197222205;
+            setpoint2 = 2*0.11;
             break;
         case 2:
-            setpoint2 = -1*0.197222205;
+            setpoint2 = 0.11;
             break;
         case 3:
             setpoint2 = 0;
@@ -545,17 +509,17 @@
 
     switch (force) {
         case 1:
-            setpoint1 = 6.8;
+            setpoint1 = 8;
             break;
         case 2:
-            setpoint1 = 7.4;
+            setpoint1 = 8;
             break;
         case 3:
             setpoint1 = 8;
             break;
     }
 
-    while(batjeset<200) {      // loop voor het goed plaatsen van motor2 (batje hoek)
+    while(1) {      // loop voor het goed plaatsen van motor2 (batje hoek)
         while(!looptimerflag);
         looptimerflag = false; //clear flag
 
@@ -572,27 +536,37 @@
             motor2dir = 0;
         }
 
-        //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;
+        //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++;
+            }
         } else {
-            batjeset++;
+            pwm_motor2.write(0);
+            batjeset = integral = 0;
+            wait(1);
+            goto motor1control;
         }
     }
-    pwm_motor2.write(0);
-    batjeset = integral = 0;
-    wait(1);
-    
-    while(balhit==0) {      // loop voor het slaan mbv motor1 (batje snelheid)
+
+motor1control:
+    while(1) {      // loop voor het slaan mbv motor1 (batje snelheid)
         while(!looptimerflag);
         looptimerflag = false; //clear flag
 
-        //regelaar motor1, bepaalt snelheid
-        controlerror = setpoint1 - motor1.getSpeed()*omrekenfactor1;
-        integral = integral + controlerror*TSAMP;
-        pwm = Kp1*controlerror + Ki1*integral;
-        
+        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;
+        }
+
         keep_in_range(&pwm, -1,1);
         pwm_motor1.write(abs(pwm));
 
@@ -608,11 +582,9 @@
             balhit = 1;
         }
     }
-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
+
+resetpositionmotor1:
+    while(1) {      // slagarm wordt weer in oorspronkelijke positie geplaatst
         while(!looptimerflag);
         looptimerflag = false; //clear flag
 
@@ -631,17 +603,22 @@
         pwm_motor1.write(abs(pwm));
 
         //controleert of arm terug in positie is
-        if (motor1.getPosition()*omrekenfactor1 > 0.03 || motor1.getPosition()*omrekenfactor1 < -0.03) {
-            batjeset = 0;
+        if(batjeset < 200) {
+            if (motor1.getPosition()*omrekenfactor1 > 0.03 || motor1.getPosition()*omrekenfactor1 < -0.03) {
+                batjeset = 0;
+            } else {
+                batjeset++;
+            }
         } else {
-            batjeset++;
+            pwm_motor1.write(0);
+            batjeset = integral = 0;
+            wait(1);
+            goto resetpositionmotor2;
         }
     }
-    pwm_motor1.write(0);
-    batjeset = integral = 0;
-    wait(1);
-    
-    while(batjeset < 200) {      // loop voor het goed plaatsen van motor2 (batje hoek)
+
+resetpositionmotor2:
+    while(1) {      // loop voor het goed plaatsen van motor2 (batje hoek)
         while(!looptimerflag);
         looptimerflag = false; //clear flag
 
@@ -661,18 +638,18 @@
         pwm_motor2.write(abs(pwm));
 
         //controleert of batje positie heeft bepaald
-        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;
+        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++;
+            }
         } else {
-            batjeset++;
-        }            
+            pwm_motor2.write(0);
+            batjeset = integral = 0;
+            wait(1);
+            direction = force = 0;
+            goto directionchoice;
+        }
     }
-    pwm_motor2.write(0);
-    batjeset = integral = 0;
-    direction = force = 0;
-    wait(1);    
-    } 
-                                                          
-    }         
 } // end main
\ No newline at end of file