Marjolein Scheffers / Mbed 2 deprecated gr14_motor_HidScope

emg trial

Dependencies:   FastPWM HIDScope MODSERIAL mbed biquadFilter

main.cpp@8:39519b33f767, 2018-10-30 (annotated)

Committer:
s1680897
Date:
Tue Oct 30 14:20:49 2018 +0000
Revision:
8:39519b33f767
Parent:
7:d72cbd7055af 
hjhlkj

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s1680897 6:51c87d04371d 1 #include "mbed.h"
s1680897 1:e81ca29ae626 2 #include "HIDScope.h"
s1680897 0:94b5c70f818b 3 #include "MODSERIAL.h"
s1680897 2:0189c5170834 4 #include "BiQuad.h"
s1680897 0:94b5c70f818b 5
s1680897 6:51c87d04371d 6
s1680897 1:e81ca29ae626 7 // Two analog inputs to read from
s1680897 1:e81ca29ae626 8 AnalogIn a0(A0);
s1680897 1:e81ca29ae626 9 AnalogIn a1(A1);
s1680897 6:51c87d04371d 10 AnalogIn a2(A2);
s1680897 6:51c87d04371d 11 AnalogIn a3(A3);
s1680897 6:51c87d04371d 12 DigitalOut ledr(LED_RED);
s1680897 0:94b5c70f818b 13 MODSERIAL pc(USBTX, USBRX);
s1680897 0:94b5c70f818b 14
s1680897 0:94b5c70f818b 15
s1680897 1:e81ca29ae626 16 // Define the HIDScope and Ticker objects
s1680897 8:39519b33f767 17 HIDScope scope(2);
s1680897 1:e81ca29ae626 18 Ticker scopeTimer;
s1680897 2:0189c5170834 19 Ticker EMGTicker;
s1680897 2:0189c5170834 20
s1680897 6:51c87d04371d 21 // BiQuad filters
s1680897 6:51c87d04371d 22 //BiQuad Chains
s1680897 6:51c87d04371d 23 BiQuadChain bqc1;
s1680897 6:51c87d04371d 24 BiQuadChain bqc2;
s1680897 6:51c87d04371d 25 BiQuadChain bqc3;
s1680897 6:51c87d04371d 26 BiQuadChain bqc4;
s1680897 6:51c87d04371d 27
s1680897 6:51c87d04371d 28 //High pass filters 20 Hz
s1680897 6:51c87d04371d 29 BiQuad HP_emg1(1,-2,1,-1.142980502539901,0.412801598096189);
s1680897 6:51c87d04371d 30 BiQuad HP_emg2(1,-2,1,-1.142980502539901,0.412801598096189);
s1680897 6:51c87d04371d 31 BiQuad HP_emg3(1,-2,1,-1.142980502539901,0.412801598096189);
s1680897 6:51c87d04371d 32 BiQuad HP_emg4(1,-2,1,-1.142980502539901,0.412801598096189);
s1680897 6:51c87d04371d 33
s1680897 6:51c87d04371d 34 //Notch filters 50 Hz
s1680897 6:51c87d04371d 35 BiQuad Notch_emg1(1,-1.225251386743515e-16,1,-1.187919512117619e-16,0.939062505817492);
s1680897 6:51c87d04371d 36 BiQuad Notch_emg2(1,-1.225251386743515e-16,1,-1.187919512117619e-16,0.939062505817492);
s1680897 6:51c87d04371d 37 BiQuad Notch_emg3(1,-1.225251386743515e-16,1,-1.187919512117619e-16,0.939062505817492);
s1680897 6:51c87d04371d 38 BiQuad Notch_emg4(1,-1.225251386743515e-16,1,-1.187919512117619e-16,0.939062505817492);
s1680897 6:51c87d04371d 39
s1680897 2:0189c5170834 40
s1680897 4:bd4dd411dc7b 41 // Global variables
s1680897 6:51c87d04371d 42 float EMGdata1;
s1680897 6:51c87d04371d 43 float EMGdata2;
s1680897 6:51c87d04371d 44 float EMGdata3;
s1680897 6:51c87d04371d 45 float EMGdata4;
s1680897 4:bd4dd411dc7b 46 int count;
s1680897 2:0189c5170834 47
s1680897 2:0189c5170834 48 void ReadEMG()
s1680897 6:51c87d04371d 49 {
s1680897 4:bd4dd411dc7b 50 EMGdata1 = a0.read(); // store values in the scope
s1680897 4:bd4dd411dc7b 51 EMGdata2 = a1.read();
s1680897 6:51c87d04371d 52 EMGdata3 = a2.read();
s1680897 6:51c87d04371d 53 EMGdata4 = a3.read();
s1680897 6:51c87d04371d 54 }
s1680897 6:51c87d04371d 55
s1680897 6:51c87d04371d 56 // EMG High pass filters
s1680897 6:51c87d04371d 57 float EMG_HP1(float EMGdata) //data 1
s1680897 6:51c87d04371d 58 {
s1680897 6:51c87d04371d 59 float HP_abs_EMGdata = bqc1.step(EMGdata);
s1680897 6:51c87d04371d 60
s1680897 6:51c87d04371d 61 return fabs(HP_abs_EMGdata);
s1680897 2:0189c5170834 62 }
s1680897 2:0189c5170834 63
s1680897 6:51c87d04371d 64 float EMG_HP2(float EMGdata) //data 2
s1680897 6:51c87d04371d 65 {
s1680897 6:51c87d04371d 66 float HP_abs_EMGdata = bqc2.step(EMGdata);
s1680897 6:51c87d04371d 67
s1680897 6:51c87d04371d 68 return fabs(HP_abs_EMGdata);
s1680897 6:51c87d04371d 69 }
s1680897 6:51c87d04371d 70
s1680897 6:51c87d04371d 71 float EMG_HP3(float EMGdata) //data 3
s1680897 1:e81ca29ae626 72 {
s1680897 6:51c87d04371d 73 float HP_abs_EMGdata = bqc3.step(EMGdata);
s1680897 6:51c87d04371d 74
s1680897 6:51c87d04371d 75 return fabs(HP_abs_EMGdata);
s1680897 6:51c87d04371d 76 }
s1680897 6:51c87d04371d 77
s1680897 6:51c87d04371d 78 float EMG_HP4(float EMGdata) // data 4
s1680897 6:51c87d04371d 79 {
s1680897 6:51c87d04371d 80 float HP_abs_EMGdata = bqc4.step(EMGdata);
s1680897 6:51c87d04371d 81
s1680897 4:bd4dd411dc7b 82 return fabs(HP_abs_EMGdata);
s1680897 4:bd4dd411dc7b 83 }
s1680897 4:bd4dd411dc7b 84
s1680897 8:39519b33f767 85 // EMG moving filter
s1680897 7:d72cbd7055af 86 float EMG_mean (float EMGarray[100])
s1680897 6:51c87d04371d 87 {
s1680897 6:51c87d04371d 88 float sum = 0.0;
s1680897 6:51c87d04371d 89
s1680897 7:d72cbd7055af 90 for(int j=0; j<100; j++)
s1680897 6:51c87d04371d 91 {
s1680897 6:51c87d04371d 92 sum += EMGarray[j];
s1680897 6:51c87d04371d 93 }
s1680897 6:51c87d04371d 94
s1680897 7:d72cbd7055af 95 float EMG_filt = sum / 100.0;
s1680897 6:51c87d04371d 96
s1680897 6:51c87d04371d 97 return EMG_filt;
s1680897 1:e81ca29ae626 98 }
s1680897 0:94b5c70f818b 99
s1680897 6:51c87d04371d 100 // Filtered signal to output between -1 and 1
s1680897 6:51c87d04371d 101 float filt2kin (float EMG_filt1, float EMG_filt2, float max1, float max2)
s1680897 6:51c87d04371d 102 {
s1680897 6:51c87d04371d 103 float EMG_scaled1 = EMG_filt1 / max1;
s1680897 6:51c87d04371d 104 float EMG_scaled2 = EMG_filt2 / max2;
s1680897 6:51c87d04371d 105
s1680897 6:51c87d04371d 106 float kin_input = EMG_scaled2 - EMG_scaled1;
s1680897 6:51c87d04371d 107
s1680897 6:51c87d04371d 108 if (kin_input > 1.0) {
s1680897 5:dd64e0cf20fe 109 kin_input = 1.0;
s1680897 5:dd64e0cf20fe 110 }
s1680897 6:51c87d04371d 111 if (kin_input < -1.0) {
s1680897 5:dd64e0cf20fe 112 kin_input = -1.0;
s1680897 5:dd64e0cf20fe 113 }
s1680897 6:51c87d04371d 114
s1680897 5:dd64e0cf20fe 115 return kin_input;
s1680897 5:dd64e0cf20fe 116 }
s1680897 5:dd64e0cf20fe 117
s1680897 2:0189c5170834 118 void EMG_filtering ()
s1680897 6:51c87d04371d 119 {
s1680897 2:0189c5170834 120 ReadEMG();
s1680897 6:51c87d04371d 121
s1680897 6:51c87d04371d 122 float HP_abs_EMGdata1 = EMG_HP1(EMGdata1);
s1680897 6:51c87d04371d 123 float HP_abs_EMGdata2 = EMG_HP2(EMGdata2);
s1680897 6:51c87d04371d 124 float HP_abs_EMGdata3 = EMG_HP3(EMGdata3);
s1680897 6:51c87d04371d 125 float HP_abs_EMGdata4 = EMG_HP4(EMGdata4);
s1680897 6:51c87d04371d 126
s1680897 6:51c87d04371d 127 static float EMG_array1[100];
s1680897 6:51c87d04371d 128 static float EMG_array2[100];
s1680897 6:51c87d04371d 129 static float EMG_array3[100];
s1680897 6:51c87d04371d 130 static float EMG_array4[100];
s1680897 4:bd4dd411dc7b 131
s1680897 6:51c87d04371d 132 // Fill array 1
s1680897 6:51c87d04371d 133 for(int i = 100-1; i >= 1; i--)
s1680897 6:51c87d04371d 134 {
s1680897 6:51c87d04371d 135 EMG_array1[i] = EMG_array1[i-1];
s1680897 6:51c87d04371d 136 }
s1680897 6:51c87d04371d 137 EMG_array1[0] = HP_abs_EMGdata1;
s1680897 4:bd4dd411dc7b 138
s1680897 6:51c87d04371d 139 // Fill array 2
s1680897 6:51c87d04371d 140 for(int i = 100-1; i >= 1; i--)
s1680897 6:51c87d04371d 141 {
s1680897 6:51c87d04371d 142 EMG_array2[i] = EMG_array2[i-1];
s1680897 6:51c87d04371d 143 }
s1680897 6:51c87d04371d 144 EMG_array2[0] = HP_abs_EMGdata2;
s1680897 6:51c87d04371d 145
s1680897 6:51c87d04371d 146 // Fill array 3
s1680897 6:51c87d04371d 147 for(int i = 100-1; i >= 1; i--)
s1680897 6:51c87d04371d 148 {
s1680897 6:51c87d04371d 149 EMG_array3[i] = EMG_array3[i-1];
s1680897 6:51c87d04371d 150 }
s1680897 6:51c87d04371d 151 EMG_array3[0] = HP_abs_EMGdata3;
s1680897 2:0189c5170834 152
s1680897 6:51c87d04371d 153 // Fill array 4
s1680897 6:51c87d04371d 154 for(int i = 100-1; i >= 1; i--)
s1680897 6:51c87d04371d 155 {
s1680897 6:51c87d04371d 156 EMG_array4[i] = EMG_array4[i-1];
s1680897 6:51c87d04371d 157 }
s1680897 6:51c87d04371d 158 EMG_array4[0] = HP_abs_EMGdata4;
s1680897 6:51c87d04371d 159
s1680897 5:dd64e0cf20fe 160
s1680897 6:51c87d04371d 161 float EMG_filt1 = EMG_mean(EMG_array1);
s1680897 6:51c87d04371d 162 float EMG_filt2 = EMG_mean(EMG_array2);
s1680897 6:51c87d04371d 163 float EMG_filt3 = EMG_mean(EMG_array3);
s1680897 6:51c87d04371d 164 float EMG_filt4 = EMG_mean(EMG_array4);
s1680897 6:51c87d04371d 165
s1680897 8:39519b33f767 166 float max1 = 0.01;
s1680897 8:39519b33f767 167 float max2 = 0.03;
s1680897 8:39519b33f767 168 float max3 = 0.02;
s1680897 8:39519b33f767 169 float max4 = 0.01;
s1680897 6:51c87d04371d 170
s1680897 6:51c87d04371d 171 float kin_input_horizontal = filt2kin (EMG_filt1, EMG_filt2, max1, max2);
s1680897 6:51c87d04371d 172 float kin_input_vertical = filt2kin (EMG_filt3, EMG_filt4, max3, max4);
s1680897 6:51c87d04371d 173
s1680897 8:39519b33f767 174 scope.set(0, kin_input_horizontal);
s1680897 8:39519b33f767 175 scope.set(1, kin_input_vertical);
s1680897 8:39519b33f767 176 //scope.set(2, EMG_filt3);
s1680897 8:39519b33f767 177 //scope.set(3, EMG_filt4);
s1680897 4:bd4dd411dc7b 178
s1680897 6:51c87d04371d 179
s1680897 4:bd4dd411dc7b 180 count++;
s1680897 6:51c87d04371d 181
s1680897 8:39519b33f767 182 if (count == 100)
s1680897 4:bd4dd411dc7b 183 {
s1680897 8:39519b33f767 184 pc.printf("EMG filt 1 = %lf \t EMG filt 2 = %lf \t EMG filt 3 = %lf \t EMG filt 4 = %lf \n\r", EMG_filt1, EMG_filt2, EMG_filt3, EMG_filt4);
s1680897 4:bd4dd411dc7b 185 count = 0;
s1680897 4:bd4dd411dc7b 186 }
s1680897 2:0189c5170834 187 }
s1680897 6:51c87d04371d 188
s1680897 0:94b5c70f818b 189 int main()
s1680897 0:94b5c70f818b 190 {
s1680897 0:94b5c70f818b 191 pc.baud(115200);
s1680897 6:51c87d04371d 192 //BiQuad chains
s1680897 6:51c87d04371d 193 bqc1.add( &HP_emg1 ).add( &Notch_emg1 );
s1680897 6:51c87d04371d 194 bqc2.add( &HP_emg2 ).add( &Notch_emg2 );
s1680897 6:51c87d04371d 195 bqc3.add( &HP_emg3 ).add( &Notch_emg3 );
s1680897 6:51c87d04371d 196 bqc4.add( &HP_emg4 ).add( &Notch_emg4 );
s1680897 0:94b5c70f818b 197
s1680897 6:51c87d04371d 198 // Attach the HIDScope::send method from the scope object to the timer at 50Hz
s1680897 6:51c87d04371d 199 scopeTimer.attach_us(&scope, &HIDScope::send, 5e3);
s1680897 6:51c87d04371d 200 EMGTicker.attach_us(EMG_filtering, 5e3);
s1680897 6:51c87d04371d 201
s1680897 2:0189c5170834 202 while(1) {}
s1680897 6:51c87d04371d 203
s1680897 0:94b5c70f818b 204 }