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@25:8bb3c013752f, 2018-10-23 (annotated)

Committer:
MarijkeZondag
Date:
Tue Oct 23 12:02:18 2018 +0000
Revision:
25:8bb3c013752f
Parent:
24:6cde78e776b6
Child:
26:ff5b080e70c1
Zie vorige bericht + ticker moving average attached aan movag functie. MovAg functie uit de algehele emg-filter-functie gehaald. Dan kunnen ze los van elkaar getest 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 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 22:8e61050064a9 79
MarijkeZondag 22:8e61050064a9 80
MarijkeZondag 23:67b1b46e7f6f 81 scope.send(); //Send data to HIDScope server
MarijkeZondag 19:466ada92bf65 82 }
MarijkeZondag 19:466ada92bf65 83
MarijkeZondag 10:39ec51206c8b 84 void EMGFilter0()
MarijkeZondag 24:6cde78e776b6 85 {
MarijkeZondag 24:6cde78e776b6 86 double emg0_raw = emg0_in.read(); //give name to raw EMG0 data
MarijkeZondag 24:6cde78e776b6 87 double emg0filt = emg0filter.step(emg0); //Use biquad chain to filter raw EMG data
MarijkeZondag 24:6cde78e776b6 88 double emg0filt = abs(emg0filt); //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch.
MarijkeZondag 10:39ec51206c8b 89 }
MarijkeZondag 10:39ec51206c8b 90
MarijkeZondag 10:39ec51206c8b 91 void EMGFilter1()
MarijkeZondag 10:39ec51206c8b 92 {
MarijkeZondag 24:6cde78e776b6 93 double emg1_raw = emg1_in.read(); //give name to raw EMG1 data
MarijkeZondag 24:6cde78e776b6 94 double emg1filt = emg1filter.step(emg1); //Use biquad chain to filter raw EMG data
MarijkeZondag 24:6cde78e776b6 95 double emg1filt = abs(emg1filt); //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch.
MarijkeZondag 10:39ec51206c8b 96 }
MarijkeZondag 10:39ec51206c8b 97
MarijkeZondag 10:39ec51206c8b 98 void EMGFilter2()
MarijkeZondag 10:39ec51206c8b 99 {
MarijkeZondag 24:6cde78e776b6 100 double emg2_raw = emg2_in.read(); //give name to raw EMG1 data
MarijkeZondag 24:6cde78e776b6 101 double emg2filt = emg2filter.step(emg2); //Use biquad chain to filter raw EMG data
MarijkeZondag 24:6cde78e776b6 102 double emg2filt = abs(emg2filt); //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch.
MarijkeZondag 10:39ec51206c8b 103 }
MarijkeZondag 23:67b1b46e7f6f 104
MarijkeZondag 25:8bb3c013752f 105 void MovAg() //Calculate moving average (MovAg), klopt nog niet!!
MarijkeZondag 13:a3d4b4daf5b4 106 {
MarijkeZondag 13:a3d4b4daf5b4 107 for (int i = windowsize-1; i>=0; i--) //Make array of the last datapoints of the filtered signal
MarijkeZondag 13:a3d4b4daf5b4 108 {
MarijkeZondag 13:a3d4b4daf5b4 109 StoreArray0[i] = StoreArray0[i-1];
MarijkeZondag 13:a3d4b4daf5b4 110 StoreArray1[i] = StoreArray1[i-1];
MarijkeZondag 13:a3d4b4daf5b4 111 StoreArray2[i] = StoreArray2[i-1];
MarijkeZondag 13:a3d4b4daf5b4 112 }
MarijkeZondag 13:a3d4b4daf5b4 113
MarijkeZondag 24:6cde78e776b6 114 StoreArray0[0] = emgfilt0; //Stores the latest datapoint in the first element of the array
MarijkeZondag 24:6cde78e776b6 115 StoreArray1[0] = emgfilt1;
MarijkeZondag 24:6cde78e776b6 116 StoreArray2[0] = emgfilt2;
MarijkeZondag 13:a3d4b4daf5b4 117
MarijkeZondag 13:a3d4b4daf5b4 118 sum1 = 0.0;
MarijkeZondag 13:a3d4b4daf5b4 119 sum2 = 0.0;
MarijkeZondag 13:a3d4b4daf5b4 120 sum3 = 0.0;
MarijkeZondag 13:a3d4b4daf5b4 121
MarijkeZondag 13:a3d4b4daf5b4 122 for(int a = 0; a<= windowsize-1; a++) //Sum the elements in the array
MarijkeZondag 13:a3d4b4daf5b4 123 {
MarijkeZondag 13:a3d4b4daf5b4 124 sum1 += StoreArray0[a];
MarijkeZondag 13:a3d4b4daf5b4 125 sum2 += StoreArray1[a];
MarijkeZondag 13:a3d4b4daf5b4 126 sum3 += StoreArray2[a];
MarijkeZondag 13:a3d4b4daf5b4 127 }
MarijkeZondag 13:a3d4b4daf5b4 128
MarijkeZondag 13:a3d4b4daf5b4 129 movAg0 = sum1/windowsize; //calculates an average in the array
MarijkeZondag 13:a3d4b4daf5b4 130 movAg1 = sum2/windowsize;
MarijkeZondag 14:fa09dae67390 131 movAg2 = sum3/windowsize;
MarijkeZondag 22:8e61050064a9 132 //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 133 }
MarijkeZondag 13:a3d4b4daf5b4 134
MarijkeZondag 13:a3d4b4daf5b4 135 void emg_filtered() //Call all filter functions
MarijkeZondag 12:eaed305a76c3 136 {
MarijkeZondag 12:eaed305a76c3 137 EMGFilter0();
MarijkeZondag 12:eaed305a76c3 138 EMGFilter1();
MarijkeZondag 12:eaed305a76c3 139 EMGFilter2();
MarijkeZondag 12:eaed305a76c3 140 }
MarijkeZondag 12:eaed305a76c3 141
MarijkeZondag 24:6cde78e776b6 142
MarijkeZondag 8:895d941a5910 143
vsluiter 0:c8f15874531b 144 int main()
MarijkeZondag 22:8e61050064a9 145 {
MarijkeZondag 22:8e61050064a9 146 pc.baud(115200);
MarijkeZondag 24:6cde78e776b6 147 pc.printf("hello\n\r"); //Check, does it work?
MarijkeZondag 24:6cde78e776b6 148
MarijkeZondag 24:6cde78e776b6 149 emg0filter.add( &emg0band1 ).add( &emg0band2 ).add( &emg0band3 ).add( &notch1 ); //attach biquad elements to chain
MarijkeZondag 24:6cde78e776b6 150 emg1filter.add( &emg1band1 ).add( &emg1band2 ).add( &emg1band3 ).add( &notch2 );
MarijkeZondag 24:6cde78e776b6 151 emg2filter.add( &emg2band1 ).add( &emg2band2 ).add( &emg2band3 ).add( &notch3 );
MarijkeZondag 22:8e61050064a9 152
MarijkeZondag 22:8e61050064a9 153 while(true)
MarijkeZondag 22:8e61050064a9 154 {
MarijkeZondag 6:f4bbb73f3989 155
MarijkeZondag 25:8bb3c013752f 156 filter_tick.attach(&emg_filtered,T); //EMG signals filtered every T sec.
MarijkeZondag 25:8bb3c013752f 157 MovAg_tick.attach(&MovAg,T); //Moving average calculation every T sec.
MarijkeZondag 24:6cde78e776b6 158 HIDScope_tick.attach(&HIDScope_sample,T); //EMG signals raw + filtered to HIDScope every T sec.
MarijkeZondag 24:6cde78e776b6 159
MarijkeZondag 22:8e61050064a9 160 pc.printf("\n\r Moving average EMG 3 is: %d \n\r",movAg2);
MarijkeZondag 10:39ec51206c8b 161
MarijkeZondag 22:8e61050064a9 162 }
vsluiter 0:c8f15874531b 163 }