Diff: main.cpp

Revision:

10:cd89569cd847

Parent:

9:7e9b63fe8988

Child:

11:687aa4656a6e

--- a/main.cpp	Sat Nov 01 16:29:46 2014 +0000
+++ b/main.cpp	Mon Nov 03 12:34:33 2014 +0000
@@ -28,7 +28,6 @@
 #define K_I (0.01)
 #define K_D (0.01)
 #define I_LIMIT 100.
-#define PI 3.1415926535897
 #define lengte_arm 0.5
 
 /*
@@ -63,7 +62,7 @@
 int current_pos_motor1;
 int current_pos_motor2; 
 int EMG = 1;
-int aantal_pieken;
+int aantal_pieken = 0;
 int doel;
 int doel_richting;
 int doel_hoogte;
@@ -112,7 +111,8 @@
     {
         aanspan=false;
         aantal_pieken++;
-        doel = aantal_pieken-((aantal_pieken/3)*3)+1;
+        doel = aantal_pieken-(((aantal_pieken-1)/3)*3); //aantal_pieken-((aantal_pieken/3)*3)+1;
+        
     }//if
 }//void pieken_tellen
 
@@ -127,13 +127,15 @@
     arm_biquad_cascade_df1_f32(&notch, &emg_filtered, &emg_filtered, 1);
     emg_filtered = fabs(emg_filtered);
     arm_biquad_cascade_df1_f32(&lowpass_2, &emg_filtered, &emg_filtered, 1 );
-    scope.set(0,emg_value);     //uint value
-    scope.set(1,emg_filtered);  //processed float
+    //scope.set(0,emg_value);     //uint value
+    //scope.set(1,emg_filtered);  //processed float
     if(state!=EMG_KALIBRATIE)
     {
         pieken_tellen();
     }//if
-    scope.set(2,doel);
+    //pc.printf("%d\n\r",doel);
+    scope.set(0, doel); 
+    scope.set(1, aantal_pieken);
     scope.send();
 }//void emg_filtering()
 
@@ -144,7 +146,7 @@
         emg_max=emg_filtered;
     }//if
     emg_treshhold_laag = 0.3*emg_max;
-    emg_treshhold_hoog = 0.7*emg_max;
+    emg_treshhold_hoog = 0.6*emg_max;
 }//void emg_max_meting
 
 void akkoord_geven(){
@@ -171,18 +173,21 @@
 }//void arm_kalibratie
 
 void doel_bepaling() {
-    if(200<=current_herhalingen<1200){
+    if(200<=current_herhalingen && current_herhalingen <1200){
         emg_filtering();
+        doel = aantal_pieken-(((aantal_pieken-1)/3)*3);
     }//if
     else if(current_herhalingen == 1200 && state==METEN_HOOGTE){
         doel_hoogte = doel;
         aantal_pieken = 0;
+        doel = 0; 
     }
     else if(current_herhalingen == 1200 && state==METEN_RICHTING){
         doel_richting = doel;
         aantal_pieken = 0;//op 0 omdat bij akkoord geven dit ook gebruikt wordt.
+        doel = 0;
     }
-    else if(1200<current_herhalingen<=2200){
+    else if(1200<current_herhalingen && current_herhalingen<=2200){
         akkoord_geven();        
     }//else if
     else{
@@ -206,16 +211,16 @@
         motordir2 = 1;
         while (current_pos_motor2 > 0){
             pwm_motor2.write(PWM_richting1);
-            current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1;
         }//while (current_pos_motor2 < rad_richting1)
+        current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1;
     }//if (doel_richting == 1)
     else{
     motordir2 = 0;
         while (current_pos_motor2 < puls_richting1){
             pwm_motor2.write(PWM_richting1);
-            current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1;
         }//while (current_pos_motor2 < rad_richting1)
-    }//if (doel_richting == 1)
+        current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1;
+    }//else
 }//if (doel_richting ==1)
 
 
@@ -224,15 +229,15 @@
         motordir2 = 1;
         while (current_pos_motor2 > 0){
             pwm_motor2.write(PWM_richting2);
-            current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1;
         }//while (current_pos_motor2 < rad_richting1)
+        current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1;
     }//if (doel_richting == 1)
     else{
         motordir2 = 0;
         while (current_pos_motor2 < puls_richting2){
             pwm_motor2.write(PWM_richting2);
-            current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1;
         }//while (current_pos_motor2 < rad_richting1)
+        current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1;
     }//if (doel_richting == 1)
 }//if (doel_richting ==1)
 
@@ -241,15 +246,15 @@
         motordir2 =1;
         while (current_pos_motor2 > 0){
             pwm_motor2.write(PWM_richting3);
-            current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1;
         }//while (current_pos_motor2 < rad_richting1)
+        current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1;
     }//if (doel_richting == 1)
     else{
         motordir2 = 0;
         while (current_pos_motor2 < puls_richting3){
             pwm_motor2.write(PWM_richting3);
-            current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1;
         }//while (current_pos_motor2 < rad_richting1)
+        current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1;
     }//if (doel_richting == 1)
 }//if (doel_richting ==1)
 }//void instellen_richting
@@ -378,13 +383,13 @@
         case METEN_HOOGTE: 
         {
             meten_hoogte();
-            if (1200 < current_herhalingen <2200 && aantal_pieken == 1) 
+            if (1200 < current_herhalingen && current_herhalingen <2200 && aantal_pieken == 1 && doel_hoogte>=1 ) 
             {
                 current_herhalingen = 0;
                 previous_herhalingen = 0;
                 aantal_pieken = 0;
                 doel = 0;
-                doel_hoogte = 0; 
+                //doel_hoogte = 0; 
                 state = METEN_RICHTING;
             }//if (current_herhalingen == 2800 && aantal_pieken == 1) 
             else if (current_herhalingen == 2200)
@@ -401,7 +406,7 @@
         case METEN_RICHTING: 
         {
             meten_richting();
-            if (1200 < current_herhalingen <2200 && aantal_pieken == 1) 
+            if (1200 < current_herhalingen && current_herhalingen <2200 && aantal_pieken == 1 && doel_richting>=1 ) 
             {
                 current_herhalingen = 0;
                 previous_herhalingen = 0;
@@ -523,7 +528,7 @@
             lcd.locate(0,1);
             lcd.printf("span aan per vak");
         }//if(current_herhalingen<=200){
-        else if(200<=current_herhalingen<1200){
+        else if(200<=current_herhalingen && current_herhalingen<1200){
             lcd.locate(0,0);
             lcd.printf("Vak %d",doel);
             if(current_herhalingen<=400){
@@ -569,7 +574,7 @@
             lcd.locate(0,1);
             lcd.printf("span aan per vak");
         }//if(current_herhalingen<=200)
-        else if(200<=current_herhalingen<1200){
+        else if(200<=current_herhalingen && current_herhalingen<1200){
             lcd.locate(0,0);
             lcd.printf("Vak %d",doel);
             if(current_herhalingen<=400){
@@ -607,6 +612,30 @@
         }//else
     }//else if(state==METEN_RICHTING){
         
+    else if(state==INSTELLEN_RICHTING){
+        lcd.cls();
+        lcd.locate(0,0);
+        lcd.printf("Instellen hoek");
+        lcd.locate(0,1);
+        lcd.printf("Even geduld...");
+    }//else if(state==INSTELLEN_RICHTING){
+        
+    else if(state==SLAAN){
+        lcd.cls();
+        lcd.locate(0,0);
+        lcd.printf("Slaan, pas op");
+        lcd.locate(0,1);
+        lcd.printf("Let's pray");
+    }//else if(state==INSTELLEN_RICHTING){
+        
+    else if(state==RETURN2RUST){
+        lcd.cls();
+        lcd.locate(0,0);
+        lcd.printf("Terug naar");
+        lcd.locate(0,1);
+        lcd.printf("0-positie");
+    }//else if(state==INSTELLEN_RICHTING){
+        
     else{     
         lcd.cls();
         lcd.printf("state %d", state); //hier nog aan toevoegen hoe je de 'waarde', dus eigenlijk tekst, die opgeslagen staat in state kan printen.