EMG.cpp

00001 #include "EMG.h"
00002 #include"BiQuad.h"
00003 
00004 EMG::EMG(PinName data) : _data(data)
00005 {
00006     cntr = 0;
00007 }
00008 
00009 
00010 double EMG::get_notch(double data){
00011     
00012     double a0=1,                        a1=-1.525271192436899,          a2=0.881618592363190,
00013        b0=0.940809296181595,        b1=-1.525271192436899,          b2=0.940809296181595;
00014            
00015     BiQuad MainsReject(b0, b1, b2, a0, a1, a2);
00016     
00017     return MainsReject.step(data);
00018     
00019 }
00020     
00021     
00022 double EMG::get_noise(double data){
00023    
00024    double a0=1,                        a1=-0.671029090774096,          a2=0.252324626282266,
00025        b0=0.145323883877042,        b1= 0.290647767754085,          b2= 0.145323883877042;
00026            
00027     BiQuad Low_pass(b0, b1, b2, a0, a1, a2);
00028     
00029     return Low_pass.step(data);
00030 }
00031     
00032     
00033 double EMG::get_DC(double data){
00034     
00035     double a_0_0=1,                        a_0_1=-1.475480443592646,          a_0_2=0.586919508061190,
00036        b_0_0=0.765599987913459,        b_0_1=-1.531199975826918,          b_0_2=0.765599987913459;
00037            
00038     BiQuad HiPass(b_0_0, b_0_1, b_0_2, a_0_0, a_0_1, a_0_2);
00039     
00040     return HiPass.step(data);
00041     
00042 }
00043     
00044     
00045 double EMG::get_absolute(double data){
00046     
00047     return abs(data);
00048     
00049     
00050 }
00051     
00052     
00053 double EMG::get_envelope(double data){
00054     
00055     double a_1_0=1,                        a_1_1=-1.982228929792529,          a_1_2=0.982385450614126,
00056        b_1_0=0.00003913020539916823,    b_1_1=0.00007826041079833645,           b_1_2=0.00003913020539916823;
00057            
00058     BiQuad LoPass(b_1_0, b_1_1, b_1_2, a_1_0, a_1_1, a_1_2);
00059     
00060     return LoPass.step(data);
00061 }
00062     
00063 double EMG::filter_emg(double data){
00064     
00065     if(cntr<=500)
00066     {
00067         cntr++;
00068         return 0;
00069     }
00070     else
00071     {
00072         return get_envelope(get_absolute(get_DC(get_notch(get_noise(data)))));
00073     }
00074     
00075 }