Marjolein Scheffers / Mbed 2 deprecated gr14_motor_HidScope

emg trial

Dependencies:   FastPWM HIDScope MODSERIAL mbed biquadFilter

main.cpp@4:bd4dd411dc7b, 2018-10-22 (annotated)

Committer:
s1680897
Date:
Mon Oct 22 13:05:16 2018 +0000
Revision:
4:bd4dd411dc7b
Parent:
3:b96d952083be
Child:
5:dd64e0cf20fe
Werkend script voor 2 EMG signalen + filter

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s1680897 3:b96d952083be 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 1:e81ca29ae626 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 1:e81ca29ae626 10 DigitalOut ledr(LED_RED);
s1680897 0:94b5c70f818b 11 MODSERIAL pc(USBTX, USBRX);
s1680897 0:94b5c70f818b 12
s1680897 0:94b5c70f818b 13
s1680897 1:e81ca29ae626 14 // Define the HIDScope and Ticker objects
s1680897 4:bd4dd411dc7b 15 HIDScope scope(4);
s1680897 1:e81ca29ae626 16 Ticker scopeTimer;
s1680897 2:0189c5170834 17 Ticker EMGTicker;
s1680897 2:0189c5170834 18
s1680897 2:0189c5170834 19 // BiQuad filter
s1680897 4:bd4dd411dc7b 20 BiQuad HP_emg1(1,-2,1,1.142980502539901,0.412801598096189); //Highpassfilter
s1680897 4:bd4dd411dc7b 21 BiQuad HP_emg2(1,-2,1,1.142980502539901,0.412801598096189); //Highpassfilter
s1680897 2:0189c5170834 22
s1680897 4:bd4dd411dc7b 23 // Global variables
s1680897 4:bd4dd411dc7b 24 double EMGdata1;
s1680897 4:bd4dd411dc7b 25 double EMGdata2;
s1680897 4:bd4dd411dc7b 26 int count;
s1680897 2:0189c5170834 27
s1680897 2:0189c5170834 28 void ReadEMG()
s1680897 2:0189c5170834 29 {
s1680897 4:bd4dd411dc7b 30 EMGdata1 = a0.read(); // store values in the scope
s1680897 4:bd4dd411dc7b 31 EMGdata2 = a1.read();
s1680897 4:bd4dd411dc7b 32
s1680897 2:0189c5170834 33 }
s1680897 2:0189c5170834 34
s1680897 4:bd4dd411dc7b 35 double EMG_HP1(double EMGdata)
s1680897 1:e81ca29ae626 36 {
s1680897 4:bd4dd411dc7b 37 double HP_abs_EMGdata = HP_emg1.step(EMGdata);
s1680897 4:bd4dd411dc7b 38
s1680897 4:bd4dd411dc7b 39 return fabs(HP_abs_EMGdata);
s1680897 4:bd4dd411dc7b 40 }
s1680897 4:bd4dd411dc7b 41
s1680897 4:bd4dd411dc7b 42 double EMG_HP2(double EMGdata)
s1680897 4:bd4dd411dc7b 43 {
s1680897 4:bd4dd411dc7b 44 double HP_abs_EMGdata = HP_emg2.step(EMGdata);
s1680897 2:0189c5170834 45
s1680897 2:0189c5170834 46 return fabs(HP_abs_EMGdata);
s1680897 1:e81ca29ae626 47 }
s1680897 0:94b5c70f818b 48
s1680897 4:bd4dd411dc7b 49 // Moving mean of EMG data 1
s1680897 4:bd4dd411dc7b 50 double Moving_mean1(double HP_abs_EMGdata1)
s1680897 1:e81ca29ae626 51 {
s1680897 4:bd4dd411dc7b 52 int P1 = 50;
s1680897 4:bd4dd411dc7b 53 static double EMGarray1[50];
s1680897 2:0189c5170834 54
s1680897 4:bd4dd411dc7b 55 for(int i1 = P1-1; i1 >= 1; i1--)
s1680897 2:0189c5170834 56 {
s1680897 4:bd4dd411dc7b 57 EMGarray1[i1] = EMGarray1[i1-1];
s1680897 2:0189c5170834 58 }
s1680897 4:bd4dd411dc7b 59 EMGarray1[0] = HP_abs_EMGdata1;
s1680897 2:0189c5170834 60
s1680897 4:bd4dd411dc7b 61 double sum1 = 0.0;
s1680897 2:0189c5170834 62
s1680897 4:bd4dd411dc7b 63 for(int j1=0; j1<50; j1++)
s1680897 2:0189c5170834 64 {
s1680897 4:bd4dd411dc7b 65 sum1 += EMGarray1[j1];
s1680897 3:b96d952083be 66
s1680897 2:0189c5170834 67 }
s1680897 2:0189c5170834 68
s1680897 4:bd4dd411dc7b 69 double EMG_filt_1 = sum1 / (double) P1;
s1680897 4:bd4dd411dc7b 70
s1680897 4:bd4dd411dc7b 71 return EMG_filt_1;
s1680897 4:bd4dd411dc7b 72 }
s1680897 4:bd4dd411dc7b 73
s1680897 4:bd4dd411dc7b 74 // Moving mean of EMG data 2
s1680897 4:bd4dd411dc7b 75 double Moving_mean2(double HP_abs_EMGdata2)
s1680897 4:bd4dd411dc7b 76 {
s1680897 4:bd4dd411dc7b 77 int P2 = 50;
s1680897 4:bd4dd411dc7b 78 static double EMGarray2[50];
s1680897 2:0189c5170834 79
s1680897 4:bd4dd411dc7b 80 for(int i2 = P2-1; i2 >= 1; i2--)
s1680897 4:bd4dd411dc7b 81 {
s1680897 4:bd4dd411dc7b 82 EMGarray2[i2] = EMGarray2[i2-1];
s1680897 4:bd4dd411dc7b 83 }
s1680897 4:bd4dd411dc7b 84 EMGarray2[0] = HP_abs_EMGdata2;
s1680897 4:bd4dd411dc7b 85
s1680897 4:bd4dd411dc7b 86 double sum2;
s1680897 4:bd4dd411dc7b 87
s1680897 4:bd4dd411dc7b 88 for(int j2=0; j2<50; j2++)
s1680897 4:bd4dd411dc7b 89 {
s1680897 4:bd4dd411dc7b 90 sum2 += EMGarray2[j2];
s1680897 4:bd4dd411dc7b 91
s1680897 4:bd4dd411dc7b 92 }
s1680897 4:bd4dd411dc7b 93
s1680897 4:bd4dd411dc7b 94 double EMG_filt_2 = sum2 / (double) P2;
s1680897 4:bd4dd411dc7b 95
s1680897 4:bd4dd411dc7b 96 return(EMG_filt_2);
s1680897 1:e81ca29ae626 97 }
s1680897 0:94b5c70f818b 98
s1680897 2:0189c5170834 99 void EMG_filtering ()
s1680897 2:0189c5170834 100 {
s1680897 2:0189c5170834 101 ReadEMG();
s1680897 4:bd4dd411dc7b 102 scope.set(0, EMGdata1);
s1680897 4:bd4dd411dc7b 103 scope.set(1, EMGdata2);
s1680897 4:bd4dd411dc7b 104
s1680897 4:bd4dd411dc7b 105 double HP_abs_EMGdata1 = EMG_HP1(EMGdata1);
s1680897 4:bd4dd411dc7b 106 double HP_abs_EMGdata2 = EMG_HP2(EMGdata2);
s1680897 4:bd4dd411dc7b 107
s1680897 4:bd4dd411dc7b 108 double EMG_filt1 = Moving_mean1(HP_abs_EMGdata1);
s1680897 4:bd4dd411dc7b 109 double EMG_filt2 = Moving_mean2(HP_abs_EMGdata2);
s1680897 2:0189c5170834 110
s1680897 4:bd4dd411dc7b 111 scope.set(2, EMG_filt1);
s1680897 4:bd4dd411dc7b 112 scope.set(3, EMG_filt2);
s1680897 4:bd4dd411dc7b 113
s1680897 4:bd4dd411dc7b 114 count++;
s1680897 2:0189c5170834 115
s1680897 4:bd4dd411dc7b 116 if (count == 50)
s1680897 4:bd4dd411dc7b 117 {
s1680897 4:bd4dd411dc7b 118 pc.printf("EMG filt 1 = %lf \t EMG filt 2 = %lf \n\r", EMG_filt1, EMG_filt2);
s1680897 4:bd4dd411dc7b 119 count = 0;
s1680897 4:bd4dd411dc7b 120 }
s1680897 2:0189c5170834 121 }
s1680897 1:e81ca29ae626 122
s1680897 0:94b5c70f818b 123 int main()
s1680897 0:94b5c70f818b 124 {
s1680897 0:94b5c70f818b 125 pc.baud(115200);
s1680897 1:e81ca29ae626 126 // Attach the HIDScope::send method from the scope object to the timer at 50Hz
s1680897 1:e81ca29ae626 127 scopeTimer.attach_us(&scope, &HIDScope::send, 2e4);
s1680897 2:0189c5170834 128 EMGTicker.attach_us(EMG_filtering, 2e4);
s1680897 0:94b5c70f818b 129
s1680897 2:0189c5170834 130 while(1) {}
s1680897 1:e81ca29ae626 131
s1680897 0:94b5c70f818b 132 }