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@27:1885e388b4ce, 2018-10-24 (annotated)

Committer:
MarijkeZondag
Date:
Wed Oct 24 17:39:24 2018 +0000
Revision:
27:1885e388b4ce
Parent:
26:ff5b080e70c1
Child:
28:4534a096b677
Encoder troep eruit gehaald, eerst EMG los

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