Remi van Veen / Mbed 2 deprecated EMG-AANSTURING-MOTOREN

not checked because compiler was down, but this should do everything!!!!

Dependencies:   FastPWM HIDScope MODSERIAL QEI biquadFilter mbed

Fork of EMG4 by Remi van Veen

main.cpp@25:07187cf76863, 2016-11-03 (annotated)

Committer:
relvorelvo
Date:
Thu Nov 03 14:58:42 2016 +0000
Revision:
25:07187cf76863
Parent:
24:26659f1de039
Child:
26:c9ba45bdd5c9
iets meer gefine tuned

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsluiter 0:32bb76391d89 1 #include "mbed.h"
vsluiter 11:ce72ec658a95 2 #include "HIDScope.h"
relvorelvo 21:136a1ab8163c 3 #include "BiQuad.h"
relvorelvo 21:136a1ab8163c 4 #include "MODSERIAL.h"
vsluiter 0:32bb76391d89 5
vsluiter 4:8b298dfada81 6 //Define objects
relvorelvo 25:07187cf76863 7 AnalogIn emg1_in( A0 ); /* read out the signal */
relvorelvo 23:e5db011bd410 8 AnalogIn emg2_in( A1 );
relvorelvo 23:e5db011bd410 9 AnalogIn emg3_in( A2 );
relvorelvo 23:e5db011bd410 10 DigitalIn max_reader1( SW2 );
relvorelvo 23:e5db011bd410 11 DigitalIn max_reader3( SW3 );
relvorelvo 23:e5db011bd410 12
relvorelvo 23:e5db011bd410 13 Ticker main_timer;
relvorelvo 23:e5db011bd410 14 Ticker max_read1;
relvorelvo 23:e5db011bd410 15 Ticker max_read3;
relvorelvo 23:e5db011bd410 16 HIDScope scope( 5 );
relvorelvo 23:e5db011bd410 17 DigitalOut red(LED_RED);
relvorelvo 23:e5db011bd410 18 DigitalOut blue(LED_BLUE);
relvorelvo 23:e5db011bd410 19 DigitalOut green(LED_GREEN);
relvorelvo 23:e5db011bd410 20 MODSERIAL pc(USBTX, USBRX);
relvorelvo 21:136a1ab8163c 21
relvorelvo 21:136a1ab8163c 22
relvorelvo 21:136a1ab8163c 23 // EMG variables
relvorelvo 23:e5db011bd410 24 //Right Biceps
relvorelvo 23:e5db011bd410 25 double emg1;
relvorelvo 23:e5db011bd410 26 double emg1highfilter;
relvorelvo 23:e5db011bd410 27 double emg1notchfilter;
relvorelvo 23:e5db011bd410 28 double emg1abs;
relvorelvo 23:e5db011bd410 29 double emg1lowfilter;
relvorelvo 23:e5db011bd410 30 double emg1peak;
relvorelvo 24:26659f1de039 31 double maxpart1;
relvorelvo 23:e5db011bd410 32 // Left Biceps
relvorelvo 23:e5db011bd410 33 double emg2;
relvorelvo 23:e5db011bd410 34 double emg2highfilter;
relvorelvo 23:e5db011bd410 35 double emg2notchfilter;
relvorelvo 23:e5db011bd410 36 double emg2abs;
relvorelvo 23:e5db011bd410 37 double emg2lowfilter;
relvorelvo 23:e5db011bd410 38 double emg2peak;
relvorelvo 23:e5db011bd410 39 double max1;
relvorelvo 24:26659f1de039 40 double maxpart2;
relvorelvo 23:e5db011bd410 41 // Left Lower Arm OR Triceps
relvorelvo 23:e5db011bd410 42 double emg3;
relvorelvo 23:e5db011bd410 43 double emg3highfilter;
relvorelvo 23:e5db011bd410 44 double emg3notchfilter;
relvorelvo 23:e5db011bd410 45 double emg3abs;
relvorelvo 23:e5db011bd410 46 double emg3lowfilter;
relvorelvo 23:e5db011bd410 47 double emg3peak;
relvorelvo 23:e5db011bd410 48 double max3;
relvorelvo 23:e5db011bd410 49 double maxpart3;
vsluiter 2:e314bb3b2d99 50
relvorelvo 25:07187cf76863 51 // BiQuad Filter Settings
relvorelvo 23:e5db011bd410 52 // Right Biceps
relvorelvo 23:e5db011bd410 53 BiQuad filterhigh1(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01); /* Filter at 10 Hz */
relvorelvo 23:e5db011bd410 54 BiQuad filternotch1(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01); /* Filter at 50 Hz */
relvorelvo 25:07187cf76863 55 BiQuad filterpeak1(1.0033,-1.984,9.852e-01,-1.9838,9.8855e-01); /* 4dB Gain peak at 11 Hz */
relvorelvo 24:26659f1de039 56 BiQuad filterlow1(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01); /* Filter at 15 Hz */
relvorelvo 23:e5db011bd410 57 // Left Biceps
relvorelvo 23:e5db011bd410 58 BiQuad filterhigh2(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01);
relvorelvo 23:e5db011bd410 59 BiQuad filternotch2(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01);
relvorelvo 24:26659f1de039 60 BiQuad filterpeak2(1.0033,-1.984,9.852e-01,-1.9838,9.8855e-01);
relvorelvo 24:26659f1de039 61 BiQuad filterlow2(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
relvorelvo 23:e5db011bd410 62 // Left Lower Arm OR Triceps
relvorelvo 23:e5db011bd410 63 BiQuad filterhigh3(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01);
relvorelvo 23:e5db011bd410 64 BiQuad filternotch3(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01);
relvorelvo 24:26659f1de039 65 BiQuad filterpeak3(1.0033,-1.984,9.852e-01,-1.9838,9.8855e-01);
relvorelvo 24:26659f1de039 66 BiQuad filterlow3(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
relvorelvo 23:e5db011bd410 67 //
relvorelvo 21:136a1ab8163c 68
relvorelvo 25:07187cf76863 69 // Finding max values for correct motor switch if the button is pressed
relvorelvo 23:e5db011bd410 70 void get_max1(){
relvorelvo 23:e5db011bd410 71 if (max_reader1==0){
relvorelvo 23:e5db011bd410 72 green = !green;
relvorelvo 23:e5db011bd410 73 red = 1;
relvorelvo 23:e5db011bd410 74 blue = 1;
relvorelvo 25:07187cf76863 75
relvorelvo 25:07187cf76863 76 for(int n=0;n<2000;n++){ /* measure 2000 samples and filter it */
relvorelvo 25:07187cf76863 77 emg1 = emg1_in.read(); /* read out emg */
relvorelvo 25:07187cf76863 78 emg1highfilter = filterhigh1.step(emg1); /* high pass filtered */
relvorelvo 25:07187cf76863 79 emg1notchfilter = filternotch1.step(emg1highfilter); /* notch filtered */
relvorelvo 25:07187cf76863 80 emg1abs = fabs(emg1notchfilter); /* take the absolute value */
relvorelvo 25:07187cf76863 81 emg1lowfilter = filterlow1.step(emg1abs); /* low pass filtered */
relvorelvo 25:07187cf76863 82 emg1peak = filterpeak1.step(emg1lowfilter); /* 4dB gain peak */
relvorelvo 23:e5db011bd410 83
relvorelvo 23:e5db011bd410 84 if (max1<emg1peak){
relvorelvo 23:e5db011bd410 85 max1 = emg1peak;
relvorelvo 23:e5db011bd410 86 }
relvorelvo 23:e5db011bd410 87 wait(0.001f);
relvorelvo 23:e5db011bd410 88 }
relvorelvo 23:e5db011bd410 89 wait(0.2f);
relvorelvo 23:e5db011bd410 90 green = 1;
relvorelvo 23:e5db011bd410 91 }
relvorelvo 25:07187cf76863 92 maxpart1 = 0.25*max1;
relvorelvo 25:07187cf76863 93 maxpart2 = 0.15*max1;
relvorelvo 23:e5db011bd410 94 }
relvorelvo 21:136a1ab8163c 95
relvorelvo 23:e5db011bd410 96 void get_max3(){
relvorelvo 23:e5db011bd410 97 if (max_reader3==0){
relvorelvo 23:e5db011bd410 98 green = 1;
relvorelvo 23:e5db011bd410 99 blue = 1;
relvorelvo 23:e5db011bd410 100 red = !red;
relvorelvo 23:e5db011bd410 101 for(int n=0;n<2000;n++){
relvorelvo 23:e5db011bd410 102
relvorelvo 23:e5db011bd410 103 emg3 = emg3_in.read();
relvorelvo 23:e5db011bd410 104 emg3highfilter = filterhigh3.step(emg3);
relvorelvo 23:e5db011bd410 105 emg3notchfilter = filternotch3.step(emg3highfilter);
relvorelvo 23:e5db011bd410 106 emg3abs = fabs(emg3notchfilter);
relvorelvo 23:e5db011bd410 107 emg3lowfilter = filterlow3.step(emg3abs);
relvorelvo 23:e5db011bd410 108 emg3peak = filterpeak3.step(emg3lowfilter);
relvorelvo 23:e5db011bd410 109
relvorelvo 23:e5db011bd410 110 if (max3<emg3peak){
relvorelvo 23:e5db011bd410 111 max3 = emg3peak;
relvorelvo 23:e5db011bd410 112 }
relvorelvo 23:e5db011bd410 113 wait(0.001f);
relvorelvo 23:e5db011bd410 114 }
relvorelvo 23:e5db011bd410 115 wait(0.2f);
relvorelvo 23:e5db011bd410 116 red = 1;
relvorelvo 23:e5db011bd410 117 }
relvorelvo 25:07187cf76863 118 maxpart3 = 0.25*max3;
vsluiter 2:e314bb3b2d99 119 }
vsluiter 0:32bb76391d89 120
relvorelvo 23:e5db011bd410 121 // Filtering & Scope
relvorelvo 23:e5db011bd410 122 void filter() {
relvorelvo 23:e5db011bd410 123 // Right Biceps
relvorelvo 23:e5db011bd410 124 emg1 = emg1_in.read();
relvorelvo 23:e5db011bd410 125 emg1highfilter = filterhigh1.step(emg1);
relvorelvo 23:e5db011bd410 126 emg1notchfilter = filternotch1.step(emg1highfilter);
relvorelvo 23:e5db011bd410 127 emg1abs = fabs(emg1notchfilter);
relvorelvo 23:e5db011bd410 128 emg1lowfilter = filterlow1.step(emg1abs);
relvorelvo 23:e5db011bd410 129 emg1peak = filterpeak1.step(emg1lowfilter);
relvorelvo 23:e5db011bd410 130 // Left Biceps
relvorelvo 23:e5db011bd410 131 emg2 = emg2_in.read();
relvorelvo 23:e5db011bd410 132 emg2highfilter = filterhigh2.step(emg2);
relvorelvo 23:e5db011bd410 133 emg2notchfilter = filternotch2.step(emg2highfilter);
relvorelvo 23:e5db011bd410 134 emg2abs = fabs(emg2notchfilter);
relvorelvo 23:e5db011bd410 135 emg2lowfilter = filterlow2.step(emg2abs);
relvorelvo 23:e5db011bd410 136 emg2peak = filterpeak2.step(emg2lowfilter);
relvorelvo 23:e5db011bd410 137 // Left Lower Arm OR Triceps
relvorelvo 23:e5db011bd410 138 emg3 = emg3_in.read();
relvorelvo 23:e5db011bd410 139 emg3highfilter = filterhigh3.step(emg3);
relvorelvo 23:e5db011bd410 140 emg3notchfilter = filternotch3.step(emg3highfilter);
relvorelvo 23:e5db011bd410 141 emg3abs = fabs(emg3notchfilter);
relvorelvo 23:e5db011bd410 142 emg3lowfilter = filterlow3.step(emg3abs);
relvorelvo 23:e5db011bd410 143 emg3peak = filterpeak3.step(emg3lowfilter);
relvorelvo 23:e5db011bd410 144
relvorelvo 23:e5db011bd410 145
relvorelvo 23:e5db011bd410 146 /* Compare measurement to the calibrated value to decide actions */
relvorelvo 23:e5db011bd410 147 if (maxpart1<emg1peak){
relvorelvo 23:e5db011bd410 148 red = 0;
relvorelvo 23:e5db011bd410 149 blue = 1;
relvorelvo 23:e5db011bd410 150 green = 1;
relvorelvo 23:e5db011bd410 151 }
relvorelvo 23:e5db011bd410 152 else {
relvorelvo 24:26659f1de039 153 if (maxpart2<emg2peak){
relvorelvo 23:e5db011bd410 154 red = 1;
relvorelvo 23:e5db011bd410 155 blue = 0;
relvorelvo 23:e5db011bd410 156 green = 1;
relvorelvo 23:e5db011bd410 157 }
relvorelvo 23:e5db011bd410 158
relvorelvo 23:e5db011bd410 159 else {
relvorelvo 24:26659f1de039 160 if (maxpart3<emg3peak){
relvorelvo 23:e5db011bd410 161 red = 1;
relvorelvo 23:e5db011bd410 162 blue = 1;
relvorelvo 23:e5db011bd410 163 green = 0;
relvorelvo 23:e5db011bd410 164 }
relvorelvo 23:e5db011bd410 165
relvorelvo 23:e5db011bd410 166 else {
relvorelvo 23:e5db011bd410 167 red = 1;
relvorelvo 23:e5db011bd410 168 blue = 1;
relvorelvo 23:e5db011bd410 169 green = 1;
relvorelvo 23:e5db011bd410 170 }
relvorelvo 23:e5db011bd410 171 }
relvorelvo 23:e5db011bd410 172 }
relvorelvo 23:e5db011bd410 173 /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
relvorelvo 23:e5db011bd410 174 scope.set(0, emg1peak );
relvorelvo 23:e5db011bd410 175 scope.set(1, emg2peak );
relvorelvo 23:e5db011bd410 176 scope.set(2, maxpart1 );
relvorelvo 23:e5db011bd410 177 scope.set(3, emg3peak );
relvorelvo 23:e5db011bd410 178 scope.set(4, maxpart3 );
relvorelvo 23:e5db011bd410 179
relvorelvo 25:07187cf76863 180 scope.send(); /* send everything to the HID scope */
relvorelvo 23:e5db011bd410 181 }
tomlankhorst 15:0da764eea774 182
relvorelvo 23:e5db011bd410 183 int main(){
relvorelvo 22:68ab712b62b2 184
relvorelvo 25:07187cf76863 185 main_timer.attach(&filter, 0.001); /* set frequency for the filters at 1000Hz */
relvorelvo 25:07187cf76863 186 max_read1.attach(&get_max1, 2); /* set the frequency of the calibration loop at 0.5Hz */
relvorelvo 23:e5db011bd410 187 max_read3.attach(&get_max3, 2);
tomlankhorst 15:0da764eea774 188 while(1) {}
vsluiter 0:32bb76391d89 189 }