Diff: PROJECT_main.cpp

Revision:

8:75980dc35763

Parent:

7:ca1ade91bd14

Child:

9:0bc7f83b761e

--- a/PROJECT_main.cpp	Mon Nov 03 18:22:20 2014 +0000
+++ b/PROJECT_main.cpp	Mon Nov 03 18:52:57 2014 +0000
@@ -103,8 +103,17 @@
 float previouserror = 0;
 float pwm = 0;
 
+float pwm1 =0;
+float integral1 = 0;
+float derivative1 = 0;
+float controlerror1 = 0;
+float previouserror1 = 0;
+
+int state = 1;
+int count = 1;
+
 float omrekenfactor1 = 0.0028035714; // 6.28/(32*70)
-float omrekenfactor2 =  0.0015213178; // 6.28/(24*172); 
+float omrekenfactor2 =  0.0015213178; // 6.28/(24*172);
 
 float setpoint1 = 0; //te behalen speed van motor1 (37D)
 float setpoint2 = 0; //te behalen hoek van motor2 (25D)
@@ -134,7 +143,7 @@
     looptimerflag = true;
     scope.set(0, motor2.getPosition()*omrekenfactor2);
     scope.set(0, motor1.getPosition()*omrekenfactor1);
-    scope.send(); 
+    scope.send();
 
 }
 
@@ -307,7 +316,7 @@
     motor1dir = 0; //linksom
     wait(1);                // anders wordt de while(1) meteen onderbroken
     while(1) {
-        if(motor1.getSpeed()*omrekenfactor1 > -0.20 && motor1.getSpeed()*omrekenfactor1 < 0.20) { // motor1.getSpeed()*omrekenfactor1 < 0.20, 0.20 is nog aan te passen 
+        if(motor1.getSpeed()*omrekenfactor1 > -0.20 && motor1.getSpeed()*omrekenfactor1 < 0.20) { // motor1.getSpeed()*omrekenfactor1 < 0.20, 0.20 is nog aan te passen
             pwm_motor1.write(0);
             motor1.setPosition(0);
             goto emgcal;
@@ -323,35 +332,35 @@
     wait (1);
     for1 = for2 = for3 = 0;
 
-if(kalibratie==0) {
-    //biceps kalibratie
-    blink.attach(kalbi, 0.2);
-    for (int kaltime = 0; kaltime<KALIBRATIONTIME; kaltime++) {
-        if (bi_max < bi_result) {
-            bi_max = bi_result;
+    if(kalibratie==0) {
+        //biceps kalibratie
+        blink.attach(kalbi, 0.2);
+        for (int kaltime = 0; kaltime<KALIBRATIONTIME; kaltime++) {
+            if (bi_max < bi_result) {
+                bi_max = bi_result;
+            }
+            wait (0.01);
         }
-        wait (0.01);
-    }
-    blink.detach();
-    dir1 = dir2 = dir3 = 1;
-    pc.printf("kalbi ");
-    wait (1);
+        blink.detach();
+        dir1 = dir2 = dir3 = 1;
+        pc.printf("kalbi ");
+        wait (1);
 
-    //triceps kalibratie
-    blink.attach(kaltri, 0.2);
-    for (int kaltime = 0; kaltime<KALIBRATIONTIME; kaltime++) {
-        if (tri_max < tri_result) {
-            tri_max = tri_result;
+        //triceps kalibratie
+        blink.attach(kaltri, 0.2);
+        for (int kaltime = 0; kaltime<KALIBRATIONTIME; kaltime++) {
+            if (tri_max < tri_result) {
+                tri_max = tri_result;
+            }
+            wait (0.01);
         }
-        wait (0.01);
+        blink.detach();
+        for1 = for2 = for3 = 1;
+        pc.printf("kaltri ");
+        wait (1);
+        for1 = for2 = for3 = 0;
+        kalibratie = 1;
     }
-    blink.detach();
-    for1 = for2 = for3 = 1;
-    pc.printf("kaltri ");
-    wait (1);
-    for1 = for2 = for3 = 0;
-    kalibratie = 1;
-}
 
 directionchoice:
     log_timer.attach(looper, TSAMP_EMG);
@@ -515,168 +524,225 @@
 
     /* Vanaf hier komt de aansturing van de motor */
 
-    switch (direction) {
-        case 1:
-            setpoint2 = (0.436332313+0.197222205);    //25 graden + 11,3 graden, slag naar linkerdoel
-            break;
-        case 2:
-            setpoint2 = (0.436332313);                //25 graden vanaf nul-punt (precies midden)
-            break;
-        case 3:
-            setpoint2 = (0.436332313-0.197222205);    // 25 graden - 11,3 graden, slag naar rechterdoel
-            break;
-    }
+
+// FORMAT_CODE_START
+    setpoint1=0;
+    setpoint2=0;
+    integral1 = integral = 0;
+    previouserror1 = previouserror = 0;
+    while(1) {      // loop voor het goed plaatsen van motor2 (batje hoek)
+        while(!looptimerflag)
+            looptimerflag = false; //clear flag
+
+        switch (direction) {
+            case 1:
+                setpoint2 = (0.436332313+0.197222205);    //25 graden + 11,3 graden, slag naar linkerdoel
+                break;
+            case 2:
+                setpoint2 = (0.436332313);                //25 graden vanaf nul-punt (precies midden)
+                break;
+            case 3:
+                setpoint2 = (0.436332313-0.197222205);    // 25 graden - 11,3 graden, slag naar rechterdoel
+                break;
+        }
 
-    switch (force) {
-        case 1:
-            setpoint1 = 6.8;
-            break;
-        case 2:
-            setpoint1 = 7.4;
-            break;
-        case 3:
-            setpoint1 = 8.0;
-            break;
-    }
+        switch(state) {
+            case 1:
+                setpoint1=0;
+                if(abs(motor2.getPosition()*omrekenfactor2-setpoint2)<0.01) {
+                    state = 2;
+                }
+                break;
+                
+            case 2:
+                switch (force) {
+                    case 1:
+                        setpoint1 += 6.8*TSAMP;
+                        break;
+                    case 2:
+                        setpoint1 += 7.4*TSAMP;
+                        break;
+                    case 3:
+                        setpoint1 += 8.0*TSAMP;
+                        break;
+                }
+                if(abs(motor1.getPosition()*omrekenfactor1)>2.1){
+                    state = 3;    
+                }                
+                break;
+            case 3:
+                setpoint2 = 0;
+                setpoint1 -= 1.0*TSAMP;
+                if(setpoint1 < 0){
+                    state = 4;    
+                }            
+                break;
 
-    while(1) {      // loop voor het goed plaatsen van motor2 (batje hoek)
-        while(!looptimerflag);
-        looptimerflag = false; //clear flag
+            case 4:
+                setpoint1 = setpoint2 = 0;       
+                count++;
+                if(count>1000){
+                    count = 0;
+                    state = 1;
+                    goto directionchoice;
+                }     
+                break;
+        }
+
+        //motor regeling
+
+        //regelaar motor1, bepaalt positie
+        controlerror1 = setpoint1 - motor1.getPosition()*omrekenfactor1;
+        integral1 = integral1 + controlerror1*TSAMP;
+        derivative1 = (controlerror1 - previouserror1)/TSAMP;
+        pwm1 = Kp1*controlerror1 + Ki1*integral1 + Kd1*derivative1;
+        previouserror1 = controlerror1;
+
+        keep_in_range(&pwm1, -1,1);
+        pwm_motor1.write(abs(pwm1));
+        if(pwm1 > 0) {
+            motor1dir = 1;
+        } else {
+            motor1dir = 0;
+        }
+
 
         //regelaar motor2, bepaalt positie
         controlerror = setpoint2 - motor2.getPosition()*omrekenfactor2;
         integral = integral + controlerror*TSAMP;
         derivative = (controlerror - previouserror)/TSAMP;
         pwm = Kp2*controlerror + Ki2*integral + Kd2*derivative;
-        previouserror = controlerror; 
+        previouserror = controlerror;
 
-        keep_in_range(&pwm, -1,1);    
-        pwm_motor2.write(abs(pwm));    
+        keep_in_range(&pwm, -1,1);
+        pwm_motor2.write(abs(pwm));
         if(pwm > 0) {
             motor2dir = 1;
         } else {
             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.05 || motor2.getPosition()*omrekenfactor2 < setpoint2*0.95) {
+                        batjeset = 0;
+                    } else {
+                        batjeset++;
+                    }
+                } else {
+                    pwm_motor2.write(0);
+                    batjeset = integral = derivative = previouserror = 0;
+                    wait(1);
+                    //goto motor1control;
+                }
+        */
+    }
+    /*
+    motor1control:
+        while(1) {      // loop voor het slaan mbv motor1 (batje snelheid)
+            while(!looptimerflag);
+            looptimerflag = false; //clear flag
 
-        //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.05 || motor2.getPosition()*omrekenfactor2 < setpoint2*0.95) {
-                batjeset = 0;
-            } else {
-                batjeset++;
+            if (balhit == 0) {  //regelaar motor1, bepaalt snelheid
+                controlerror = setpoint1 - motor1.getSpeed()*omrekenfactor1;
+                integral = integral + controlerror*TSAMP;
+                derivative = (controlerror - previouserror)/TSAMP;
+                pwm = Kp1*controlerror + Ki1*integral + Kd1*derivative;
+                previouserror = controlerror;
+            } else {            //regelaar motor1, bepaalt positie
+                balhit = integral = derivative = previouserror = 0;
+                goto resetpositionmotor1;
             }
-        } else {
-            pwm_motor2.write(0);
-            batjeset = integral = derivative = previouserror = 0;
-            wait(1);
-            goto motor1control;
-        }
-    }
+
+            keep_in_range(&pwm, -1,1);
+            pwm_motor1.write(abs(pwm));
+
+            if(pwm > 0) {
+                motor1dir = 1;
+            } else {
+                motor1dir = 0;
+            }
 
-motor1control:
-    while(1) {      // loop voor het slaan mbv motor1 (batje snelheid)
-        while(!looptimerflag);
-        looptimerflag = false; //clear flag
+            //controleert of batje balletje heeft bereikt
+            //if (motor1.getSpeed()*omrekenfactor1 >= 7.5 && motor1.getPosition()*omrekenfactor1 > 1.03 && motor1.getPosition()*omrekenfactor1 < 1.07) { vrij specifieke if-statement ter controle
+            if (motor1.getPosition()*omrekenfactor1 > 1.60) {
+                balhit = 1;
+            }
+        }
+    // FORMAT_CODE_END
 
-        if (balhit == 0) {  //regelaar motor1, bepaalt snelheid
-            controlerror = setpoint1 - motor1.getSpeed()*omrekenfactor1;
+    resetpositionmotor1:
+        while(1) {      // slagarm wordt weer in oorspronkelijke positie geplaatst
+            while(!looptimerflag);
+            looptimerflag = false; //clear flag
+
+            //regelaar motor1, bepaalt positie
+            controlerror = -1*motor1.getPosition()*omrekenfactor1;
             integral = integral + controlerror*TSAMP;
             derivative = (controlerror - previouserror)/TSAMP;
-            pwm = Kp1*controlerror + Ki1*integral + Kd1*derivative;
-            previouserror = controlerror; 
-        } else {            //regelaar motor1, bepaalt positie
-            balhit = integral = derivative = previouserror = 0;
-            goto resetpositionmotor1;
-        }
+            pwm = Kp3*controlerror + Ki3*integral + Kd3*derivative;
+            previouserror = controlerror;
+
+            keep_in_range(&pwm, -1,1);
+            if(pwm > 0) {
+                motor1dir = 1;
+            } else {
+                motor1dir = 0; //1 = rechtsom, 0 = linksom
+            }
+
+            pwm_motor1.write(abs(pwm));
 
-        keep_in_range(&pwm, -1,1);
-        pwm_motor1.write(abs(pwm));
-
-        if(pwm > 0) {
-            motor1dir = 1;
-        } else {
-            motor1dir = 0;
+            //controleert of arm terug in positie is
+            if(batjeset < 200) {
+                if (motor1.getPosition()*omrekenfactor1 > 0.1 || motor1.getPosition()*omrekenfactor1 < -0.1) {
+                    batjeset = 0;
+                } else {
+                    batjeset++;
+                }
+            } else {
+                pwm_motor1.write(0);
+                batjeset = integral = derivative = previouserror = 0;
+                wait(1);
+                goto resetpositionmotor2;
+            }
         }
 
-        //controleert of batje balletje heeft bereikt
-        //if (motor1.getSpeed()*omrekenfactor1 >= 7.5 && motor1.getPosition()*omrekenfactor1 > 1.03 && motor1.getPosition()*omrekenfactor1 < 1.07) { vrij specifieke if-statement ter controle
-        if (motor1.getPosition()*omrekenfactor1 > 1.60) {
-            balhit = 1; 
-        }
-    }
-
-resetpositionmotor1:
-    while(1) {      // slagarm wordt weer in oorspronkelijke positie geplaatst
-        while(!looptimerflag);
-        looptimerflag = false; //clear flag
+    resetpositionmotor2:
+        while(1) {      // loop voor het goed plaatsen van motor2 (batje hoek)
+            while(!looptimerflag);
+            looptimerflag = false; //clear flag
 
-        //regelaar motor1, bepaalt positie
-        controlerror = -1*motor1.getPosition()*omrekenfactor1;
-        integral = integral + controlerror*TSAMP;
-        derivative = (controlerror - previouserror)/TSAMP;
-        pwm = Kp3*controlerror + Ki3*integral + Kd3*derivative;
-        previouserror = controlerror; 
+            //regelaar motor2, bepaalt positie
+            controlerror = -1*motor2.getPosition()*omrekenfactor2;
+            integral = integral + controlerror*TSAMP;
+            derivative = (controlerror - previouserror)/TSAMP;
+            pwm = Kp4*controlerror + Ki4*integral + Kd4*derivative;
+            previouserror = controlerror;
 
-        keep_in_range(&pwm, -1,1);
-        if(pwm > 0) {
-            motor1dir = 1;
-        } else {
-            motor1dir = 0; //1 = rechtsom, 0 = linksom
-        }
+            keep_in_range(&pwm, -1,1);
 
-        pwm_motor1.write(abs(pwm));
-
-        //controleert of arm terug in positie is
-        if(batjeset < 200) {
-            if (motor1.getPosition()*omrekenfactor1 > 0.1 || motor1.getPosition()*omrekenfactor1 < -0.1) {
-                batjeset = 0;
+            if(pwm > 0) {
+                motor2dir = 1;
             } else {
-                batjeset++;
+                motor2dir = 0;
             }
-        } else {
-            pwm_motor1.write(0);
-            batjeset = integral = derivative = previouserror = 0;
-            wait(1);
-            goto resetpositionmotor2;
-        }
-    }
 
-resetpositionmotor2:
-    while(1) {      // loop voor het goed plaatsen van motor2 (batje hoek)
-        while(!looptimerflag);
-        looptimerflag = false; //clear flag
-
-        //regelaar motor2, bepaalt positie
-        controlerror = -1*motor2.getPosition()*omrekenfactor2;
-        integral = integral + controlerror*TSAMP;
-        derivative = (controlerror - previouserror)/TSAMP;
-        pwm = Kp4*controlerror + Ki4*integral + Kd4*derivative;
-        previouserror = controlerror; 
-
-        keep_in_range(&pwm, -1,1);
+            pwm_motor2.write(abs(pwm));
 
-        if(pwm > 0) {
-            motor2dir = 1;
-        } else {
-            motor2dir = 0;
-        }
-
-        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.1 || motor2.getPosition()*omrekenfactor2 < -0.1) {
-                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 > 0.1 || motor2.getPosition()*omrekenfactor2 < -0.1) {
+                    batjeset = 0;
+                } else {
+                    batjeset++;
+                }
             } else {
-                batjeset++;
+                pwm_motor2.write(0);
+                batjeset = integral = derivative = previouserror = 0;
+                wait(1);
+                direction = force = 0;
+                goto motor1cal;
             }
-        } else {
-            pwm_motor2.write(0);
-            batjeset = integral = derivative = previouserror = 0;
-            wait(1);
-            direction = force = 0;
-            goto motor1cal;
-        }
-    }
+        }*/
 } // end main
\ No newline at end of file