Marijke Zondag / Mbed 2 deprecated Project_script_gedeelte

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   HIDScope MODSERIAL biquadFilter mbed

Fork of Milestone_sample by Marijke Zondag

main.cpp@29:c72e49a3edbb, 2018-10-24 (annotated)

Committer:
MarijkeZondag
Date:
Wed Oct 24 18:32:51 2018 +0000
Revision:
29:c72e49a3edbb
Parent:
28:4534a096b677
Child:
30:12b2107dc139
EMG filter versie Marijke af.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsluiter 0:c8f15874531b 1 #include "mbed.h"
vsluiter 0:c8f15874531b 2 #include "MODSERIAL.h"
MarijkeZondag 10:39ec51206c8b 3 #include "BiQuad.h"
MarijkeZondag 10:39ec51206c8b 4 #include "HIDScope.h"
MarijkeZondag 10:39ec51206c8b 5 #include <math.h>
vsluiter 0:c8f15874531b 6
MarijkeZondag 22:8e61050064a9 7 //ATTENTION: set mBed to version 151
MarijkeZondag 22:8e61050064a9 8 // set QEI to version 0, (gebruiken wij (nog) niet, is voor encoder)
MarijkeZondag 22:8e61050064a9 9 // set MODSERIAL to version 44
MarijkeZondag 22:8e61050064a9 10 // set HIDScope to version 7
MarijkeZondag 22:8e61050064a9 11 // set biquadFilter to version 7
MarijkeZondag 22:8e61050064a9 12
MarijkeZondag 22:8e61050064a9 13 AnalogIn emg0_in (A0); //First raw EMG signal input
MarijkeZondag 22:8e61050064a9 14 AnalogIn emg1_in (A1); //Second raw EMG signal input
MarijkeZondag 22:8e61050064a9 15 AnalogIn emg2_in (A2); //Third raw EMG signal input
MarijkeZondag 29:c72e49a3edbb 16 DigitalOut myled (LED_GREEN); //Led to test if the code works
MarijkeZondag 10:39ec51206c8b 17
MarijkeZondag 29:c72e49a3edbb 18 //MODSERIAL pc(USBTX, USBRX); //Serial communication to see if the code works step by step, turn on if hidscope is off
MarijkeZondag 9:c722418997b5 19
MarijkeZondag 24:6cde78e776b6 20 HIDScope scope( 6 ); //HIDScope set to 3x2 channels for 3 muscles
vsluiter 0:c8f15874531b 21
MarijkeZondag 27:1885e388b4ce 22 //Tickers
MarijkeZondag 24:6cde78e776b6 23 Ticker HIDScope_tick; //Ticker for HIDScope
MarijkeZondag 22:8e61050064a9 24 Ticker filter_tick; //Ticker for EMG filter
MarijkeZondag 22:8e61050064a9 25 Ticker MovAg_tick; //Ticker to calculate Moving Average
MarijkeZondag 9:c722418997b5 26
MarijkeZondag 9:c722418997b5 27 //Global variables
MarijkeZondag 24:6cde78e776b6 28 const float T = 0.002f; //Ticker period
MarijkeZondag 10:39ec51206c8b 29
MarijkeZondag 13:a3d4b4daf5b4 30 //EMG filter
MarijkeZondag 29:c72e49a3edbb 31 double emg0_filt, emg1_filt, emg2_filt; //Variables for filtered EMG data channel 0, 1 and 2
MarijkeZondag 28:4534a096b677 32 double emg0_raw, emg1_raw, emg2_raw;
MarijkeZondag 22:8e61050064a9 33 const int windowsize = 150; //Size of the array over which the moving average (MovAg) is calculated. (random number)
MarijkeZondag 13:a3d4b4daf5b4 34 double sum, sum1, sum2, sum3; //variables used to sum elements in array
MarijkeZondag 22:8e61050064a9 35 double StoreArray0[windowsize], StoreArray1[windowsize], StoreArray2[windowsize]; //Empty arrays to calculate MoveAg
MarijkeZondag 29:c72e49a3edbb 36 double movAg0, movAg1, movAg2; //outcome of MovAg (moet dit een array zijn??)
MarijkeZondag 24:6cde78e776b6 37
MarijkeZondag 22:8e61050064a9 38 //Biquad //Variables for the biquad band filters (alle 3 dezelfde maar je kan niet 3x 'emg0band' aanroepen ofzo)
MarijkeZondag 24:6cde78e776b6 39 BiQuadChain emg0filter;
MarijkeZondag 10:39ec51206c8b 40 BiQuad emg0band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
MarijkeZondag 10:39ec51206c8b 41 BiQuad emg0band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
MarijkeZondag 10:39ec51206c8b 42 BiQuad emg0band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
MarijkeZondag 24:6cde78e776b6 43 BiQuad notch1( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter biquad coefficients
MarijkeZondag 10:39ec51206c8b 44
MarijkeZondag 24:6cde78e776b6 45 BiQuadChain emg1filter;
MarijkeZondag 10:39ec51206c8b 46 BiQuad emg1band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
MarijkeZondag 10:39ec51206c8b 47 BiQuad emg1band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
MarijkeZondag 10:39ec51206c8b 48 BiQuad emg1band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
MarijkeZondag 24:6cde78e776b6 49 BiQuad notch2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter
MarijkeZondag 10:39ec51206c8b 50
MarijkeZondag 24:6cde78e776b6 51 BiQuadChain emg2filter;
MarijkeZondag 10:39ec51206c8b 52 BiQuad emg2band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
MarijkeZondag 10:39ec51206c8b 53 BiQuad emg2band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
MarijkeZondag 10:39ec51206c8b 54 BiQuad emg2band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
MarijkeZondag 14:fa09dae67390 55 BiQuad notch3( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter
MarijkeZondag 10:39ec51206c8b 56
MarijkeZondag 22:8e61050064a9 57 //Functions
MarijkeZondag 10:39ec51206c8b 58
MarijkeZondag 24:6cde78e776b6 59 void HIDScope_sample()
MarijkeZondag 19:466ada92bf65 60 {
MarijkeZondag 19:466ada92bf65 61 /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
MarijkeZondag 24:6cde78e776b6 62 scope.set(0,emg0_raw);
MarijkeZondag 24:6cde78e776b6 63 scope.set(1,emg0_filt);
MarijkeZondag 29:c72e49a3edbb 64 //scope.set(1,movAg0); //als moving average werkt
MarijkeZondag 24:6cde78e776b6 65 scope.set(2,emg1_raw);
MarijkeZondag 24:6cde78e776b6 66 scope.set(3,emg1_filt);
MarijkeZondag 29:c72e49a3edbb 67 //scope.set(3,movAg1); //als moving average werkt
MarijkeZondag 24:6cde78e776b6 68 scope.set(4,emg2_raw);
MarijkeZondag 24:6cde78e776b6 69 scope.set(5,emg2_filt);
MarijkeZondag 29:c72e49a3edbb 70 //scope.set(5,movAg2); //als moving average werkt
MarijkeZondag 25:8bb3c013752f 71
MarijkeZondag 23:67b1b46e7f6f 72 scope.send(); //Send data to HIDScope server
MarijkeZondag 19:466ada92bf65 73 }
MarijkeZondag 19:466ada92bf65 74
MarijkeZondag 10:39ec51206c8b 75 void EMGFilter0()
MarijkeZondag 24:6cde78e776b6 76 {
MarijkeZondag 29:c72e49a3edbb 77 double emg0_raw = emg0_in.read(); //give name to raw EMG0 data
MarijkeZondag 28:4534a096b677 78 double emg0_filt_x = emg0filter.step(emg0_raw); //Use biquad chain to filter raw EMG data
MarijkeZondag 29:c72e49a3edbb 79 double emg0_filt = abs(emg0_filt_x); //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch, stel er komt iets raars uit, dan Notch uit de biquad chain halen en aparte chain voor aanmaken.
MarijkeZondag 10:39ec51206c8b 80 }
MarijkeZondag 10:39ec51206c8b 81
MarijkeZondag 10:39ec51206c8b 82 void EMGFilter1()
MarijkeZondag 10:39ec51206c8b 83 {
MarijkeZondag 29:c72e49a3edbb 84 double emg1_raw = emg1_in.read(); //give name to raw EMG1 data
MarijkeZondag 28:4534a096b677 85 double emg1_filt_x = emg1filter.step(emg1_raw); //Use biquad chain to filter raw EMG data
MarijkeZondag 29:c72e49a3edbb 86 double emg1_filt = abs(emg1_filt_x); //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch.
MarijkeZondag 10:39ec51206c8b 87 }
MarijkeZondag 10:39ec51206c8b 88
MarijkeZondag 10:39ec51206c8b 89 void EMGFilter2()
MarijkeZondag 10:39ec51206c8b 90 {
MarijkeZondag 29:c72e49a3edbb 91 double emg2_raw = emg2_in.read(); //Give name to raw EMG1 data
MarijkeZondag 28:4534a096b677 92 double emg2_filt_x = emg2filter.step(emg2_raw); //Use biquad chain to filter raw EMG data
MarijkeZondag 29:c72e49a3edbb 93 double emg2_filt = abs(emg2_filt_x); //Rectifier. LET OP: volgorde filter: band-notch-rectifier.
MarijkeZondag 10:39ec51206c8b 94 }
MarijkeZondag 23:67b1b46e7f6f 95
MarijkeZondag 25:8bb3c013752f 96 void MovAg() //Calculate moving average (MovAg), klopt nog niet!!
MarijkeZondag 13:a3d4b4daf5b4 97 {
MarijkeZondag 26:ff5b080e70c1 98 for (int i = windowsize-1; i>=0; i--) //Make arrays for the last datapoints of the filtered signals
MarijkeZondag 13:a3d4b4daf5b4 99 {
MarijkeZondag 26:ff5b080e70c1 100 StoreArray0[i] = StoreArray0[i-1]; //Shifts the i'th element one place to the right, this makes it "rolling or moving" average.
MarijkeZondag 13:a3d4b4daf5b4 101 StoreArray1[i] = StoreArray1[i-1];
MarijkeZondag 13:a3d4b4daf5b4 102 StoreArray2[i] = StoreArray2[i-1];
MarijkeZondag 13:a3d4b4daf5b4 103 }
MarijkeZondag 13:a3d4b4daf5b4 104
MarijkeZondag 29:c72e49a3edbb 105 StoreArray0[0] = emg0_filt; //Stores the latest datapoint of the filtered signal in the first element of the array
MarijkeZondag 28:4534a096b677 106 StoreArray1[0] = emg1_filt;
MarijkeZondag 28:4534a096b677 107 StoreArray2[0] = emg2_filt;
MarijkeZondag 13:a3d4b4daf5b4 108
MarijkeZondag 13:a3d4b4daf5b4 109 sum1 = 0.0;
MarijkeZondag 13:a3d4b4daf5b4 110 sum2 = 0.0;
MarijkeZondag 13:a3d4b4daf5b4 111 sum3 = 0.0;
MarijkeZondag 13:a3d4b4daf5b4 112
MarijkeZondag 26:ff5b080e70c1 113 for(int a = 0; a<= windowsize-1; a++) //Sums the elements in the arrays
MarijkeZondag 13:a3d4b4daf5b4 114 {
MarijkeZondag 13:a3d4b4daf5b4 115 sum1 += StoreArray0[a];
MarijkeZondag 13:a3d4b4daf5b4 116 sum2 += StoreArray1[a];
MarijkeZondag 13:a3d4b4daf5b4 117 sum3 += StoreArray2[a];
MarijkeZondag 13:a3d4b4daf5b4 118 }
MarijkeZondag 13:a3d4b4daf5b4 119
MarijkeZondag 13:a3d4b4daf5b4 120 movAg0 = sum1/windowsize; //calculates an average in the array
MarijkeZondag 13:a3d4b4daf5b4 121 movAg1 = sum2/windowsize;
MarijkeZondag 14:fa09dae67390 122 movAg2 = sum3/windowsize;
MarijkeZondag 22:8e61050064a9 123 //serial getallen sturen, als het 1 getal is gaat hier wat fout, als het een reeks is dan gaat er bij de input naar HIDscope wat fout.
MarijkeZondag 13:a3d4b4daf5b4 124 }
MarijkeZondag 13:a3d4b4daf5b4 125
MarijkeZondag 13:a3d4b4daf5b4 126 void emg_filtered() //Call all filter functions
MarijkeZondag 12:eaed305a76c3 127 {
MarijkeZondag 12:eaed305a76c3 128 EMGFilter0();
MarijkeZondag 12:eaed305a76c3 129 EMGFilter1();
MarijkeZondag 12:eaed305a76c3 130 EMGFilter2();
MarijkeZondag 12:eaed305a76c3 131 }
MarijkeZondag 12:eaed305a76c3 132
MarijkeZondag 24:6cde78e776b6 133
vsluiter 0:c8f15874531b 134 int main()
MarijkeZondag 22:8e61050064a9 135 {
MarijkeZondag 29:c72e49a3edbb 136 //pc.baud(115200);
MarijkeZondag 29:c72e49a3edbb 137 //pc.printf("Hello World!\r\n"); //Serial communication only works if hidscope is turned off.
MarijkeZondag 29:c72e49a3edbb 138 myled == 0; //Does the code work?
MarijkeZondag 24:6cde78e776b6 139
MarijkeZondag 24:6cde78e776b6 140 emg0filter.add( &emg0band1 ).add( &emg0band2 ).add( &emg0band3 ).add( &notch1 ); //attach biquad elements to chain
MarijkeZondag 24:6cde78e776b6 141 emg1filter.add( &emg1band1 ).add( &emg1band2 ).add( &emg1band3 ).add( &notch2 );
MarijkeZondag 24:6cde78e776b6 142 emg2filter.add( &emg2band1 ).add( &emg2band2 ).add( &emg2band3 ).add( &notch3 );
MarijkeZondag 22:8e61050064a9 143
MarijkeZondag 25:8bb3c013752f 144 filter_tick.attach(&emg_filtered,T); //EMG signals filtered every T sec.
MarijkeZondag 25:8bb3c013752f 145 MovAg_tick.attach(&MovAg,T); //Moving average calculation every T sec.
MarijkeZondag 24:6cde78e776b6 146 HIDScope_tick.attach(&HIDScope_sample,T); //EMG signals raw + filtered to HIDScope every T sec.
MarijkeZondag 10:39ec51206c8b 147
MarijkeZondag 29:c72e49a3edbb 148 while(1){}
vsluiter 0:c8f15874531b 149 }