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@26:ff5b080e70c1, 2018-10-24 (annotated)

Committer:
MarijkeZondag
Date:
Wed Oct 24 17:31:44 2018 +0000
Revision:
26:ff5b080e70c1
Parent:
25:8bb3c013752f
Child:
27:1885e388b4ce
Controle 1

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 25:8bb3c013752f 71 //scope.set(1,movAg0); //als moving average werkt
MarijkeZondag 24:6cde78e776b6 72 scope.set(2,emg1_raw);
MarijkeZondag 24:6cde78e776b6 73 scope.set(3,emg1_filt);
MarijkeZondag 25:8bb3c013752f 74 //scope.set(3,movAg1); //als moving average werkt
MarijkeZondag 24:6cde78e776b6 75 scope.set(4,emg2_raw);
MarijkeZondag 24:6cde78e776b6 76 scope.set(5,emg2_filt);
MarijkeZondag 25:8bb3c013752f 77 //scope.set(5,movAg2); //als moving average werkt
MarijkeZondag 25:8bb3c013752f 78
MarijkeZondag 23:67b1b46e7f6f 79 scope.send(); //Send data to HIDScope server
MarijkeZondag 19:466ada92bf65 80 }
MarijkeZondag 19:466ada92bf65 81
MarijkeZondag 10:39ec51206c8b 82 void EMGFilter0()
MarijkeZondag 24:6cde78e776b6 83 {
MarijkeZondag 24:6cde78e776b6 84 double emg0_raw = emg0_in.read(); //give name to raw EMG0 data
MarijkeZondag 24:6cde78e776b6 85 double emg0filt = emg0filter.step(emg0); //Use biquad chain to filter raw EMG data
MarijkeZondag 26:ff5b080e70c1 86 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 87 }
MarijkeZondag 10:39ec51206c8b 88
MarijkeZondag 10:39ec51206c8b 89 void EMGFilter1()
MarijkeZondag 10:39ec51206c8b 90 {
MarijkeZondag 24:6cde78e776b6 91 double emg1_raw = emg1_in.read(); //give name to raw EMG1 data
MarijkeZondag 24:6cde78e776b6 92 double emg1filt = emg1filter.step(emg1); //Use biquad chain to filter raw EMG data
MarijkeZondag 24:6cde78e776b6 93 double emg1filt = abs(emg1filt); //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch.
MarijkeZondag 10:39ec51206c8b 94 }
MarijkeZondag 10:39ec51206c8b 95
MarijkeZondag 10:39ec51206c8b 96 void EMGFilter2()
MarijkeZondag 10:39ec51206c8b 97 {
MarijkeZondag 26:ff5b080e70c1 98 double emg2_raw = emg2_in.read(); //Give name to raw EMG1 data
MarijkeZondag 24:6cde78e776b6 99 double emg2filt = emg2filter.step(emg2); //Use biquad chain to filter raw EMG data
MarijkeZondag 26:ff5b080e70c1 100 double emg2filt = abs(emg2filt); //Rectifier. LET OP: volgorde filter: band-notch-rectifier.
MarijkeZondag 10:39ec51206c8b 101 }
MarijkeZondag 23:67b1b46e7f6f 102
MarijkeZondag 25:8bb3c013752f 103 void MovAg() //Calculate moving average (MovAg), klopt nog niet!!
MarijkeZondag 13:a3d4b4daf5b4 104 {
MarijkeZondag 26:ff5b080e70c1 105 for (int i = windowsize-1; i>=0; i--) //Make arrays for the last datapoints of the filtered signals
MarijkeZondag 13:a3d4b4daf5b4 106 {
MarijkeZondag 26:ff5b080e70c1 107 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 108 StoreArray1[i] = StoreArray1[i-1];
MarijkeZondag 13:a3d4b4daf5b4 109 StoreArray2[i] = StoreArray2[i-1];
MarijkeZondag 13:a3d4b4daf5b4 110 }
MarijkeZondag 13:a3d4b4daf5b4 111
MarijkeZondag 26:ff5b080e70c1 112 StoreArray0[0] = emg0filt; //Stores the latest datapoint of the filtered signal in the first element of the array
MarijkeZondag 26:ff5b080e70c1 113 StoreArray1[0] = emg1filt;
MarijkeZondag 26:ff5b080e70c1 114 StoreArray2[0] = emg2filt;
MarijkeZondag 13:a3d4b4daf5b4 115
MarijkeZondag 13:a3d4b4daf5b4 116 sum1 = 0.0;
MarijkeZondag 13:a3d4b4daf5b4 117 sum2 = 0.0;
MarijkeZondag 13:a3d4b4daf5b4 118 sum3 = 0.0;
MarijkeZondag 13:a3d4b4daf5b4 119
MarijkeZondag 26:ff5b080e70c1 120 for(int a = 0; a<= windowsize-1; a++) //Sums the elements in the arrays
MarijkeZondag 13:a3d4b4daf5b4 121 {
MarijkeZondag 13:a3d4b4daf5b4 122 sum1 += StoreArray0[a];
MarijkeZondag 13:a3d4b4daf5b4 123 sum2 += StoreArray1[a];
MarijkeZondag 13:a3d4b4daf5b4 124 sum3 += StoreArray2[a];
MarijkeZondag 13:a3d4b4daf5b4 125 }
MarijkeZondag 13:a3d4b4daf5b4 126
MarijkeZondag 13:a3d4b4daf5b4 127 movAg0 = sum1/windowsize; //calculates an average in the array
MarijkeZondag 13:a3d4b4daf5b4 128 movAg1 = sum2/windowsize;
MarijkeZondag 14:fa09dae67390 129 movAg2 = sum3/windowsize;
MarijkeZondag 22:8e61050064a9 130 //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 131 }
MarijkeZondag 13:a3d4b4daf5b4 132
MarijkeZondag 13:a3d4b4daf5b4 133 void emg_filtered() //Call all filter functions
MarijkeZondag 12:eaed305a76c3 134 {
MarijkeZondag 12:eaed305a76c3 135 EMGFilter0();
MarijkeZondag 12:eaed305a76c3 136 EMGFilter1();
MarijkeZondag 12:eaed305a76c3 137 EMGFilter2();
MarijkeZondag 12:eaed305a76c3 138 }
MarijkeZondag 12:eaed305a76c3 139
MarijkeZondag 24:6cde78e776b6 140
vsluiter 0:c8f15874531b 141 int main()
MarijkeZondag 22:8e61050064a9 142 {
MarijkeZondag 22:8e61050064a9 143 pc.baud(115200);
MarijkeZondag 24:6cde78e776b6 144 pc.printf("hello\n\r"); //Check, does it work?
MarijkeZondag 24:6cde78e776b6 145
MarijkeZondag 24:6cde78e776b6 146 emg0filter.add( &emg0band1 ).add( &emg0band2 ).add( &emg0band3 ).add( &notch1 ); //attach biquad elements to chain
MarijkeZondag 24:6cde78e776b6 147 emg1filter.add( &emg1band1 ).add( &emg1band2 ).add( &emg1band3 ).add( &notch2 );
MarijkeZondag 24:6cde78e776b6 148 emg2filter.add( &emg2band1 ).add( &emg2band2 ).add( &emg2band3 ).add( &notch3 );
MarijkeZondag 22:8e61050064a9 149
MarijkeZondag 22:8e61050064a9 150 while(true)
MarijkeZondag 22:8e61050064a9 151 {
MarijkeZondag 6:f4bbb73f3989 152
MarijkeZondag 25:8bb3c013752f 153 filter_tick.attach(&emg_filtered,T); //EMG signals filtered every T sec.
MarijkeZondag 25:8bb3c013752f 154 MovAg_tick.attach(&MovAg,T); //Moving average calculation 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 }