Motor class working
Dependencies: QEI biquadFilter mbed
EMG.cpp
- Committer:
- Alex_Kyrl
- Date:
- 2017-10-16
- Revision:
- 4:38c653bfec5f
File content as of revision 4:38c653bfec5f:
#include "EMG.h" #include"BiQuad.h" EMG::EMG(PinName data) : _data(data) { cntr = 0; } double EMG::get_notch(double data){ double a0=1, a1=-1.525271192436899, a2=0.881618592363190, b0=0.940809296181595, b1=-1.525271192436899, b2=0.940809296181595; BiQuad MainsReject(b0, b1, b2, a0, a1, a2); return MainsReject.step(data); } double EMG::get_noise(double data){ double a0=1, a1=-0.671029090774096, a2=0.252324626282266, b0=0.145323883877042, b1= 0.290647767754085, b2= 0.145323883877042; BiQuad Low_pass(b0, b1, b2, a0, a1, a2); return Low_pass.step(data); } double EMG::get_DC(double data){ double a_0_0=1, a_0_1=-1.475480443592646, a_0_2=0.586919508061190, b_0_0=0.765599987913459, b_0_1=-1.531199975826918, b_0_2=0.765599987913459; BiQuad HiPass(b_0_0, b_0_1, b_0_2, a_0_0, a_0_1, a_0_2); return HiPass.step(data); } double EMG::get_absolute(double data){ return abs(data); } double EMG::get_envelope(double data){ double a_1_0=1, a_1_1=-1.982228929792529, a_1_2=0.982385450614126, b_1_0=0.00003913020539916823, b_1_1=0.00007826041079833645, b_1_2=0.00003913020539916823; BiQuad LoPass(b_1_0, b_1_1, b_1_2, a_1_0, a_1_1, a_1_2); return LoPass.step(data); } double EMG::filter_emg(double data){ if(cntr<=500) { cntr++; return 0; } else { return get_envelope(get_absolute(get_DC(get_notch(get_noise(data))))); } }