Diff: main.cpp

Revision:

7:d7aafc5b9efc

Parent:

6:464d2fdfd7de

--- a/main.cpp	Thu Apr 11 12:32:20 2019 +0000
+++ b/main.cpp	Tue Apr 16 13:33:37 2019 +0000
@@ -8,8 +8,8 @@
 // Algemeen
 DigitalIn button3(SW3);
 DigitalIn button2(SW2);
-AnalogIn But2(A5);
-AnalogIn But1(A3);
+DigitalIn But2(D12);
+DigitalIn But1(D13);
 
 DigitalOut led1(LED_GREEN);
 DigitalOut led2(LED_RED);
@@ -35,11 +35,11 @@
 double motor_position1;
 double Pulses2;
 double motor_position2;
-double error1;
+volatile double error1;
 double u1;
 
 //Pot meter
-AnalogIn pot(A1);
+AnalogIn pot(A5);
 AnalogIn pot0(A0);
 float Pot2;
 float Pot1;
@@ -68,7 +68,6 @@
 float Wijsvinger_krom = 0.15;
 float Wijsvinger_recht = 0.93;
 
-
 // EMG
 float EMG1;       // Rotatie
 float EMG2;       // Elleboog
@@ -113,6 +112,237 @@
 double Kd2 = 1;
 double Ts = 0.0005;      // Sample time in seconds
 
+// EMG waardes
+// Tickers
+
+Ticker sample_ticker; //ticker voor filteren met 1000Hz
+Ticker sample_timer;
+Ticker threshold_check_ticker; //ticker voor het checken van de threshold met 1000Hz
+Timer timer_calibration; //timer voor EMG Kalibratie
+double ts = 0.001; //tijdsstap !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+double calibration_time = 37; //Kalibratie tijd
+
+//Input
+AnalogIn    emg1( A1 ); //Duim
+AnalogIn    emg2( A2 ); //Bicep
+AnalogIn    emg3( A3 ); //Dorsaal
+AnalogIn    emg4( A4 ); //Palmair
+
+// GLOBALS EMG
+//Gefilterde EMG signalen
+volatile double emg1_filtered, emg2_filtered, emg3_filtered, emg4_filtered;
+
+bool thresholdreach1 = false;
+bool thresholdreach2 = false;
+bool thresholdreach3 = false;
+bool thresholdreach4 = false;
+
+volatile double temp_highest_emg1 = 0; //Hoogste waarde gevonden tijdens kalibratie
+volatile double temp_highest_emg2 = 0;
+volatile double temp_highest_emg3 = 0;
+volatile double temp_highest_emg4 = 0;
+
+//Percentage van de hoogste waarde waar de bovenste treshold gezet moet worden
+double Duim_p_t = 0.5; 
+double Bicep_p_t = 0.4; 
+double Dorsaal_p_t = 0.6; 
+double Palmair_p_t = 0.5;
+
+//Percentage van de hoogste waarde waar de onderste treshold gezet moet worden
+double Duim_p_tL = 0.5; 
+double Bicep_p_tL = 0.4; 
+double Dorsaal_p_tL = 0.5; 
+double Palmair_p_tL = 0.5;
+
+// Waarde bovenste treshold waar het signaal overheen moet om de arm te activeren
+volatile double threshold1;
+volatile double threshold2;
+volatile double threshold3;
+volatile double threshold4;
+
+// Waarde onderste treshold waar het signaal onder moet om de arm te deactiveren
+volatile double threshold1L;
+volatile double threshold2L;
+volatile double threshold3L;
+volatile double threshold4L;
+
+// thresholdreads bools
+int Duim;
+int Bicep;
+int Dorsaal;
+int Palmair;
+
+// Biquad filters
+//EMG1!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+//Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren
+BiQuadChain highp1;
+BiQuad highp1_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 );
+BiQuad highp1_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 );
+BiQuad notch1_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 );
+
+//Lowpass first order cutoff 0.4Hz
+BiQuad lowp1( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 );
+
+//EMG2!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+//Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren
+BiQuadChain highp2;
+BiQuad highp2_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 );
+BiQuad highp2_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 );
+BiQuad notch2_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 );
+
+//Lowpass first order cutoff 0.4Hz
+BiQuad lowp2( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 );
+
+//EMG3!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+//Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren
+BiQuadChain highp3;
+BiQuad highp3_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 );
+BiQuad highp3_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 );
+BiQuad notch3_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 );
+
+//Lowpass first order cutoff 0.4Hz
+BiQuad lowp3( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 );
+
+//EMG4!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+//Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren
+BiQuadChain highp4;
+BiQuad highp4_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 );
+BiQuad highp4_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 );
+BiQuad notch4_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 );
+
+//Lowpass first order cutoff 0.4Hz
+BiQuad lowp4( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 );
+
+
+// ~~~~~~~~~~~~~~~~~~~EMG FUNCTIONS~~~~~~~~~~~~~~~~~~
+void emgsample()
+{
+    
+    // EMG signaal lezen
+    double emgread1 = emg1.read();
+    double emgread2 = emg2.read();
+    double emgread3 = emg3.read();
+    double emgread4 = emg4.read();
+    
+    // Vierde orde highpass filter + notch filter
+    double emg1_highpassed = highp1.step(emgread1);
+    double emg2_highpassed = highp2.step(emgread2);
+    double emg3_highpassed = highp3.step(emgread3);
+    double emg4_highpassed = highp4.step(emgread4);
+
+    //Rectificatie
+    double emg1_abs = abs(emg1_highpassed);
+    double emg2_abs = abs(emg2_highpassed);
+    double emg3_abs = abs(emg3_highpassed);
+    double emg4_abs = abs(emg4_highpassed);
+
+    //All EMG abs into lowpass
+    emg1_filtered = lowp1.step(emg1_abs);
+    emg2_filtered = lowp2.step(emg2_abs);
+    emg3_filtered = lowp3.step(emg3_abs);
+    emg4_filtered = lowp4.step(emg4_abs);
+
+}
+
+void threshold_check()
+{
+
+// EMG1 Check    
+if (thresholdreach1 == false){            //Als emg_filtered nog niet boven de bovenste threshold is geweest
+    //bovenste threshold check
+    if(emg1_filtered>threshold1) {
+        Duim = 1;
+        thresholdreach1 = true;
+        
+    } else {
+        Duim= 0;
+    }
+    }
+else{                                   //Als emg_filtered boven de bovenste threshold is geweest
+    //onderste threshold check
+    if(emg1_filtered<threshold1L) {
+        Duim = 0;
+        thresholdreach1 = false;
+        
+    } else {
+        Duim= 1;
+    }
+    }
+
+// EMG2 Check    
+if (thresholdreach2 == false){            //Als emg_filtered nog niet boven de bovenste threshold is geweest
+    //bovenste threshold check
+    if(emg2_filtered>threshold2) {
+        Bicep = 1;
+        thresholdreach2 = true;
+    } else {
+        Bicep= 0;
+    }
+    }
+else{                                   //Als emg_filtered boven de bovenste threshold is geweest
+    //onderste threshold check
+    if(emg2_filtered<threshold2L) {
+        Bicep = 0;
+        thresholdreach2 = false;
+        
+    } else {
+        Bicep= 1;
+    }
+    }
+
+// EMG3 Check    
+if (thresholdreach3 == false){            //Als emg_filtered nog niet boven de bovenste threshold is geweest
+    //bovenste threshold check
+    if(emg3_filtered>threshold3) {
+        Dorsaal = 1;
+        thresholdreach3 = true;
+    } else {
+        Dorsaal= 0;
+    }
+    }
+else{                                   //Als emg_filtered boven de bovenste threshold is geweest
+    //onderste threshold check
+    if(emg3_filtered<threshold3L) {
+        Dorsaal = 0;
+        thresholdreach3 = false;
+        
+    } else {
+        Dorsaal= 1;
+    }
+    }
+
+// EMG4 Check    
+if (thresholdreach4 == false){            //Als emg_filtered nog niet boven de bovenste threshold is geweest
+    //bovenste threshold check
+    if(emg4_filtered>threshold4) {
+        Palmair = 1;
+        thresholdreach4 = true;
+    } else {
+        Palmair= 0;
+    }
+    }
+else{                                   //Als emg_filtered boven de bovenste threshold is geweest
+    //onderste threshold check
+    if(emg4_filtered<threshold4L) {
+        Palmair = 0;
+        thresholdreach4 = false;
+        
+    } else {
+        Palmair= 1;
+    }
+    }
+
+    }
+    
+    void sample()
+{
+pc.printf("Duim Right = %i\r\n", Duim);
+pc.printf("Bicep Right = %i\r\n",Bicep);
+pc.printf("Dorsaal Left = %i\r\n", Dorsaal);
+pc.printf("Palmair Left = %i\r\n", Palmair);           
+wait(0.001);
+}
+
 // Functies Kinematica
 float Kinematics1(float EMG1)
 {
@@ -287,6 +517,10 @@
 
     double error1 = PolsReference - motor_position1;
     double u1 = PID_controller1(error1);
+    if (error1 <= 3 && error1 >= -3){
+        error1 = 0;
+        }
+    //printf("error = %f\n\r",error1);
     moter1_control(u1);
 
     double error2 = ElbowReference - motor_position2;
@@ -380,22 +614,62 @@
 
         case CALIBRATION:
             // Actions
-            if (stateChanged) {
-                // state initialization: oranje
                 led1 = 0;
                 led2 = 0;
                 led3 = 1;
+            sample_ticker.attach(&emgsample, 0.001);            // Leest het ruwe EMG signaal af met een frequentie van 1000Hz
+            if (stateChanged) {
+                timer_calibration.reset();
+                timer_calibration.start();
+                while(timer_calibration<5) {                           //Duim
+                    if(timer_calibration>0 && timer_calibration<5) {
+                        if(emg1_filtered>temp_highest_emg1) {
+                            temp_highest_emg1= emg1_filtered;
 
-                wait(1);
-                stateChanged = false;
+                        }
+
+                        if(emg2_filtered>temp_highest_emg2) {
+                            temp_highest_emg2= emg2_filtered;
+
+                        }
 
-            }
+                        if(emg3_filtered>temp_highest_emg3) {
+                            temp_highest_emg3= emg3_filtered;
+
+                        }
+
+                        if(emg4_filtered>temp_highest_emg4) {
+                            temp_highest_emg4= emg4_filtered;
+
+                        }
 
-            // State transition logic: automatisch terug naar motors off.
+                    }
+
+                }
+                
+                threshold1 = temp_highest_emg1*Duim_p_t;
+                threshold2 = temp_highest_emg2*Bicep_p_t;
+                threshold3 = temp_highest_emg3*Dorsaal_p_t;
+                threshold4 = temp_highest_emg4*Palmair_p_t;
 
-            currentState = HOMING1;
-            stateChanged = true;
-            break;
+                threshold1L = temp_highest_emg1*Duim_p_tL;
+                threshold2L = temp_highest_emg2*Bicep_p_tL;
+                threshold3L = temp_highest_emg3*Dorsaal_p_tL;
+                threshold4L = temp_highest_emg4*Palmair_p_tL;
+            }
+            pc.printf("Highest value Duim= %f \r\n", temp_highest_emg1);
+            pc.printf("Highest value Bicep= %f \r\n", temp_highest_emg2);
+            pc.printf("Highest value Dorsaal= %f \r\n", temp_highest_emg3);
+            pc.printf("Highest value Palmair= %f \r\n", temp_highest_emg4);
+            sample_ticker.detach();
+            stateChanged = false;
+            
+
+    // State transition logic: automatisch terug naar motors off.
+
+    currentState = HOMING1;
+    stateChanged = true;
+    break;
 
         case HOMING1:
             // Actions
@@ -529,7 +803,8 @@
 
         case MOVEMENT:
             // Actions
-
+//threshold_check_ticker.attach(&threshold_check, 0.01);
+//sample_timer.attach(&sample, 0.5);
             if (stateChanged) {
                 // state initialization: purple
                 t.start();          // na 5 minuten terug naar Homing
@@ -642,6 +917,13 @@
     pwmpin1.period_us(60);
     //PotRead.attach(ContinuousReader,Ts);
     pc.baud(115200);
+    
+    
+    //BiQuad Chain add
+    highp1.add( &highp1_1 ).add( &highp1_2 ).add( &notch1_1 );
+    highp2.add( &highp2_1 ).add( &highp2_2 ).add( &notch2_1 );
+    highp3.add( &highp3_1 ).add( &highp3_2 ).add( &notch3_1 );
+    highp4.add( &highp4_1 ).add( &highp4_2 ).add( &notch4_1 );
 
     while(true) {
         led1 = 1;
@@ -656,6 +938,9 @@
             }
         counter++;
         */
+        
+
+        
         ProcessStateMachine();