Bayu Dharmaputra / Mbed 2 deprecated Filter_EMG

For BIOROBOTICS PROJECT

Dependencies:   HIDScope MODSERIAL QEI mbed

main.cpp@2:ac98d055a6cd, 2015-10-28 (annotated)

Committer:
dbayuadi
Date:
Wed Oct 28 09:47:23 2015 +0000
Revision:
2:ac98d055a6cd
Parent:
1:3bae5ab25e20 
filter EMG

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dbayuadi 0:8ccd4c66e07f 1 #include "mbed.h"
dbayuadi 0:8ccd4c66e07f 2 #include "HIDScope.h"
dbayuadi 0:8ccd4c66e07f 3 #include "QEI.h"
dbayuadi 2:ac98d055a6cd 4 AnalogIn emg_left(A0);
dbayuadi 2:ac98d055a6cd 5 AnalogIn emg_right(A1);
dbayuadi 2:ac98d055a6cd 6 DigitalOut led(LED_BLUE);
dbayuadi 2:ac98d055a6cd 7 HIDScope scope(6);
dbayuadi 0:8ccd4c66e07f 8 Ticker get;
dbayuadi 0:8ccd4c66e07f 9 AnalogIn pot(A1);
dbayuadi 0:8ccd4c66e07f 10 Serial pc(USBTX, USBRX); // tx, rx
dbayuadi 0:8ccd4c66e07f 11 volatile bool fn_go = false;
dbayuadi 0:8ccd4c66e07f 12 double y_notch_1;
dbayuadi 0:8ccd4c66e07f 13 double y_notch_2;
dbayuadi 0:8ccd4c66e07f 14 double y_high;
dbayuadi 0:8ccd4c66e07f 15 double y_low;
dbayuadi 1:3bae5ab25e20 16 double y_rect;
dbayuadi 2:ac98d055a6cd 17 const double Fs = 500*2;
dbayuadi 2:ac98d055a6cd 18 const double Ts = 1/Fs;
dbayuadi 1:3bae5ab25e20 19 double a;
dbayuadi 2:ac98d055a6cd 20 const double gain_notch_1 = 1.000000;
dbayuadi 0:8ccd4c66e07f 21 const double b0_notch_1 = 1.0*gain_notch_1;
dbayuadi 2:ac98d055a6cd 22 const double b1_notch_1 = -1.90215058158*gain_notch_1;
dbayuadi 0:8ccd4c66e07f 23 const double b2_notch_1 = 1.0*gain_notch_1;
dbayuadi 2:ac98d055a6cd 24 const double a1_notch_1 = -1.76851639902;
dbayuadi 2:ac98d055a6cd 25 const double a2_notch_1 = 0.89458928499;
dbayuadi 0:8ccd4c66e07f 26 double v1_notch_1 = 0, v2_notch_1 = 0;
dbayuadi 0:8ccd4c66e07f 27
dbayuadi 0:8ccd4c66e07f 28
dbayuadi 0:8ccd4c66e07f 29 const double gain_notch_2 = 1.000000;
dbayuadi 0:8ccd4c66e07f 30 const double b0_notch_2 = 1.0*gain_notch_2;
dbayuadi 2:ac98d055a6cd 31 const double b1_notch_2 = -1.90215058158*gain_notch_2;
dbayuadi 0:8ccd4c66e07f 32 const double b2_notch_2 = 1.0*gain_notch_2;
dbayuadi 2:ac98d055a6cd 33 const double a1_notch_2 = -1.85196790295;
dbayuadi 2:ac98d055a6cd 34 const double a2_notch_2 = 0.92089778695;
dbayuadi 0:8ccd4c66e07f 35 double v1_notch_2 = 0, v2_notch_2 = 0;
dbayuadi 2:ac98d055a6cd 36 //high pass of 5 Hz Wc
dbayuadi 0:8ccd4c66e07f 37
dbayuadi 2:ac98d055a6cd 38 const double gain_high = 1;
dbayuadi 2:ac98d055a6cd 39 const double b0_high = 0.978030479206560*gain_high;
dbayuadi 2:ac98d055a6cd 40 const double b1_high = -1.956060958413119*gain_high;
dbayuadi 2:ac98d055a6cd 41 const double b2_high = 0.978030479206560*gain_high;
dbayuadi 2:ac98d055a6cd 42 const double a1_high = -1.955578240315036;
dbayuadi 2:ac98d055a6cd 43 const double a2_high = 0.956543676511203;
dbayuadi 0:8ccd4c66e07f 44 double v1_high = 0, v2_high = 0;
dbayuadi 2:ac98d055a6cd 45 double v1_high_1 = 0, v2_high_1=0;
dbayuadi 0:8ccd4c66e07f 46
dbayuadi 2:ac98d055a6cd 47 //low pass of 0.3 Hz Wc
dbayuadi 2:ac98d055a6cd 48 const double gain_low = 0.00001;
dbayuadi 2:ac98d055a6cd 49 const double b0_low = 0.088708177364838*gain_low;
dbayuadi 2:ac98d055a6cd 50 const double b1_low = 0.177416354729676*gain_low;
dbayuadi 2:ac98d055a6cd 51 const double b2_low = 0.088708177364838*gain_low;
dbayuadi 2:ac98d055a6cd 52 const double a1_low = -1.997334271812535;
dbayuadi 2:ac98d055a6cd 53 const double a2_low = 0.997337820139629;
dbayuadi 0:8ccd4c66e07f 54 double v1_low = 0, v2_low = 0;
dbayuadi 2:ac98d055a6cd 55 double v1_low_1 = 0,v2_low_1=0;
dbayuadi 0:8ccd4c66e07f 56 double biquad(double u, double&v1,double&v2,const double a1, const double a2, const double b0, const double b1, const double b2)
dbayuadi 0:8ccd4c66e07f 57 {
dbayuadi 0:8ccd4c66e07f 58 double v = u - a1*v1 - a2*v2;
dbayuadi 0:8ccd4c66e07f 59 double y = b0*v + b1*v1 + b2*v2;
dbayuadi 0:8ccd4c66e07f 60 v2 = v1;
dbayuadi 0:8ccd4c66e07f 61 v1 = v;
dbayuadi 0:8ccd4c66e07f 62 return y;
dbayuadi 0:8ccd4c66e07f 63 }
dbayuadi 0:8ccd4c66e07f 64
dbayuadi 2:ac98d055a6cd 65 double biquad1(double u, double&v1,double&v2,const double a1, const double a2, const double b0, const double b1, const double b2)
dbayuadi 2:ac98d055a6cd 66 {
dbayuadi 2:ac98d055a6cd 67 double v = u - a1*v1 - a2*v2;
dbayuadi 2:ac98d055a6cd 68 double y = b0*v + b1*v1 + b2*v2;
dbayuadi 2:ac98d055a6cd 69 v2 = v1;
dbayuadi 2:ac98d055a6cd 70 v1 = v;
dbayuadi 2:ac98d055a6cd 71 return y;
dbayuadi 2:ac98d055a6cd 72 }
dbayuadi 2:ac98d055a6cd 73
dbayuadi 2:ac98d055a6cd 74 double biquad2(double u, double&v1,double&v2,const double a1, const double a2, const double b0, const double b1, const double b2)
dbayuadi 2:ac98d055a6cd 75 {
dbayuadi 2:ac98d055a6cd 76 double v = u - a1*v1 - a2*v2;
dbayuadi 2:ac98d055a6cd 77 double y = b0*v + b1*v1 + b2*v2;
dbayuadi 2:ac98d055a6cd 78 v2 = v1;
dbayuadi 2:ac98d055a6cd 79 v1 = v;
dbayuadi 2:ac98d055a6cd 80 return y;
dbayuadi 2:ac98d055a6cd 81 }
dbayuadi 2:ac98d055a6cd 82
dbayuadi 2:ac98d055a6cd 83 double biquad3(double u, double&v1,double&v2,const double a1, const double a2, const double b0, const double b1, const double b2)
dbayuadi 2:ac98d055a6cd 84 {
dbayuadi 2:ac98d055a6cd 85 double v = u - a1*v1 - a2*v2;
dbayuadi 2:ac98d055a6cd 86 double y = b0*v + b1*v1 + b2*v2;
dbayuadi 2:ac98d055a6cd 87 v2 = v1;
dbayuadi 2:ac98d055a6cd 88 v1 = v;
dbayuadi 2:ac98d055a6cd 89 return y;
dbayuadi 2:ac98d055a6cd 90 }
dbayuadi 2:ac98d055a6cd 91
dbayuadi 2:ac98d055a6cd 92 double biquad4(double u, double&v1,double&v2,const double a1, const double a2, const double b0, const double b1, const double b2)
dbayuadi 2:ac98d055a6cd 93 {
dbayuadi 2:ac98d055a6cd 94 double v = u - a1*v1 - a2*v2;
dbayuadi 2:ac98d055a6cd 95 double y = b0*v + b1*v1 + b2*v2;
dbayuadi 2:ac98d055a6cd 96 v2 = v1;
dbayuadi 2:ac98d055a6cd 97 v1 = v;
dbayuadi 2:ac98d055a6cd 98 return y;
dbayuadi 2:ac98d055a6cd 99 }
dbayuadi 2:ac98d055a6cd 100
dbayuadi 2:ac98d055a6cd 101
dbayuadi 2:ac98d055a6cd 102
dbayuadi 2:ac98d055a6cd 103
dbayuadi 0:8ccd4c66e07f 104 void fn_activate()
dbayuadi 0:8ccd4c66e07f 105 {
dbayuadi 0:8ccd4c66e07f 106
dbayuadi 0:8ccd4c66e07f 107 fn_go = true;
dbayuadi 0:8ccd4c66e07f 108 }
dbayuadi 0:8ccd4c66e07f 109
dbayuadi 2:ac98d055a6cd 110 double z1;
dbayuadi 2:ac98d055a6cd 111 double y_high_1;
dbayuadi 2:ac98d055a6cd 112 double y_notch_1_1;
dbayuadi 2:ac98d055a6cd 113 double y_notch_2_1;
dbayuadi 2:ac98d055a6cd 114 double y_rect_1;
dbayuadi 2:ac98d055a6cd 115 double y_low_1;
dbayuadi 2:ac98d055a6cd 116 double t = 0;
dbayuadi 2:ac98d055a6cd 117 double value = 0;
dbayuadi 2:ac98d055a6cd 118 double max() // calibration
dbayuadi 1:3bae5ab25e20 119 {
dbayuadi 2:ac98d055a6cd 120 if (t <= 10) {
dbayuadi 2:ac98d055a6cd 121 value = value + 0.0;
dbayuadi 2:ac98d055a6cd 122 } else if(t>10 && t<=20) {
dbayuadi 2:ac98d055a6cd 123 if(value<=y_low) {
dbayuadi 2:ac98d055a6cd 124 value = y_low;
dbayuadi 2:ac98d055a6cd 125
dbayuadi 2:ac98d055a6cd 126 } else {
dbayuadi 2:ac98d055a6cd 127 value = value +0.0;
dbayuadi 1:3bae5ab25e20 128 }
dbayuadi 2:ac98d055a6cd 129
dbayuadi 2:ac98d055a6cd 130 } else {
dbayuadi 2:ac98d055a6cd 131 value = value + 0.0;
dbayuadi 2:ac98d055a6cd 132
dbayuadi 2:ac98d055a6cd 133
dbayuadi 2:ac98d055a6cd 134 }
dbayuadi 2:ac98d055a6cd 135 return value*0.80;
dbayuadi 1:3bae5ab25e20 136 }
dbayuadi 2:ac98d055a6cd 137 double val;
dbayuadi 2:ac98d055a6cd 138 double gain()
dbayuadi 2:ac98d055a6cd 139 {
dbayuadi 2:ac98d055a6cd 140
dbayuadi 2:ac98d055a6cd 141 if(max()==0) {
dbayuadi 2:ac98d055a6cd 142 val = 0;
dbayuadi 2:ac98d055a6cd 143 } else if(max()>=0) {
dbayuadi 2:ac98d055a6cd 144 val = 1/max();
dbayuadi 2:ac98d055a6cd 145 }
dbayuadi 2:ac98d055a6cd 146 return val;
dbayuadi 2:ac98d055a6cd 147 }
dbayuadi 2:ac98d055a6cd 148 const double a1 = 11.1111;
dbayuadi 2:ac98d055a6cd 149 const double b1 = -0.1111;
dbayuadi 2:ac98d055a6cd 150 const double a2 = 4.7619;
dbayuadi 2:ac98d055a6cd 151 const double b2 = -0.047619;
dbayuadi 0:8ccd4c66e07f 152 void scopeSend()
dbayuadi 0:8ccd4c66e07f 153 {
dbayuadi 2:ac98d055a6cd 154 y_high = biquad1(emg_left.read(), v1_high, v2_high, a1_high, a2_high, b0_high, b1_high, b2_high);
dbayuadi 2:ac98d055a6cd 155 y_rect = abs(y_high);
dbayuadi 2:ac98d055a6cd 156 y_low = biquad2(y_rect, v1_low, v2_low, a1_low, a2_low, b0_low, b1_low, b2_low);
dbayuadi 2:ac98d055a6cd 157 t = t+Ts;
dbayuadi 2:ac98d055a6cd 158 y_high_1 = biquad3(emg_right.read(), v1_high_1, v2_high_1, a1_high, a2_high, b0_high, b1_high, b2_high);
dbayuadi 2:ac98d055a6cd 159 y_rect_1 = abs(y_high_1);
dbayuadi 2:ac98d055a6cd 160 y_low_1 = biquad4(y_rect_1, v1_low_1, v2_low_1, a1_low, a2_low, b0_low, b1_low, b2_low);
dbayuadi 2:ac98d055a6cd 161
dbayuadi 2:ac98d055a6cd 162 scope.set(0,emg_left.read());
dbayuadi 2:ac98d055a6cd 163 scope.set(1,emg_right.read());
dbayuadi 2:ac98d055a6cd 164 scope.set(2,y_low);
dbayuadi 2:ac98d055a6cd 165 scope.set(3,y_low_1);
dbayuadi 2:ac98d055a6cd 166 scope.set(4,a1*y_low + b1);
dbayuadi 2:ac98d055a6cd 167 scope.set(5,a2*y_low_1 + b2);
dbayuadi 2:ac98d055a6cd 168
dbayuadi 1:3bae5ab25e20 169
dbayuadi 0:8ccd4c66e07f 170 scope.send();
dbayuadi 0:8ccd4c66e07f 171 }
dbayuadi 0:8ccd4c66e07f 172
dbayuadi 0:8ccd4c66e07f 173 int main()
dbayuadi 0:8ccd4c66e07f 174 {
dbayuadi 2:ac98d055a6cd 175 get.attach(&fn_activate,Ts);
dbayuadi 0:8ccd4c66e07f 176
dbayuadi 0:8ccd4c66e07f 177 while (true) {
dbayuadi 2:ac98d055a6cd 178 if(t<=10) {
dbayuadi 2:ac98d055a6cd 179 led = 0;
dbayuadi 2:ac98d055a6cd 180 }
dbayuadi 2:ac98d055a6cd 181 if(t>10 && t<=20)
dbayuadi 2:ac98d055a6cd 182 {
dbayuadi 2:ac98d055a6cd 183 led = 1;
dbayuadi 2:ac98d055a6cd 184 }
dbayuadi 2:ac98d055a6cd 185 if(t>20)
dbayuadi 2:ac98d055a6cd 186 {
dbayuadi 2:ac98d055a6cd 187 led = 0;
dbayuadi 2:ac98d055a6cd 188 }
dbayuadi 0:8ccd4c66e07f 189 if(fn_go == true) {
dbayuadi 2:ac98d055a6cd 190
dbayuadi 0:8ccd4c66e07f 191 scopeSend();
dbayuadi 2:ac98d055a6cd 192
dbayuadi 2:ac98d055a6cd 193
dbayuadi 0:8ccd4c66e07f 194 fn_go = false;
dbayuadi 0:8ccd4c66e07f 195 }
dbayuadi 0:8ccd4c66e07f 196 }
dbayuadi 0:8ccd4c66e07f 197 }