Revision 9:7e9b63fe8988, committed 2014-11-01

Comitter:

lauradeheus

Date:

Sat Nov 01 16:29:46 2014 +0000

Parent:

8:f733c6a27c15

Child:

10:cd89569cd847

Commit message:

Ik heb het script zo aangepast dat alles het (ZOU) moeten doen. Niets is getest. Het script voor de LCD moet nog toegevoegd worden.

Changed in this revision

--- a/main.cpp	Sat Nov 01 13:20:22 2014 +0000
+++ b/main.cpp	Sat Nov 01 16:29:46 2014 +0000
@@ -57,13 +57,19 @@
 arm_biquad_casd_df1_inst_f32 notch; //2e orde lowpass biquad butterworthfilter 50Hz
 int previous_herhalingen = 0;
 int current_herhalingen = 0;
+int current_herhalingen_1 = 0; 
+int previous_herhalingen_1 = 0; 
 int previous_pos_motor1 = 0;
 int current_pos_motor1;
+int current_pos_motor2; 
 int EMG = 1;
 int aantal_pieken;
 int doel;
 int doel_richting;
 int doel_hoogte;
+int puls_richting1;
+int puls_richting2;
+int puls_richting3;
 bool aanspan;
 void clamp(float * in, float min, float max);
 float pid(float setpoint, float measurement);
@@ -85,6 +91,10 @@
 float emg_max = 0;
 float emg_treshhold_laag = 0;
 float emg_treshhold_hoog = 0;
+float marge = 0; 
+float PWM_richting1;
+float PWM_richting2;
+float PWM_richting3;
 
 enum  state_t {RUST, EMG_KALIBRATIE, ARM_KALIBRATIE, METEN_HOOGTE, METEN_RICHTING, INSTELLEN_RICHTING, SLAAN, RETURN2RUST}; //verschillende stadia definieren voor gebruik in CASES
 state_t state = RUST;
@@ -119,12 +129,12 @@
     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(2,doel);
-    scope.send();
     if(state!=EMG_KALIBRATIE)
     {
         pieken_tellen();
     }//if
+    scope.set(2,doel);
+    scope.send();
 }//void emg_filtering()
 
 void emg_max_meting(){
@@ -137,6 +147,10 @@
     emg_treshhold_hoog = 0.7*emg_max;
 }//void emg_max_meting
 
+void akkoord_geven(){
+    emg_filtering();
+}
+
 void emg_kalibratie() {
     //if(emg_filtered>=0.05){//Deze if-loop alleen zodat het  nog op de hidscope kan worden gezien, dit mag weg wanneer er een display is, current_herhalingen wel laten.
         current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
@@ -149,17 +163,27 @@
 
 void arm_kalibratie() {
     //voor nu om de loop te doorlopen wordt onderstaande code gebruikt. Nogmaal gesproken moet er gewacht worden op een 'hoog' signaal van een knop
-    current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
     motor1.setPosition(0);
-    motor2.setPosition(0);    
+    motor2.setPosition(0);  
+    pwm_motor1.period_us(100);
+    pwm_motor2.period_us(100);
+    akkoord_geven();  
 }//void arm_kalibratie
 
 void doel_bepaling() {
-    if(200<=current_herhalingen<=1200){
+    if(200<=current_herhalingen<1200){
         emg_filtering();
     }//if
-    else if(1600<=current_herhalingen<=2200){
-        emg_filtering();        
+    else if(current_herhalingen == 1200 && state==METEN_HOOGTE){
+        doel_hoogte = doel;
+        aantal_pieken = 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.
+    }
+    else if(1200<current_herhalingen<=2200){
+        akkoord_geven();        
     }//else if
     else{
     }//else    
@@ -168,33 +192,76 @@
 void meten_hoogte() {
     current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
     doel_bepaling();
-    if(1600<=current_herhalingen<=2200 && aantal_pieken==1){
-        doel=doel_hoogte;
-    }//if
 }//void meten_hoogte
 
 void meten_richting() {
     current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
     doel_bepaling();
-    if(1600<=current_herhalingen<=2200 && aantal_pieken==1){
-        doel=doel_richting;
-    }//if
 }//void meten_richting
 
 void instellen_richting() {
-    current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
+    current_pos_motor2 = motor2.getPosition();
+if (doel_richting ==1){
+    if (state==RETURN2RUST){
+        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)
+    }//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)
+}//if (doel_richting ==1)
+
+
+else if (doel_richting == 2){
+    if (state==RETURN2RUST){
+        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)
+    }//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)
+    }//if (doel_richting == 1)
+}//if (doel_richting ==1)
+
+else if(doel_richting == 3){
+    if (state==RETURN2RUST){
+        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)
+    }//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)
+    }//if (doel_richting == 1)
+}//if (doel_richting ==1)
 }//void instellen_richting
 
-void GotoPosition (float position_setpoint_rad, float speed_radpersecond) {
+void GotoPosition (float position_setpoint_rad, float speed_radpersecond, float marge){
     
     current_pos_motor1 = motor1.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is
     pos_motor1_rad = current_pos_motor1/(1600/(2*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaar
-    
     previous_pos_motor1 = current_pos_motor1;  //sla de huidige waarde op als vorige waarde.
     
-    //nu gaan we positie regelen i.p.v. snelheid.
-    
-    if (fabs(pos_motor1_rad - position_setpoint_rad) <= 0.05) //if position error < ...rad, then stop.
+    //nu gaan we snelheid volgen d.m.v. positie regeling
+    if (fabs(pos_motor1_rad - position_setpoint_rad) <= marge) //if position error < ...rad, then stop.
     {
         speed_radpersecond = 0; 
         setpoint = pos_motor1_rad;
@@ -233,7 +300,7 @@
         motordir1 = 0;
     }//else
         
-    pwm_motor1.write(abs(PWM1_percentage/100.))
+    pwm_motor1.write(abs(PWM1_percentage/100.));
     prev_setpoint = setpoint;
 }//void GotoPosition
 
@@ -268,60 +335,65 @@
             {
                 current_herhalingen = 0;
                 previous_herhalingen = 0;
-                state = ARM_KALIBRATIE;
-                EMG = 0;
+                state = EMG_KALIBRATIE;
+                EMG = 0; //door EMG op 0 te zetten word deze loop nooit meer doorlopen, daarna zal altijd else worden uitgevoerd. Wat ook gelijk het kalibreren van de arm overslaat. Men kan na 1 keer kalibreren dus vaker achter elkaar schieten
             }//if (current_herhalingen == 100 && EMG == 1) 
+            else if(current_herhalingen == 100)
+            {
+                current_herhalingen = 0; 
+                previous_herhalingen = 0; 
+                state = METEN_HOOGTE; 
+            }//else
             break;
         }//case RUST: 
 
-        case ARM_KALIBRATIE: 
-        {
-            arm_kalibratie();
-            if (current_herhalingen == 100) 
-            {
-                current_herhalingen = 0;
-                previous_herhalingen = 0;
-                state = EMG_KALIBRATIE;
-                motor1.setPosition(0);
-                motor2.setPosition(0);
-                pwm_motor1.period_us(100);
-                pwm_motor2.period_us(100);
-            }//if (current_herhalingen == 100) 
-            break;
-        }//case ARM_KALIBRATIE:
-
         case EMG_KALIBRATIE: 
         {
             emg_kalibratie();
-            if (current_herhalingen >=1000) 
+            if (current_herhalingen >=1000) /*waarom >= en niet ==?*/
             {
-                state = METEN_HOOGTE;
                 current_herhalingen = 0;
                 previous_herhalingen = 0;
                 aantal_pieken = 0;
                 doel = 0;
+                state = ARM_KALIBRATIE;
             }//if (current_herhalingen >=1000) 
             break;
         }//case EMG_KALIBRATIE
+        
+        case ARM_KALIBRATIE: 
+        {
+            arm_kalibratie();
+            if (aantal_pieken == 1) 
+            {
+                current_herhalingen = 0;
+                previous_herhalingen = 0;
+                aantal_pieken = 0; 
+                doel = 0; 
+                state = METEN_HOOGTE;
+            }//if (current_herhalingen == 100) 
+            break;
+        }//case ARM_KALIBRATIE:
 
         case METEN_HOOGTE: 
         {
             meten_hoogte();
-            if (current_herhalingen == 2800 && aantal_pieken == 1) 
+            if (1200 < current_herhalingen <2200 && aantal_pieken == 1) 
             {
-                state = METEN_RICHTING;
                 current_herhalingen = 0;
                 previous_herhalingen = 0;
                 aantal_pieken = 0;
                 doel = 0;
+                doel_hoogte = 0; 
+                state = METEN_RICHTING;
             }//if (current_herhalingen == 2800 && aantal_pieken == 1) 
-            else
+            else if (current_herhalingen == 2200)
             {
-                state = METEN_HOOGTE;
                 current_herhalingen = 0;
                 previous_herhalingen = 0;
                 aantal_pieken = 0;
-                doel = 0;
+                doel = 0; 
+                state = METEN_HOOGTE;
             }///else
             break;
         }//case METEN_HOOGTE
@@ -329,22 +401,22 @@
         case METEN_RICHTING: 
         {
             meten_richting();
-            if (current_herhalingen == 2800 && aantal_pieken == 1) 
+            if (1200 < current_herhalingen <2200 && aantal_pieken == 1) 
             {
-                state = INSTELLEN_RICHTING;
                 current_herhalingen = 0;
                 previous_herhalingen = 0;
                 aantal_pieken = 0;
                 doel = 0;
+                state = INSTELLEN_RICHTING;
             }//if (current_herhalingen == 2800 && aantal_pieken == 1) 
-            else
+            else if (current_herhalingen == 2200)
             {
-                state = METEN_RICHTING;
                 current_herhalingen = 0;
                 previous_herhalingen = 0;
                 aantal_pieken = 0;
-                doel = 0;
-            }//else
+                doel = 0; 
+                state = METEN_RICHTING;
+            }///else
             break;
         }//case METEN_RICHTING
 
@@ -353,9 +425,10 @@
             instellen_richting();
             if (current_herhalingen == 100) 
             {
+                current_herhalingen_1 = 0;
+                previous_herhalingen_1 = 0;
+                doel_richting = 0;
                 state = SLAAN;
-                current_herhalingen = 0;
-                previous_herhalingen = 0;
             }//if (current_herhalingen == 100)
         break;  
         }//case INSTELLEN_RICHTING
@@ -363,26 +436,31 @@
         case SLAAN: 
         {
             GotoPosition(1.5 ,8, 0.1);
-            if (current_herhalingen == 100) 
+            if (current_herhalingen == 400) 
             {
-                state = RETURN2RUST;
                 current_herhalingen = 0;
                 previous_herhalingen = 0;
                 prev_setpoint =0; 
                 setpoint =0;
+                state = RETURN2RUST;
             }//if (current_herhalingen == 100) 
         break;    
         }//case SLAAN
 
         case RETURN2RUST: 
         {
+            instellen_richting();
             GotoPosition(0,4, 0.05);
-            //if (current_herhalingen == 100) 
-            //{
-            //    state = RUST;
-            //    current_herhalingen = 0;
-            //    previous_herhalingen = 0;
-            //}//if (current_herhalingen == 100) 
+            doel_richting = 0;
+            doel_hoogte = 0; 
+            if (current_herhalingen >= 200 && current_herhalingen_1 >= 200) 
+            {
+                state = RUST;
+                current_herhalingen = 0;
+                previous_herhalingen = 0;
+                current_herhalingen = 0; 
+                current_herhalingen = 0;
+            }//if (current_herhalingen == 100) 
             break;
         }// case RETURN2RUST
         
@@ -429,6 +507,14 @@
         }//else if(current_herhalingen<=1000)
     }//else if(state==EMG_KALIBRATIE)
     
+    else if(state==ARM_KALIBRATIE){
+        lcd.cls();
+        lcd.locate(0,0);
+        lcd.printf("Set arm to zero");
+        lcd.locate(0,1); 
+        lcd.printf("Klaar? Span aan");
+    }//else if(state==ARM_KALIBRATIE)
+    
     else if(state==METEN_HOOGTE){
         lcd.cls();
         if(current_herhalingen<=200){
@@ -437,7 +523,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<1200){
             lcd.locate(0,0);
             lcd.printf("Vak %d",doel);
             if(current_herhalingen<=400){
@@ -456,21 +542,17 @@
                 lcd.locate(0,1); 
                 lcd.printf("nog 2 sec.");
             }//else if(current_herhalingen<=1000)
-            else if(current_herhalingen<=1200){
+            else if(current_herhalingen<1200){
                 lcd.locate(0,1); 
                 lcd.printf("nog 1 sec.");
             }//else if(current_herhalingen<=1200)
         }//else if(200<=current_herhalingen<=1200)
-        else if(current_herhalingen<=1600){
+        else if(current_herhalingen<=2200){
             lcd.locate(0,0);
-            lcd.printf("Vak %d akkoord?",doel);
+            lcd.printf("Vak %d akkoord?",doel_hoogte);
             lcd.locate(0,1);
             lcd.printf("Span aan");
         }//else if(current_herhalingen<=1600){
-        else if(1600<=current_herhalingen<=2800 && aantal_pieken==1){
-            lcd.locate(0,0);
-            lcd.printf("Vak %d akkoord",doel_hoogte);   
-        }//else if(1600<=current_herhalingen<=2800 && aantal_pieken==1){
         else{
             lcd.locate(0,0);
             lcd.printf("Opnieuw hoogte");
@@ -487,7 +569,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<1200){
             lcd.locate(0,0);
             lcd.printf("Vak %d",doel);
             if(current_herhalingen<=400){
@@ -506,21 +588,17 @@
                 lcd.locate(0,1); 
                 lcd.printf("nog 2 sec.");
             }//else if(current_herhalingen<=1000)
-            else if(current_herhalingen<=1200){
+            else if(current_herhalingen<1200){
                 lcd.locate(0,1); 
                 lcd.printf("nog 1 sec.");
             }//else if(current_herhalingen<=1200)
         }//else if(200<=current_herhalingen<=1200)
-        else if(current_herhalingen<=1600){
+        else if(current_herhalingen<=2200){
             lcd.locate(0,0);
-            lcd.printf("Vak %d akkoord?",doel);
+            lcd.printf("Vak %d akkoord?",doel_richting);
             lcd.locate(0,1);
             lcd.printf("Span aan");
         }//else if(current_herhalingen<=1600)
-        else if(1600<=current_herhalingen<=2800 && aantal_pieken==1){
-            lcd.locate(0,0);
-            lcd.printf("Vak %d akkoord",doel_richting);   
-        }//else if(1600<=current_herhalingen<=2800 && aantal_pieken==1)
         else{
             lcd.locate(0,0);
             lcd.printf("Opnieuw richting");