Diff: main.cpp

Revision:

9:454e7da8ab65

Parent:

7:dd3cba61b34b

Child:

10:52b22bff409a

--- a/main.cpp	Fri Oct 17 07:52:22 2014 +0000
+++ b/main.cpp	Mon Oct 20 20:06:06 2014 +0000
@@ -5,7 +5,7 @@
 
 //Constanten definiëren en waarde geven
 #define SAMPLETIME_REGELAAR     0.005       //Sampletijd ticker regelaar motor
-#define KP                      1           //Factor proprotionele regelaar
+#define KP                      0.001       //Factor proprotionele regelaar
 #define SAMPLETIME_EMG          0.005       //Sampletijd ticker EMG meten
 
 //Pinverdeling en naamgeving variabelen
@@ -13,24 +13,22 @@
 MODSERIAL       pc(USBTX, USBRX);                               //PC
 
 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);           //PWM naar motor arm 2
-DigitalOut dir_motor_arm2(PTD3);        //Ricting van motor arm 2
-Encoder puls_motor_arm2(PTD4, PTC8);    //Encoder pulsen tellen van motor arm 2
+DigitalOut dir_motor_arm1(PTA4);        //Richting van motor arm 1
+Encoder puls_motor_arm1(PTD2, PTD0);    //Encoder pulsen tellen van motor arm 1
+PwmOut pwm_motor_arm2(PTC8);           //PWM naar motor arm 2
+DigitalOut dir_motor_arm2(PTC9);        //Ricting van motor arm 2
+Encoder puls_motor_arm2(PTD5, PTA13);    //Encoder pulsen tellen van motor arm 2
 AnalogIn EMG_bi(PTB1);                  //Meten EMG signaal biceps
 
 
 Ticker ticker_regelaar;                 //Ticker voor regelaar motor
 Ticker ticker_EMG;                      //Ticker voor EMG meten
 
+//States definiëren
+enum pipostate {RUST, KALIBRATIE_ARM1, KALIBRATIE_ARM2, KALIBRATIE_BICEPS, KALIBRATIE_TRICEPS};     //Alle states benoemen, ieder krijgt een getal beginnend met 0
+uint8_t state = RUST;                   //State is rust aan het begin
+
 //Gedefinieerde datatypen en naamgeving en beginwaarden
-bool rust = false;                      //Bool voor controleren volgorde in programma
-bool kalibratie_positie_arm1 = false;   //Bool voor controleren volgorde in programma
-bool kalibratie_positie_arm2 = false;   //Bool voor controleren volgorde in programma
-bool kalibratie_EMG_bi = false;         //Bool voor controleren volgorde in programma
-bool kalibratie_EMG_tri = false;        //Bool voor controleren volgorde in programma
-
 char *lcd_r1 = new char[16];            //Char voor tekst op eerste regel LCD scherm
 char *lcd_r2 = new char[16];            //Char voor tekst op tweede regel LCD scherm
 
@@ -73,11 +71,13 @@
     else {                                          //Anders richting nul
         dir_motor_arm1.write(0);
     }
-    pwm_motor_arm1.write(abs(pwm_to_motor_arm1));   //Output PWM naar motor is de absolute waarde van de berekende PWM
+    pwm_motor_arm1.write(fabs(pwm_to_motor_arm1));   //Output PWM naar motor is de absolute waarde van de berekende PWM
+    pc.printf("pulsmotorgetposition %f\n\r", puls_motor_arm1.getPosition());
+    pc.printf("pwmmotor %f\n\r", pwm_to_motor_arm1);
             
     if (pwm_to_motor_arm1 == 0) {                           //Als het verschil tussen de voorgestelde en huidige positie nul is wordt de while loop gestopt (condities kloppen niet meer)
-        kalibratie_positie_arm1 = true;
-        pc.printf("Arm 1 naar thuispositie compleet");     //Tekst voor controle Arm 1 naar thuispositie
+        state = KALIBRATIE_ARM2;                            //State wordt kalibratie arm 2, zo daar naar volgende onderdeel
+        pc.printf("KALIBRATIE_ARM1 afgerond");              //Tekst voor controle Arm 1 naar thuispositie
     }  
 }   
 
@@ -91,11 +91,11 @@
     else {                                          //Anders richting nul
         dir_motor_arm2.write(0);
     }
-    pwm_motor_arm2.write(abs(pwm_to_motor_arm2));   //Output PWM naar motor is de absolute waarde van de berekende PWM
+    pwm_motor_arm2.write(fabs(pwm_to_motor_arm2));   //Output PWM naar motor is de absolute waarde van de berekende PWM
             
     if (pwm_to_motor_arm2 == 0) {                           //Als het verschil tussen de voorgestelde en huidige positie nul is wordt de while loop gestopt (condities kloppen niet meer)
-        kalibratie_positie_arm2 = true;
-        pc.printf("Arm 2 naar thuispositie compleet");     //Tekst voor controle Arm 2 naar thuispositie
+        state = KALIBRATIE_BICEPS;                          //State wordt kalibratie arm 2, zo daar naar volgende onderdeel
+        pc.printf("KALIBRATIE_ARM2 afgerond");              //Tekst voor controle Arm 2 naar thuispositie
     }
 }
 
@@ -104,53 +104,59 @@
     
     //PC baud rate instellen
     pc.baud(38400);             //PC baud rate is 38400 bits/seconde
+   
+    switch(state)               //Switch benoemen, zorgt ervoor dat in de goede volgorde de dingen worden doorlopen, aan einde een case wordt de state de naam van de nieuwe case
+    {
     
-    //Aanzetten
-    if (rust == false && kalibratie_positie_arm1 == false && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){
+    case RUST:                  //Aanzetten
+    {
         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
-        pc.printf("Aanzetten compleet");    //Tekst voor controle Aanzetten
-        rust = true;                        //Rust wordt waar zodat door kan worden gegaan naar het volgende deel
+        pc.printf("RUST afgerond");         //Tekst voor controle Aanzetten
+        state = KALIBRATIE_ARM1;            //State wordt kalibratie arm 1, zo door naar volgende onderdeel
+        break;                              //Stopt acties in deze case
     }
     
-    
-    
-    //Arm 1 naar thuispositie
-    if (rust == true && kalibratie_positie_arm1 == false && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){
+    case KALIBRATIE_ARM1:                   //Arm 1 naar thuispositie
+    {
         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 == false && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false) {
+        while(state == KALIBRATIE_ARM1){
             while(regelaar_ticker_flag != true) ;           //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
             regelaar_ticker_flag = false;                   //Flag weer naar beneden, zodat deze straks weer omhoog kan
             
             arm1_naar_thuispositie();                       //Voer acties uit om arm 1 naar thuispositie te krijgen
-        }   
-        wait(1);                    //Een seconde wachten
+        }
+        wait(1);                                            //Een seconde wachten
+        break;                              //Stopt acties in deze case
     }
     
-    //Arm 2 naar thuispositie
-    if (rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){
+    case KALIBRATIE_ARM2:                    //Arm 2 naar thuispositie
+    {
         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_bi == false && kalibratie_EMG_tri == false) {
+        while(state == KALIBRATIE_ARM2){
             while(regelaar_ticker_flag != true) ;           //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
             regelaar_ticker_flag = false;                   //Flag weer naar beneden, zodat deze straks weer omhoog kan
             
             arm2_naar_thuispositie();                       //Voer acties uit om arm 2 naar thuispositie te krijgen
         }
         wait(1);            //Een seconde wachten
-        ticker_regelaar.detach();       //Ticker detachten, ticker doet nu niks meer
+        ticker_regelaar.detach();           //Ticker detachten, ticker doet nu niks meer
+        break;                              //Stopt acties in deze case
     }
     
-    //Ticker voor kalibratie
-    if (rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == true && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){
+    case KALIBRATIE_BICEPS:                 //Kalibratie EMG signaal biceps
+    {
+        wait(1);                            //Een seconde wachten
+          
+        ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG);     //Ticker iedere zoveel seconden de flag laten opsteken
         
-        ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG);     //Ticker iedere zoveel seconden de flag laten opsteken
         pc.printf("Ticker voor kalibratie compleet");               //Tekst voor controle Ticker voor kalibratie 
         
         //5 seconden EMG biceps meten
@@ -158,7 +164,6 @@
         wait(1);                            //Een seconde wachten
         lcd_r1 = " EMG kalibratie ";        //Tekst op eerste regel van LCD scherm
         lcd_r2 = " Span biceps aan";        //Tekst op tweede regel van LCD scherm
-        wait(2);                            //Twee seconden wachten
         
         for (i=0; i<1000; i++){
             while(regelaar_EMG_flag != true) ;              //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
@@ -170,6 +175,9 @@
     }
         
     
+    } 
+
+    
         
         
 }
\ No newline at end of file