Diff: EMG_input.cpp

Revision:

27:869ec80f2bf7

Child:

28:ceed9f02b815

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/EMG_input.cpp	Mon Oct 17 16:02:07 2016 +0000
@@ -0,0 +1,55 @@
+#include "mbed.h"
+#include "HIDScope.h"
+#include "BiQuad.h"
+#include "math.h"
+#define EMG_DEBUG
+#ifndef M_PI
+#define M_PI    3.14159265358979323846
+#endif
+
+#define EMG_THRESHOLD   0.2
+
+class EMG_input {
+    private:
+        AnalogIn emg0;     // Declare port
+        BiQuad bq1, bq2, bq3, bq4, bq5, bq6, bq7, bq8;  // Declare biquads
+        BiQuadChain bqc1, bqc2;  // Declare biquad chains
+        double maxsig;        
+    public:
+        double e0, e0f, e0flow, discrete;
+        EMG_input(PinName pin);
+        void tick(void);
+        bool read(void);
+        ~EMG_input();
+        
+};
+
+EMG_input::EMG_input(PinName pin) : emg0(pin),
+    bq1(9.755715E-01, -1.951143E+00, 9.755715E-01, 1.000000E+00, -1.949216E+00, 9.530699E-01),   // High pass 10 Hz (4th order)
+    bq2(9.442373E-01, -1.888475E+00, 9.442373E-01, 1.000000E+00, -1.886610E+00, 8.903397E-01),
+    bq3(9.955670E-01, -1.893718E+00, 9.955670E-01, 1.000000E+00, -1.890851E+00, 9.910331E-01),   // Notch (stop band) 49 - 51 Hz (4th order)
+    bq4(9.955670E-01, -1.893718E+00, 9.955670E-01, 1.000000E+00, -1.896548E+00, 9.912741E-01),
+    bq5(4.798613E-01, 9.597225E-01, 4.798613E-01, 1.000000E+00, 4.531195E-01, 4.663256E-01),     // Low pass 300 Hz (4th order)
+    bq6(3.483908E-01, 6.967817E-01, 3.483908E-01, 1.000000E+00, 3.289757E-01, 6.458765E-02),
+    bq7(8.818771E-05, 1.763754E-04, 8.818771E-05, 1.000000E+00, -1.985325E+00, 9.856773E-01),    // Low pass 3Hz on rectified signal (4th order)
+    bq8(8.730353E-05, 1.746071E-04, 8.730353E-05, 1.000000E+00, -1.965420E+00, 9.657687E-01),
+    maxsig(0) {
+    bqc1.add(&bq1).add(&bq2).add(&bq3).add(&bq4).add(&bq5).add(&bq6);    // Add filters to filter chain
+    bqc2.add(&bq7).add(&bq8);    // Add filters to filter chain   
+}
+
+
+void EMG_input::tick(void){
+    double e0 = emg0.read();    // Read emg signal
+    double e0f = fabs(bqc1.step(e0));    // Filter signal, rectify
+    double e0flow = bqc2.step(e0f); // Low pass filter rectified emg signal
+    if (e0flow>maxsig){ maxsig = e0flow;}   // Remember maximum peak (used for calibrating signal)
+    double discrete = floor(sqrt(e0flow/maxsig)*10.0)/10.0;    // Discretise signal non linearly (low muscle activity means higher gain on the input signal, sqrt used as envelope)
+}
+
+bool EMG_input::read(void){
+    return (discrete>=EMG_THRESHOLD)?true:false; 
+}
+
+EMG_input::~EMG_input(){
+}
\ No newline at end of file