Alexandros Kyrloglou / Mbed 2 deprecated BioRobotics_pool_bot

Motor class working

Dependencies:   QEI biquadFilter mbed

EMG.cpp@4:38c653bfec5f, 2017-10-16 (annotated)

Committer:
Alex_Kyrl
Date:
Mon Oct 16 12:34:11 2017 +0000
Revision:
4:38c653bfec5f
added EMG class;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Alex_Kyrl 4:38c653bfec5f 1 #include "EMG.h"
Alex_Kyrl 4:38c653bfec5f 2 #include"BiQuad.h"
Alex_Kyrl 4:38c653bfec5f 3
Alex_Kyrl 4:38c653bfec5f 4 EMG::EMG(PinName data) : _data(data)
Alex_Kyrl 4:38c653bfec5f 5 {
Alex_Kyrl 4:38c653bfec5f 6 cntr = 0;
Alex_Kyrl 4:38c653bfec5f 7 }
Alex_Kyrl 4:38c653bfec5f 8
Alex_Kyrl 4:38c653bfec5f 9
Alex_Kyrl 4:38c653bfec5f 10 double EMG::get_notch(double data){
Alex_Kyrl 4:38c653bfec5f 11
Alex_Kyrl 4:38c653bfec5f 12 double a0=1, a1=-1.525271192436899, a2=0.881618592363190,
Alex_Kyrl 4:38c653bfec5f 13 b0=0.940809296181595, b1=-1.525271192436899, b2=0.940809296181595;
Alex_Kyrl 4:38c653bfec5f 14
Alex_Kyrl 4:38c653bfec5f 15 BiQuad MainsReject(b0, b1, b2, a0, a1, a2);
Alex_Kyrl 4:38c653bfec5f 16
Alex_Kyrl 4:38c653bfec5f 17 return MainsReject.step(data);
Alex_Kyrl 4:38c653bfec5f 18
Alex_Kyrl 4:38c653bfec5f 19 }
Alex_Kyrl 4:38c653bfec5f 20
Alex_Kyrl 4:38c653bfec5f 21
Alex_Kyrl 4:38c653bfec5f 22 double EMG::get_noise(double data){
Alex_Kyrl 4:38c653bfec5f 23
Alex_Kyrl 4:38c653bfec5f 24 double a0=1, a1=-0.671029090774096, a2=0.252324626282266,
Alex_Kyrl 4:38c653bfec5f 25 b0=0.145323883877042, b1= 0.290647767754085, b2= 0.145323883877042;
Alex_Kyrl 4:38c653bfec5f 26
Alex_Kyrl 4:38c653bfec5f 27 BiQuad Low_pass(b0, b1, b2, a0, a1, a2);
Alex_Kyrl 4:38c653bfec5f 28
Alex_Kyrl 4:38c653bfec5f 29 return Low_pass.step(data);
Alex_Kyrl 4:38c653bfec5f 30 }
Alex_Kyrl 4:38c653bfec5f 31
Alex_Kyrl 4:38c653bfec5f 32
Alex_Kyrl 4:38c653bfec5f 33 double EMG::get_DC(double data){
Alex_Kyrl 4:38c653bfec5f 34
Alex_Kyrl 4:38c653bfec5f 35 double a_0_0=1, a_0_1=-1.475480443592646, a_0_2=0.586919508061190,
Alex_Kyrl 4:38c653bfec5f 36 b_0_0=0.765599987913459, b_0_1=-1.531199975826918, b_0_2=0.765599987913459;
Alex_Kyrl 4:38c653bfec5f 37
Alex_Kyrl 4:38c653bfec5f 38 BiQuad HiPass(b_0_0, b_0_1, b_0_2, a_0_0, a_0_1, a_0_2);
Alex_Kyrl 4:38c653bfec5f 39
Alex_Kyrl 4:38c653bfec5f 40 return HiPass.step(data);
Alex_Kyrl 4:38c653bfec5f 41
Alex_Kyrl 4:38c653bfec5f 42 }
Alex_Kyrl 4:38c653bfec5f 43
Alex_Kyrl 4:38c653bfec5f 44
Alex_Kyrl 4:38c653bfec5f 45 double EMG::get_absolute(double data){
Alex_Kyrl 4:38c653bfec5f 46
Alex_Kyrl 4:38c653bfec5f 47 return abs(data);
Alex_Kyrl 4:38c653bfec5f 48
Alex_Kyrl 4:38c653bfec5f 49
Alex_Kyrl 4:38c653bfec5f 50 }
Alex_Kyrl 4:38c653bfec5f 51
Alex_Kyrl 4:38c653bfec5f 52
Alex_Kyrl 4:38c653bfec5f 53 double EMG::get_envelope(double data){
Alex_Kyrl 4:38c653bfec5f 54
Alex_Kyrl 4:38c653bfec5f 55 double a_1_0=1, a_1_1=-1.982228929792529, a_1_2=0.982385450614126,
Alex_Kyrl 4:38c653bfec5f 56 b_1_0=0.00003913020539916823, b_1_1=0.00007826041079833645, b_1_2=0.00003913020539916823;
Alex_Kyrl 4:38c653bfec5f 57
Alex_Kyrl 4:38c653bfec5f 58 BiQuad LoPass(b_1_0, b_1_1, b_1_2, a_1_0, a_1_1, a_1_2);
Alex_Kyrl 4:38c653bfec5f 59
Alex_Kyrl 4:38c653bfec5f 60 return LoPass.step(data);
Alex_Kyrl 4:38c653bfec5f 61 }
Alex_Kyrl 4:38c653bfec5f 62
Alex_Kyrl 4:38c653bfec5f 63 double EMG::filter_emg(double data){
Alex_Kyrl 4:38c653bfec5f 64
Alex_Kyrl 4:38c653bfec5f 65 if(cntr<=500)
Alex_Kyrl 4:38c653bfec5f 66 {
Alex_Kyrl 4:38c653bfec5f 67 cntr++;
Alex_Kyrl 4:38c653bfec5f 68 return 0;
Alex_Kyrl 4:38c653bfec5f 69 }
Alex_Kyrl 4:38c653bfec5f 70 else
Alex_Kyrl 4:38c653bfec5f 71 {
Alex_Kyrl 4:38c653bfec5f 72 return get_envelope(get_absolute(get_DC(get_notch(get_noise(data)))));
Alex_Kyrl 4:38c653bfec5f 73 }
Alex_Kyrl 4:38c653bfec5f 74
Alex_Kyrl 4:38c653bfec5f 75 }