Diff: main.cpp

Revision:

24:ea6de9237cff

Parent:

23:3902ee714281

Child:

25:553b0ca967fc

--- a/main.cpp	Tue Oct 25 13:51:57 2016 +0000
+++ b/main.cpp	Tue Oct 25 18:15:05 2016 +0000
@@ -8,9 +8,10 @@
 #include "QEI.h"
 #include "math.h"
 #include "HIDScope.h"
+#include "BiQuad.h"
 
 // Definieren van de HIDscope ----------------------------------------
-HIDScope scope(3);               
+HIDScope scope(5);               
 
 
 // Definieren van de Serial en Encoder -------------------------------
@@ -43,12 +44,35 @@
 DigitalOut ledgreen(LED_GREEN, 1);
 DigitalOut ledblue(LED_BLUE, 1);
 
+AnalogIn    emg0( A0 );         //  right biceps
+AnalogIn    emg1( A1 );         //  left biceps
+
 
 //Definieren van de tickers ------------------------------------------
 Ticker motor_ticker;                        // Deze ticker activeert het motor_runs programma, dit leest motor pos en ref pos uit, rekent de PID uit en stuurt met dit de motoren
 Ticker hidscope_ticker;
 
 
+// Het definieren van de filters ----------------------------------------
+BiQuadChain bqc1;       //  Combined filter: high-pass (10Hz) and notch (50Hz)
+BiQuad bq1( 9.21171e-01, -1.84234e+00, 9.21171e-01, -1.88661e+00, 8.90340e-01 );
+BiQuad bq2( 1.00000e+00, -2.00000e+00, 1.00000e+00, -1.94922e+00, 9.53070e-01 );
+BiQuad bq3( 9.93756e-01, -1.89024e+00, 9.93756e-01, -1.89024e+00, 9.87512e-01 );
+
+BiQuadChain bqc2;
+BiQuad bq4 = bq1;
+BiQuad bq5 = bq2;
+BiQuad bq6 = bq3;
+
+BiQuadChain bqc3;       //  Low-pass filter (5Hz)
+BiQuad bq7( 5.84514e-08, 1.16903e-07, 5.84514e-08, -1.94264e+00, 9.43597e-01 );
+BiQuad bq8( 1.00000e+00, 2.00000e+00, 1.00000e+00, -1.97527e+00, 9.76245e-01 );
+
+BiQuadChain bqc4;
+BiQuad bq9 = bq7;
+BiQuad bq10 = bq9;
+
+
 
 // HET VASTSTELLEN VAN ALLE TE GEBRUIKEN VARIABELEN EN CONSTANTEN ======================================================
 
@@ -108,6 +132,17 @@
 enum which_motor{motor1, motor2};               // enum van de motoren
 which_motor motor_that_runs = motor1;
 
+int  digital0;               //  Define digital signals fot EMG
+int  digital1;
+
+double  threshold0 = 0.3;       //  Define thresholds (signal is normalized, so 0.3 = 30% of maximum value) fot EMG
+double  threshold1 = 0.3;
+
+double max_out0 = 0;            //  Set initial maximum values of the signals to 0, the code will check if there's a new maximum value every iteration
+double max_out1 = 0;
+
+int final_signal;               // Dit is waarmee de reference position gezocht gaat worden.
+
 
 
 
@@ -141,7 +176,6 @@
 }
 
 
-
 // Functie voor het vinden van de positie van motor 1 ---------------------
 double get_position_m1(const double radpercount){          //returned de positie van de motor in radialen (positie vd arm in radialen)
     counts1 = encoder_motor1.getPulses();           // Leest aantal pulses uit encoder af
@@ -160,10 +194,54 @@
 }
 
 
+// Functie voor het uitlezen van het EMG signaal ------------------------------------------
+int get_EMG()
+{
+    double in0 = emg0.read();                   //  EMG signal 0 (right biceps)
+    double in1 = emg1.read();                   //  EMG signal 1 (left biceps)
+    
+    double filtered0 = bqc1.step(in0);          //  Filter signals   (high pass and notch)
+    double filtered1 = bqc2.step(in1);
+    double abs_filtered0 = abs(filtered0);      //  Absolute value of filtered signals
+    double abs_filtered1 = abs(filtered1);
+    double out0 = bqc3.step(abs_filtered0);     //  Filter the signals again to remove trend  (low pass)
+    double out1 = bqc4.step(abs_filtered1);
+        
+    if (out0 > max_out0)                        //  Check if the signals have a new maximum value
+        {   max_out0 = out0;    }
+    if (out1 > max_out1)
+        {   max_out1 = out1;    }
+    
+    double normalized_out0 = out0 / max_out0;   //  Normalize the signals (divide by maximum value to scale from 0 to 1)
+    double normalized_out1 = out1 / max_out1;
+    
+    if (normalized_out0 > threshold0)           //  If the signal value is above the threshold, give an output of 1
+        {   digital0 = 1;   }
+    else                                        //  If the signal value is below the threshold, give an output of 0
+        {   digital0 = 0;   }
+            
+    if (normalized_out1 > threshold1)           //  Do the same for the second EMG signal
+    {   digital1 = 1;   }
+    else
+    {   digital1 = 0;   }
+    
+    scope.set(0,in0);           //  EMG signal 0    (right biceps)
+    scope.set(1,in1);           //  EMG signal 1    (left biceps)
+    scope.set(2,digital0);      //  Signal 0 after filtering, detrending, rectification, digitalization
+    scope.set(3,digital1);      //  Signal 1 after filtering, detrending, rectification, digitalization
+    scope.set(4,digital0 - digital1);         //  Final output signal
+    
+    scope.send();               //  Send all channels to the PC at once
+
+    return digital0 - digital1;         //  Subtract the digital signals (right arm gives positive values, left arm give negative values)
+}
+
+
+
 // Functie voor het vinden van de reference position van motor 1 --------------------
 double get_reference_position_m1(double &ref_pos_prev, const double vrijheid_rad){
     double ref_pos;
-    int final_signal = EMG_sim1.read() - EMG_sim2.read();     // Het uiteindelijke signaal uit de emg als output zit is -1, 0 of 1.
+    final_signal = get_EMG();     // Het uiteindelijke signaal uit de emg als output zit is -1, 0 of 1.
     switch(final_signal){
         case 0 :
             ref_pos = ref_pos_prev;
@@ -352,7 +430,7 @@
                 break;
             
         }
-        call_HIDscope(PID_output_2, reference_pos_m2, position_motor2);
+        //call_HIDscope(final_signal, reference_pos_m1, get_position_m1(rad_per_count));
     }  
 }