Diff: main.cpp

Revision:

5:5085197c02be

Parent:

4:527e5b5283c2

Child:

6:98a27fef0223

--- a/main.cpp	Fri Oct 31 16:52:26 2014 +0000
+++ b/main.cpp	Sat Nov 01 12:29:20 2014 +0000
@@ -206,7 +206,7 @@
                         max_value_triceps = envelop_emg1;
                     }
                 }
-               // tijdtimer.stop();
+                // tijdtimer.stop();
                 tijdtimer.reset();
                 pc.printf("max value %f\n\r", max_value_triceps);
                 wait(5);
@@ -216,24 +216,30 @@
             }// einde kalibratie case
 
             case RICHTEN: {                                  //batje richten (gebruik biceps en triceps)
+                wait(3);
                 pc.printf("Richten");                  //regel 1 LCD scherm
                 pc.printf("Kies goal!");               //regel 2 LCD scherm
                 float setpointkm;
                 float new_pwm_km;
-                wait(3);
+                wait(5);
+                pc.printf("Meting loopt");
                 float kalibratiewaarde_biceps,kalibratiewaarde_triceps;
                 kalibratiewaarde_biceps=(envelop_emg0/max_value_biceps);
                 kalibratiewaarde_triceps=(envelop_emg1/max_value_triceps);
-                pc.printf("biceps %f\n\r", kalibratiewaarde_biceps);            //WEGHALEN LATER
-                if (kalibratiewaarde_biceps > 0.3 && kalibratiewaarde_triceps <= 0.3) { //linker goal!
+                pc.printf("biceps %f\n\r", kalibratiewaarde_biceps);
+                pc.printf("triceps %f\n\r", kalibratiewaarde_triceps);        //WEGHALEN LATER
+                //if (kalibratiewaarde_biceps > 0.3 && kalibratiewaarde_triceps <= 0.3) { //linker goal!
+                if (kalibratiewaarde_triceps >= 0.35) {
                     setpointkm = -127;          //11,12graden naar links
                     pc.printf("links");
-                } else if (kalibratiewaarde_biceps <= 0.3 && kalibratiewaarde_triceps > 0.3) { //rechter goal!
-                    setpointkm = 127;        //11,12graden naar rechts
+                    //} else if (kalibratiewaarde_biceps <= 0.3 && kalibratiewaarde_triceps > 0.3) { //rechter goal!
+                } else if (kalibratiewaarde_triceps>0.1 && kalibratiewaarde_triceps<=0.35) {
+                    setpointkm = 0;        //11,12graden naar rechts
+                    pc.printf("midden");
+                    //} else { //middelste goal!
+                } else {
+                    setpointkm = 127;
                     pc.printf("rechts");
-                } else { //middelste goal!
-                    setpointkm = 0;
-                    pc.printf("midden");
                 }
                 //MOTOR 2 LATEN BEWEGEN NAAR setpointkm
                 tijdtimer.reset();
@@ -254,6 +260,7 @@
             }
 
             case SLAAN: {
+                wait(3);
                 pc.printf("Slaan");                  //regel 1 LCD scherm
                 pc.printf("Kies goal");               //regel 2 LCD scherm
                 float thetadot;
@@ -261,7 +268,8 @@
                 float new_pwm_gm;
                 float setpointkm;
                 float new_pwm_km;
-                wait(3);
+                wait(5);
+                pc.printf("Meting loopt");
                 float kalibratiewaarde_biceps;
                 kalibratiewaarde_biceps=(envelop_emg0/max_value_biceps);
                 pc.printf("biceps %f\n\r", kalibratiewaarde_biceps);      //WEGHALEN LATER
@@ -275,6 +283,7 @@
                     thetadot=THETADOT2;
                     pc.printf("BOVENSTE GOAL");
                 }
+                wait(3);
                 pc.printf("Daar gaat ie!");
 
                 //MOTOR 1 LATEN BEWEGEN met snelheid thetadot
@@ -344,7 +353,7 @@
                         motordir2 = 0;  //rechts
                     pwm_motor2.write(abs(new_pwm_km));
                 }
-                //state = RICHTEN;      //optioneel
+                state = RICHTEN;      //optioneel
                 break;
             }
             default: {