Linde Haverkate / Mbed 2 deprecated FilterenEMG

Script zelf doet het, waardes van de Biquads kloppen wss niet. Na filteren is aanspannen niet meer te detecteren

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@6:ee304066e301, 2019-10-29 (annotated)

Committer:
linde101
Date:
Tue Oct 29 18:02:58 2019 +0000
Revision:
6:ee304066e301
Parent:
5:0e197bf776fc 
Nu doet hij het wel, maar de filter waardes kloppen niet;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RobertoO 0:67c50348f842 1 #include "mbed.h"
linde101 2:76d6803aa6e3 2 #include "HIDScope.h"
linde101 2:76d6803aa6e3 3 #include "QEI.h"
RobertoO 1:b862262a9d14 4 #include "MODSERIAL.h"
linde101 2:76d6803aa6e3 5 #include "BiQuad.h"
linde101 2:76d6803aa6e3 6 #include "FastPWM.h"
linde101 5:0e197bf776fc 7 #include "math.h"
linde101 5:0e197bf776fc 8 #include <stdio.h> /* printf */
linde101 5:0e197bf776fc 9 #include <stdlib.h> /* abs */
linde101 5:0e197bf776fc 10 #include <complex>
RobertoO 0:67c50348f842 11
linde101 5:0e197bf776fc 12
RobertoO 1:b862262a9d14 13 MODSERIAL pc(USBTX, USBRX);
RobertoO 0:67c50348f842 14
linde101 5:0e197bf776fc 15 //Filters EMG
linde101 5:0e197bf776fc 16 //Filters Linker Biceps
linde101 5:0e197bf776fc 17 //In de volgorde High pass, notch, low pass
linde101 5:0e197bf776fc 18 BiQuad LBHP1(1, 2.000000023182220, 0.999999981993818, 1.893415601022358, 0.896255659548714);
linde101 5:0e197bf776fc 19 BiQuad LBHP2(1, 1.999999976817780, 0.999999976817780, 1.778313488139574, 0.790599715524785);
linde101 5:0e197bf776fc 20
linde101 5:0e197bf776fc 21 BiQuad LBN1( 0.5, 0, 0.5, 0, 0);
linde101 5:0e197bf776fc 22
linde101 5:0e197bf776fc 23 BiQuad LBLP1( 0.000155148423475721, 0.000310296846951441, 0.000155148423475721,-1.96446058020523, 0.965081173899135);
linde101 5:0e197bf776fc 24 BiQuad LBLP2( 0.000155148423475721, 0.000310296846951441, 0.000155148423475721,-1.96446058020523, 0.965081173899135);
linde101 5:0e197bf776fc 25 BiQuadChain LeftBicep;
linde101 5:0e197bf776fc 26
linde101 5:0e197bf776fc 27
linde101 5:0e197bf776fc 28
linde101 5:0e197bf776fc 29 //Filters Rechter Biceps
linde101 5:0e197bf776fc 30 //In de volgorde High pass, notch, low pass
linde101 5:0e197bf776fc 31 BiQuad RBHP1(1, 2.000000023182220, 0.999999981993818, 1.893415601022358, 0.896255659548714);
linde101 5:0e197bf776fc 32 BiQuad RBHP2(1, 1.999999976817780, 0.999999976817780, 1.778313488139574, 0.790599715524785);
linde101 5:0e197bf776fc 33
linde101 5:0e197bf776fc 34 BiQuad RBN1( 0.5, 0, 0.5, 0, 0);
linde101 5:0e197bf776fc 35
linde101 5:0e197bf776fc 36 BiQuad RBLP1( 0.000155148423475721, 0.000310296846951441, 0.000155148423475721,-1.96446058020523, 0.965081173899135);
linde101 5:0e197bf776fc 37 BiQuad RBLP2( 0.000155148423475721, 0.000310296846951441, 0.000155148423475721,-1.96446058020523, 0.965081173899135);
linde101 5:0e197bf776fc 38 BiQuadChain RightBicep;
linde101 3:73ed6985b8d4 39
linde101 4:2930e35b8e8e 40
linde101 5:0e197bf776fc 41 //Filters Rechter Quadriceps
linde101 5:0e197bf776fc 42 //In de volgorde High pass, notch, low pass
linde101 5:0e197bf776fc 43 BiQuad RQHP1(1, 2.000000023182220, 0.999999981993818, 1.893415601022358, 0.896255659548714);
linde101 5:0e197bf776fc 44 BiQuad RQHP2(1, 1.999999976817780, 0.999999976817780, 1.778313488139574, 0.790599715524785);
linde101 5:0e197bf776fc 45
linde101 5:0e197bf776fc 46 BiQuad RQN1( 0.5, 0, 0.5, 0, 0);
linde101 5:0e197bf776fc 47
linde101 5:0e197bf776fc 48 BiQuad RQLP1( 0.000155148423475721, 0.000310296846951441, 0.000155148423475721,-1.96446058020523, 0.965081173899135);
linde101 5:0e197bf776fc 49 BiQuad RQLP2( 0.000155148423475721, 0.000310296846951441, 0.000155148423475721,-1.96446058020523, 0.965081173899135);
linde101 5:0e197bf776fc 50 BiQuadChain RightLeg;
linde101 5:0e197bf776fc 51 //BiQuadChain bqc;
linde101 5:0e197bf776fc 52 //bqc.add( &bq1 ).add( &bq2 );
linde101 5:0e197bf776fc 53
linde101 5:0e197bf776fc 54 //double emgLBHP;
linde101 5:0e197bf776fc 55 //double emgLBNotch;
linde101 5:0e197bf776fc 56 //double emgAbsLPHP;
linde101 5:0e197bf776fc 57 //double emgLBLP;
linde101 5:0e197bf776fc 58 //double emgAbsLBNotch;
linde101 5:0e197bf776fc 59
linde101 5:0e197bf776fc 60 //double emgRBHP;
linde101 5:0e197bf776fc 61 //double emgRBNotch;
linde101 5:0e197bf776fc 62 //double emgAbsRBHP;
linde101 5:0e197bf776fc 63 //double emgRBLP;
linde101 5:0e197bf776fc 64 //double emgAbsRBNotch;
linde101 5:0e197bf776fc 65
linde101 5:0e197bf776fc 66 //double emgRQNotch;
linde101 5:0e197bf776fc 67 //double emgRQHP;
linde101 5:0e197bf776fc 68 //double emgAbsRQHP;
linde101 5:0e197bf776fc 69 //double emgRQLP;
linde101 5:0e197bf776fc 70 //double emgAbsRQNotch;
linde101 5:0e197bf776fc 71
linde101 5:0e197bf776fc 72 double emgLBfiltered;
linde101 5:0e197bf776fc 73 double emgRBfiltered;
linde101 5:0e197bf776fc 74 double emgRQfiltered;
linde101 5:0e197bf776fc 75 double emgLBraw;
linde101 5:0e197bf776fc 76 double emgRBraw;
linde101 5:0e197bf776fc 77 double emgRQraw;
linde101 5:0e197bf776fc 78
linde101 5:0e197bf776fc 79
linde101 5:0e197bf776fc 80 AnalogIn emgLB(A0); //read EMG
linde101 5:0e197bf776fc 81 AnalogIn emgRB(A1); //read EMG
linde101 5:0e197bf776fc 82 AnalogIn emgRQ(A2); //read EMG
linde101 5:0e197bf776fc 83
linde101 5:0e197bf776fc 84 HIDScope scope(6);
linde101 5:0e197bf776fc 85
linde101 5:0e197bf776fc 86 Ticker filter;
linde101 5:0e197bf776fc 87
linde101 5:0e197bf776fc 88 void Filteren()
linde101 5:0e197bf776fc 89 {
linde101 5:0e197bf776fc 90 //linkerbicep
linde101 5:0e197bf776fc 91 /*emgLBHP = LBHP.step(emgLB.read() ); // High-pass filter
linde101 5:0e197bf776fc 92 emgLBNotch = LBN.step(emgLBHP); // Notch filter
linde101 5:0e197bf776fc 93 emgAbsLBNotch = abs(emgLBNotch); // Absolute value
linde101 5:0e197bf776fc 94 emgLBLP = LBLP.step(emgAbsLBNotch); // Low-pass filter
linde101 5:0e197bf776fc 95 */
linde101 5:0e197bf776fc 96 emgLBraw= emgLB.read();
linde101 5:0e197bf776fc 97 emgLBfiltered=LeftBicep.step(emgLB.read());
linde101 5:0e197bf776fc 98 scope.set(0, emgLBraw);
linde101 5:0e197bf776fc 99 scope.set(1, emgLBfiltered);
linde101 5:0e197bf776fc 100
linde101 5:0e197bf776fc 101 //rechterbicep
linde101 5:0e197bf776fc 102 /*emgRBHP = RBHP.step(emgRB.read());
linde101 5:0e197bf776fc 103 emgRBNotch = RBN.step(emgRBHP);
linde101 5:0e197bf776fc 104 emgAbsRBNotch = abs(emgRBNotch);
linde101 5:0e197bf776fc 105 emgRBLP = RBLP.step(emgAbsRBNotch);
linde101 5:0e197bf776fc 106 */
linde101 5:0e197bf776fc 107 emgRBraw= emgRB.read();
linde101 5:0e197bf776fc 108 emgRBfiltered=RightBicep.step(emgRB.read());
linde101 5:0e197bf776fc 109 scope.set(2, emgRBraw);
linde101 5:0e197bf776fc 110 scope.set(3, emgRBfiltered);
linde101 5:0e197bf776fc 111
linde101 5:0e197bf776fc 112
linde101 5:0e197bf776fc 113 /*
linde101 5:0e197bf776fc 114 emgRQHP = RQHP.step(emgRQ.read());
linde101 5:0e197bf776fc 115 emgRQNotch = RQN.step(emgRQHP);
linde101 5:0e197bf776fc 116 emgAbsRQNotch = abs(emgRQNotch);
linde101 5:0e197bf776fc 117 emgRQLP = RQLP.step(emgAbsRQNotch);
linde101 5:0e197bf776fc 118 */
linde101 5:0e197bf776fc 119 emgRQraw= emgRQ.read();
linde101 5:0e197bf776fc 120 emgRQfiltered=RightLeg.step(emgRQ.read());
linde101 5:0e197bf776fc 121 scope.set(4, emgRQ.read());
linde101 5:0e197bf776fc 122 scope.set(5, emgRQfiltered);
linde101 6:ee304066e301 123 scope.send();
linde101 5:0e197bf776fc 124
linde101 5:0e197bf776fc 125 }
linde101 5:0e197bf776fc 126
linde101 5:0e197bf776fc 127 int main() {
linde101 5:0e197bf776fc 128 LeftBicep.add( &LBHP1 ).add( &LBHP2 ).add( &LBN1 ).add( &LBLP1 ).add( &LBLP2 );
linde101 5:0e197bf776fc 129 RightBicep.add( &RBHP1 ).add( &RBHP2 ).add( &RBN1 ).add( &RBLP1 ).add( &RBLP2 );
linde101 5:0e197bf776fc 130 RightLeg.add( &RQHP1 ).add( &RQHP2 ).add( &RQN1 ).add( &RQLP1 ).add( &RQLP2 );
linde101 5:0e197bf776fc 131
linde101 5:0e197bf776fc 132
linde101 6:ee304066e301 133 filter.attach(Filteren, 0.001f); //ticker aanroepen van filter
linde101 5:0e197bf776fc 134 }