Motor class working
Dependencies: QEI biquadFilter mbed
Revision 4:38c653bfec5f, committed 2017-10-16
- Comitter:
- Alex_Kyrl
- Date:
- Mon Oct 16 12:34:11 2017 +0000
- Parent:
- 3:9d861827b714
- Commit message:
- added EMG class;
Changed in this revision
EMG.cpp | Show annotated file Show diff for this revision Revisions of this file |
EMG.h | Show annotated file Show diff for this revision Revisions of this file |
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/EMG.cpp Mon Oct 16 12:34:11 2017 +0000 @@ -0,0 +1,75 @@ +#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))))); + } + +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/EMG.h Mon Oct 16 12:34:11 2017 +0000 @@ -0,0 +1,26 @@ +#ifndef EMG_H +#define EMG_H + +#include "mbed.h" + +class EMG { + private: + AnalogIn _data; + int cntr; + + double get_notch(double); + double get_noise(double); + double get_DC(double); + double get_absolute(double); + double get_envelope(double); + + public: + EMG(PinName); + + double filter_emg(double); + + + +}; + +#endif \ No newline at end of file