Revision 2:8596695c56df, committed 2014-10-30

Comitter:

DominiqueC

Date:

Thu Oct 30 22:57:50 2014 +0000

Parent:

1:0d5864272412

Child:

3:b06ada67fa4f

Commit message:

testscript, posities nog toevoegen de rest doet het!

Changed in this revision

--- a/main.cpp	Thu Oct 30 19:40:26 2014 +0000
+++ b/main.cpp	Thu Oct 30 22:57:50 2014 +0000
@@ -35,7 +35,7 @@
 
 //TextLCD pc(PTE5, PTE3, PTE2, PTB11, PTB10, PTB9); // rs, e, d4-d7 CONTROLEREN!! (Pinnen wel vrij :) )! //Textpc pc(p15, p16, p17, p18, p19, p20, Textpc::pc16x4); // rs, e, d4-d7 ok
 
-Encoder motor2(PTD2,PTD0); //geel,wit kleine motor
+Encoder motor2(PTD2,PTD0); //geel,wit kleine motor MOTOR 2
 Encoder motor1(PTD5,PTA13);//geel,wit
 PwmOut pwm_motor1(M1_PWM);
 PwmOut pwm_motor2(M2_PWM);
@@ -182,52 +182,105 @@
             case KALIBRATIE: {                                  //kalibreren met maximale inspanning
                 max_value_biceps=0;
                 max_value_triceps=0;
-                state = BICEPSMAX;
-                switch(state) {
-                    case BICEPSMAX: {                           //maximale inspanning biceps
-                        pc.printf("Kalibratie. 1:BICEPS MAX");  //pc scherm
-                        wait(1);
-                        tijdtimer.start();
-                        pc.printf("Biceps meting, meting loopt"); //pc scherm
-                        while (tijdtimer <= 3) {
-                            if (envelop_emg0 > max_value_biceps);
-                            {
-                                max_value_biceps = envelop_emg0;
-                            }
-                        }
-                        if (tijdtimer >= 3) {
-                            tijdtimer.stop();
-                            tijdtimer.reset();
-                            pc.printf("max value %f\n\r", max_value_biceps);        //pc scherm
-                            wait(1);
-                            state = TRICEPSMAX;
-                            break;
-                        }                                       //einde if statement
-                    }                                           //einde case bicepsmax
-                    case TRICEPSMAX: {                          //maximale inspanning triceps
-                        pc.printf("Kalibratie. 2:TRICEPS MAX"); //pc scherm
-                        wait(1);
-                        tijdtimer.start();
-                        pc.printf("Triceps meting, meting loopt!"); //pc scherm
-                        while (tijdtimer <= 3) {
-                            if (envelop_emg1 > max_value_triceps) {
-                                max_value_triceps = envelop_emg1;
-                            }
-                        }
-                        if (tijdtimer >= 3) {
-                            tijdtimer.stop();
-                            tijdtimer.reset();
-                            pc.printf("max value %f\n\r", max_value_triceps);
-                            wait(1);
-                        } //einde if statement
-                        break;
-                    } //einde case tricepsmax
-                    default: {
-                        state = BICEPSMAX;
-                    }  //einde default
-                } //einde switch states
+                //maximale inspanning biceps
+                pc.printf("Kalibratie. 1:BICEPS MAX");  //pc scherm
+                wait(1);
+                tijdtimer.start();
+                pc.printf("Biceps meting, meting loopt"); //pc scherm
+                while (tijdtimer <= 3) {
+                    if (envelop_emg0 > max_value_biceps);
+                    {
+                        max_value_biceps = envelop_emg0;
+                    }
+                }
+                tijdtimer.stop();
+                tijdtimer.reset();
+                pc.printf("max value %f\n\r", max_value_biceps);        //pc scherm
+                wait(1);
+
+                //maximale inspanning triceps
+                pc.printf("Kalibratie. 2:TRICEPS MAX"); //pc scherm
+                wait(1);
+                tijdtimer.start();
+                pc.printf("Triceps meting, meting loopt!"); //pc scherm
+                while (tijdtimer <= 3) {
+                    if (envelop_emg1 > max_value_triceps) {
+                        max_value_triceps = envelop_emg1;
+                    }
+                }
+                tijdtimer.stop();
+                tijdtimer.reset();
+                pc.printf("max value %f\n\r", max_value_triceps);
+                wait(1);
+                state = RICHTEN;
                 break;
             }// einde kalibratie case
+
+            case RICHTEN: {                                  //batje richten (gebruik biceps en triceps)
+                pc.printf("Richten");                  //regel 1 LCD scherm
+                pc.printf("Kies goal!");               //regel 2 LCD scherm
+                float setpointkm;
+                float new_pwm_km;
+                wait(3);
+                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);
+                //pc.printf("triceps %f\n\r", kalibratiewaarde_triceps);
+                if (kalibratiewaarde_biceps > 0.3 && kalibratiewaarde_triceps <= 0.3) { //linker goal!
+                    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
+                    pc.printf("rechts");
+                } else { //middelste goal!
+                    setpointkm = 0;
+                    pc.printf("midden");
+                }
+                //pc.printf("setpoint %f ", setpointkm);
+
+                // NU MOTOR 2 LATEN BEWEGEN NAAR setpointkm
+                
+                state = SLAAN;
+                break;
+            }
+            
+            case SLAAN: {
+                pc.printf("Slaan PingPong!");                  //regel 1 LCD scherm
+                pc.printf("Kies goal!");               //regel 2 LCD scherm
+                float thetadot;
+                float setpointgm;
+                float new_pwm_gm;
+                wait(3);
+                float kalibratiewaarde_biceps;
+                kalibratiewaarde_biceps=(envelop_emg0/max_value_biceps);
+                //pc.printf("biceps %f\n\r", kalibratiewaarde_biceps);
+                if (kalibratiewaarde_biceps <= 0.3) { //kalibratiewaarde_biceps<0.3 goal onderin
+                    thetadot=THETADOT0;
+                    pc.printf("Onderste goal");  
+                } else if (kalibratiewaarde_biceps>0.3 && kalibratiewaarde_biceps<=0.6) { //0.3<kalibratiewaarde_biceps<0.6 goal midden
+                    thetadot=THETADOT1;
+                    pc.printf("MIDDELSTE GOAL"); 
+                } else { //goal bovenin
+                    thetadot=THETADOT2;
+                    pc.printf("BOVENSTE GOAL"); 
+                }
+                //pc.printf("thetadot %f ", thetadot);
+                pc.printf("Daar gaat ie!");
+                
+                // NU MOTOR 1 LATEN BEWEGEN met snelheid thetadot
+                
+                state = HOME;
+                break;
+            }
+            
+            case HOME: {
+                // NU MOTOR 1 LATEN BEWEGEN NAAR 0
+                // NU MOTOR 2 LATEN BEWEGEN NAAR 0
+               // state = RICHTEN;      //optioneel
+                break;
+            }
+            
         }
     }
-}       
+}
\ No newline at end of file