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@24:6cde78e776b6, 2018-10-23 (annotated)

Committer:
MarijkeZondag
Date:
Tue Oct 23 11:58:42 2018 +0000
Revision:
24:6cde78e776b6
Parent:
23:67b1b46e7f6f
Child:
25:8bb3c013752f
Filter op een andere manier erin gezet. Biquad chain elements attached, mogelijk ging het daar vorige keer vast. Scope gezet zodat ook filtered data gestuurd wordt. MovAg moet nog naar gekeken worden.

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 10:39ec51206c8b 16
MarijkeZondag 16:5f7196ddc77b 17 InterruptIn button1 (D10); //Let op, is deze niet bezet? En 11? Even checken, als er een error komt, kan het hier zitten.
MarijkeZondag 22:8e61050064a9 18 InterruptIn button2 (D11); //Buttons for switch calibration states
MarijkeZondag 6:f4bbb73f3989 19
MarijkeZondag 22:8e61050064a9 20 DigitalOut ledr (LED_RED); //LEDs to show in which state you are in
MarijkeZondag 17:741798018c0d 21 DigitalOut ledb (LED_BLUE);
MarijkeZondag 17:741798018c0d 22 DigitalOut ledg (LED_GREEN);
MarijkeZondag 13:a3d4b4daf5b4 23
MarijkeZondag 22:8e61050064a9 24 MODSERIAL pc(USBTX, USBRX); //Serial communication to see if the code works step by step
MarijkeZondag 9:c722418997b5 25
MarijkeZondag 24:6cde78e776b6 26 HIDScope scope( 6 ); //HIDScope set to 3x2 channels for 3 muscles
vsluiter 0:c8f15874531b 27
MarijkeZondag 19:466ada92bf65 28 //HIDscope
MarijkeZondag 24:6cde78e776b6 29 Ticker HIDScope_tick; //Ticker for HIDScope
MarijkeZondag 22:8e61050064a9 30 Ticker filter_tick; //Ticker for EMG filter
MarijkeZondag 22:8e61050064a9 31 Ticker MovAg_tick; //Ticker to calculate Moving Average
MarijkeZondag 9:c722418997b5 32
MarijkeZondag 9:c722418997b5 33 //Global variables
MarijkeZondag 22:8e61050064a9 34 int encoder = 0; //Starting point encoder (wordt nu nog niet gebruikt in de code)
MarijkeZondag 24:6cde78e776b6 35 const float T = 0.002f; //Ticker period
MarijkeZondag 10:39ec51206c8b 36
MarijkeZondag 13:a3d4b4daf5b4 37 //EMG filter
MarijkeZondag 24:6cde78e776b6 38 double emgfilt0, emgfilt1, emgfilt2; //Variables for filtered EMG data channel 0, 1 and 2
MarijkeZondag 22:8e61050064a9 39 const int windowsize = 150; //Size of the array over which the moving average (MovAg) is calculated. (random number)
MarijkeZondag 13:a3d4b4daf5b4 40 double sum, sum1, sum2, sum3; //variables used to sum elements in array
MarijkeZondag 22:8e61050064a9 41 double StoreArray0[windowsize], StoreArray1[windowsize], StoreArray2[windowsize]; //Empty arrays to calculate MoveAg
MarijkeZondag 22:8e61050064a9 42 double movAg0,movAg1,movAg2; //outcome of MovAg (moet dit een array zijn??)
MarijkeZondag 13:a3d4b4daf5b4 43
MarijkeZondag 24:6cde78e776b6 44
MarijkeZondag 22:8e61050064a9 45 //Biquad //Variables for the biquad band filters (alle 3 dezelfde maar je kan niet 3x 'emg0band' aanroepen ofzo)
MarijkeZondag 24:6cde78e776b6 46 BiQuadChain emg0filter;
MarijkeZondag 10:39ec51206c8b 47 BiQuad emg0band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
MarijkeZondag 10:39ec51206c8b 48 BiQuad emg0band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
MarijkeZondag 10:39ec51206c8b 49 BiQuad emg0band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
MarijkeZondag 24:6cde78e776b6 50 BiQuad notch1( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter biquad coefficients
MarijkeZondag 10:39ec51206c8b 51
MarijkeZondag 24:6cde78e776b6 52 BiQuadChain emg1filter;
MarijkeZondag 10:39ec51206c8b 53 BiQuad emg1band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
MarijkeZondag 10:39ec51206c8b 54 BiQuad emg1band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
MarijkeZondag 10:39ec51206c8b 55 BiQuad emg1band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
MarijkeZondag 24:6cde78e776b6 56 BiQuad notch2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter
MarijkeZondag 10:39ec51206c8b 57
MarijkeZondag 24:6cde78e776b6 58 BiQuadChain emg2filter;
MarijkeZondag 10:39ec51206c8b 59 BiQuad emg2band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
MarijkeZondag 10:39ec51206c8b 60 BiQuad emg2band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
MarijkeZondag 10:39ec51206c8b 61 BiQuad emg2band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
MarijkeZondag 14:fa09dae67390 62 BiQuad notch3( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter
MarijkeZondag 10:39ec51206c8b 63
MarijkeZondag 22:8e61050064a9 64 //Functions
MarijkeZondag 10:39ec51206c8b 65
MarijkeZondag 24:6cde78e776b6 66 void HIDScope_sample()
MarijkeZondag 19:466ada92bf65 67 {
MarijkeZondag 19:466ada92bf65 68 /* 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 69 scope.set(0,emg0_raw);
MarijkeZondag 24:6cde78e776b6 70 scope.set(1,emg0_filt);
MarijkeZondag 24:6cde78e776b6 71 scope.set(2,emg1_raw);
MarijkeZondag 24:6cde78e776b6 72 scope.set(3,emg1_filt);
MarijkeZondag 24:6cde78e776b6 73 scope.set(4,emg2_raw);
MarijkeZondag 24:6cde78e776b6 74 scope.set(5,emg2_filt);
MarijkeZondag 22:8e61050064a9 75
MarijkeZondag 23:67b1b46e7f6f 76 //scope.set(0, movAg0.read() ); Werkt niet!! Hoe dan wel moving average in HIDScope??
MarijkeZondag 22:8e61050064a9 77
MarijkeZondag 23:67b1b46e7f6f 78 scope.send(); //Send data to HIDScope server
MarijkeZondag 19:466ada92bf65 79 }
MarijkeZondag 19:466ada92bf65 80
MarijkeZondag 10:39ec51206c8b 81 void EMGFilter0()
MarijkeZondag 24:6cde78e776b6 82 {
MarijkeZondag 24:6cde78e776b6 83 double emg0_raw = emg0_in.read(); //give name to raw EMG0 data
MarijkeZondag 24:6cde78e776b6 84 double emg0filt = emg0filter.step(emg0); //Use biquad chain to filter raw EMG data
MarijkeZondag 24:6cde78e776b6 85 double emg0filt = abs(emg0filt); //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch.
MarijkeZondag 10:39ec51206c8b 86 }
MarijkeZondag 10:39ec51206c8b 87
MarijkeZondag 10:39ec51206c8b 88 void EMGFilter1()
MarijkeZondag 10:39ec51206c8b 89 {
MarijkeZondag 24:6cde78e776b6 90 double emg1_raw = emg1_in.read(); //give name to raw EMG1 data
MarijkeZondag 24:6cde78e776b6 91 double emg1filt = emg1filter.step(emg1); //Use biquad chain to filter raw EMG data
MarijkeZondag 24:6cde78e776b6 92 double emg1filt = abs(emg1filt); //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch.
MarijkeZondag 10:39ec51206c8b 93 }
MarijkeZondag 10:39ec51206c8b 94
MarijkeZondag 10:39ec51206c8b 95 void EMGFilter2()
MarijkeZondag 10:39ec51206c8b 96 {
MarijkeZondag 24:6cde78e776b6 97 double emg2_raw = emg2_in.read(); //give name to raw EMG1 data
MarijkeZondag 24:6cde78e776b6 98 double emg2filt = emg2filter.step(emg2); //Use biquad chain to filter raw EMG data
MarijkeZondag 24:6cde78e776b6 99 double emg2filt = abs(emg2filt); //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch.
MarijkeZondag 10:39ec51206c8b 100 }
MarijkeZondag 23:67b1b46e7f6f 101
MarijkeZondag 23:67b1b46e7f6f 102 void MovAg() //Calculate moving average (MovAg)
MarijkeZondag 13:a3d4b4daf5b4 103 {
MarijkeZondag 13:a3d4b4daf5b4 104 for (int i = windowsize-1; i>=0; i--) //Make array of the last datapoints of the filtered signal
MarijkeZondag 13:a3d4b4daf5b4 105 {
MarijkeZondag 13:a3d4b4daf5b4 106 StoreArray0[i] = StoreArray0[i-1];
MarijkeZondag 13:a3d4b4daf5b4 107 StoreArray1[i] = StoreArray1[i-1];
MarijkeZondag 13:a3d4b4daf5b4 108 StoreArray2[i] = StoreArray2[i-1];
MarijkeZondag 13:a3d4b4daf5b4 109 }
MarijkeZondag 13:a3d4b4daf5b4 110
MarijkeZondag 24:6cde78e776b6 111 StoreArray0[0] = emgfilt0; //Stores the latest datapoint in the first element of the array
MarijkeZondag 24:6cde78e776b6 112 StoreArray1[0] = emgfilt1;
MarijkeZondag 24:6cde78e776b6 113 StoreArray2[0] = emgfilt2;
MarijkeZondag 13:a3d4b4daf5b4 114
MarijkeZondag 13:a3d4b4daf5b4 115 sum1 = 0.0;
MarijkeZondag 13:a3d4b4daf5b4 116 sum2 = 0.0;
MarijkeZondag 13:a3d4b4daf5b4 117 sum3 = 0.0;
MarijkeZondag 13:a3d4b4daf5b4 118
MarijkeZondag 13:a3d4b4daf5b4 119 for(int a = 0; a<= windowsize-1; a++) //Sum the elements in the array
MarijkeZondag 13:a3d4b4daf5b4 120 {
MarijkeZondag 13:a3d4b4daf5b4 121 sum1 += StoreArray0[a];
MarijkeZondag 13:a3d4b4daf5b4 122 sum2 += StoreArray1[a];
MarijkeZondag 13:a3d4b4daf5b4 123 sum3 += StoreArray2[a];
MarijkeZondag 13:a3d4b4daf5b4 124 }
MarijkeZondag 13:a3d4b4daf5b4 125
MarijkeZondag 13:a3d4b4daf5b4 126 movAg0 = sum1/windowsize; //calculates an average in the array
MarijkeZondag 13:a3d4b4daf5b4 127 movAg1 = sum2/windowsize;
MarijkeZondag 14:fa09dae67390 128 movAg2 = sum3/windowsize;
MarijkeZondag 22:8e61050064a9 129 //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 130 }
MarijkeZondag 13:a3d4b4daf5b4 131
MarijkeZondag 13:a3d4b4daf5b4 132 void emg_filtered() //Call all filter functions
MarijkeZondag 12:eaed305a76c3 133 {
MarijkeZondag 12:eaed305a76c3 134 EMGFilter0();
MarijkeZondag 12:eaed305a76c3 135 EMGFilter1();
MarijkeZondag 12:eaed305a76c3 136 EMGFilter2();
MarijkeZondag 13:a3d4b4daf5b4 137 MovAg();
MarijkeZondag 12:eaed305a76c3 138 }
MarijkeZondag 12:eaed305a76c3 139
MarijkeZondag 24:6cde78e776b6 140
MarijkeZondag 8:895d941a5910 141
vsluiter 0:c8f15874531b 142 int main()
MarijkeZondag 22:8e61050064a9 143 {
MarijkeZondag 22:8e61050064a9 144 pc.baud(115200);
MarijkeZondag 24:6cde78e776b6 145 pc.printf("hello\n\r"); //Check, does it work?
MarijkeZondag 24:6cde78e776b6 146
MarijkeZondag 24:6cde78e776b6 147 emg0filter.add( &emg0band1 ).add( &emg0band2 ).add( &emg0band3 ).add( &notch1 ); //attach biquad elements to chain
MarijkeZondag 24:6cde78e776b6 148 emg1filter.add( &emg1band1 ).add( &emg1band2 ).add( &emg1band3 ).add( &notch2 );
MarijkeZondag 24:6cde78e776b6 149 emg2filter.add( &emg2band1 ).add( &emg2band2 ).add( &emg2band3 ).add( &notch3 );
MarijkeZondag 22:8e61050064a9 150
MarijkeZondag 22:8e61050064a9 151 while(true)
MarijkeZondag 22:8e61050064a9 152 {
MarijkeZondag 6:f4bbb73f3989 153
MarijkeZondag 22:8e61050064a9 154 filter_tick.attach(&emg_filtered,T); //EMG signals filtered + moving average every T sec.
MarijkeZondag 24:6cde78e776b6 155 HIDScope_tick.attach(&HIDScope_sample,T); //EMG signals raw + filtered to HIDScope every T sec.
MarijkeZondag 24:6cde78e776b6 156
MarijkeZondag 22:8e61050064a9 157 pc.printf("\n\r Moving average EMG 3 is: %d \n\r",movAg2);
MarijkeZondag 10:39ec51206c8b 158
MarijkeZondag 22:8e61050064a9 159 }
vsluiter 0:c8f15874531b 160 }