Ralph Gerlings / Mbed 2 deprecated cpfromralph

new fork sure

Dependencies:   HIDScope MODSERIAL PID QEI biquadFilter mbed

Fork of cpfromralph by Remi van Veen

main.cpp@30:2c67abcdb892, 2017-10-30 (annotated)

Committer:
ralphg_92
Date:
Mon Oct 30 14:34:55 2017 +0000
Revision:
30:2c67abcdb892
Parent:
29:09c1567d6148
Child:
31:d346f9244b4a
now with 8 options, though unknown if it still works

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"
ralphg_92 29:09c1567d6148 3 #include "MODSERIAL.h"
relvorelvo 21:136a1ab8163c 4 #include "BiQuad.h"
ralphg_92 29:09c1567d6148 5 #include "QEI.h"
vsluiter 0:32bb76391d89 6
vsluiter 4:8b298dfada81 7 //Define objects
relvorelvo 25:07187cf76863 8 AnalogIn emg1_in( A0 ); /* read out the signal */
relvorelvo 23:e5db011bd410 9 AnalogIn emg2_in( A1 );
relvorelvo 23:e5db011bd410 10 AnalogIn emg3_in( A2 );
ralphg_92 27:ca07f895f999 11 AnalogIn emg4_in( A3 );
ralphg_92 29:09c1567d6148 12 DigitalIn max_reader12( SW2 ); /* define button press */
ralphg_92 27:ca07f895f999 13 DigitalIn max_reader34( SW3 );
relvorelvo 23:e5db011bd410 14
relvorelvo 23:e5db011bd410 15 Ticker main_timer;
relvorelvo 23:e5db011bd410 16 Ticker max_read1;
relvorelvo 23:e5db011bd410 17 Ticker max_read3;
ralphg_92 27:ca07f895f999 18 HIDScope scope( 4 );
relvorelvo 23:e5db011bd410 19 DigitalOut red(LED_RED);
relvorelvo 23:e5db011bd410 20 DigitalOut blue(LED_BLUE);
relvorelvo 23:e5db011bd410 21 DigitalOut green(LED_GREEN);
relvorelvo 23:e5db011bd410 22 MODSERIAL pc(USBTX, USBRX);
relvorelvo 21:136a1ab8163c 23
relvorelvo 21:136a1ab8163c 24
relvorelvo 21:136a1ab8163c 25 // EMG variables
relvorelvo 23:e5db011bd410 26 //Right Biceps
ralphg_92 29:09c1567d6148 27
ralphg_92 29:09c1567d6148 28
relvorelvo 23:e5db011bd410 29 double emg1;
relvorelvo 23:e5db011bd410 30 double emg1highfilter;
relvorelvo 23:e5db011bd410 31 double emg1notchfilter;
relvorelvo 23:e5db011bd410 32 double emg1abs;
relvorelvo 23:e5db011bd410 33 double emg1lowfilter;
relvorelvo 23:e5db011bd410 34 double emg1peak;
ralphg_92 27:ca07f895f999 35 double max1;
relvorelvo 24:26659f1de039 36 double maxpart1;
relvorelvo 23:e5db011bd410 37 // Left Biceps
relvorelvo 23:e5db011bd410 38 double emg2;
relvorelvo 23:e5db011bd410 39 double emg2highfilter;
relvorelvo 23:e5db011bd410 40 double emg2notchfilter;
relvorelvo 23:e5db011bd410 41 double emg2abs;
relvorelvo 23:e5db011bd410 42 double emg2lowfilter;
relvorelvo 23:e5db011bd410 43 double emg2peak;
ralphg_92 27:ca07f895f999 44 double max2;
relvorelvo 24:26659f1de039 45 double maxpart2;
ralphg_92 27:ca07f895f999 46 // Left Lower Arm
relvorelvo 23:e5db011bd410 47 double emg3;
relvorelvo 23:e5db011bd410 48 double emg3highfilter;
relvorelvo 23:e5db011bd410 49 double emg3notchfilter;
relvorelvo 23:e5db011bd410 50 double emg3abs;
relvorelvo 23:e5db011bd410 51 double emg3lowfilter;
relvorelvo 23:e5db011bd410 52 double emg3peak;
relvorelvo 23:e5db011bd410 53 double max3;
relvorelvo 23:e5db011bd410 54 double maxpart3;
ralphg_92 27:ca07f895f999 55 // Right Lower Arm
ralphg_92 27:ca07f895f999 56 double emg4;
ralphg_92 27:ca07f895f999 57 double emg4highfilter;
ralphg_92 27:ca07f895f999 58 double emg4notchfilter;
ralphg_92 27:ca07f895f999 59 double emg4abs;
ralphg_92 27:ca07f895f999 60 double emg4lowfilter;
ralphg_92 27:ca07f895f999 61 double emg4peak;
ralphg_92 27:ca07f895f999 62 double max4;
ralphg_92 27:ca07f895f999 63 double maxpart4;
ralphg_92 29:09c1567d6148 64
relvorelvo 25:07187cf76863 65 // BiQuad Filter Settings
relvorelvo 23:e5db011bd410 66 // Right Biceps
relvorelvo 23:e5db011bd410 67 BiQuad filterhigh1(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01); /* Filter at 10 Hz */
relvorelvo 23:e5db011bd410 68 BiQuad filternotch1(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01); /* Filter at 50 Hz */
relvorelvo 25:07187cf76863 69 BiQuad filterpeak1(1.0033,-1.984,9.852e-01,-1.9838,9.8855e-01); /* 4dB Gain peak at 11 Hz */
relvorelvo 24:26659f1de039 70 BiQuad filterlow1(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01); /* Filter at 15 Hz */
relvorelvo 23:e5db011bd410 71 // Left Biceps
relvorelvo 23:e5db011bd410 72 BiQuad filterhigh2(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01);
relvorelvo 23:e5db011bd410 73 BiQuad filternotch2(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01);
relvorelvo 24:26659f1de039 74 BiQuad filterpeak2(1.0033,-1.984,9.852e-01,-1.9838,9.8855e-01);
relvorelvo 24:26659f1de039 75 BiQuad filterlow2(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
relvorelvo 23:e5db011bd410 76 // Left Lower Arm OR Triceps
relvorelvo 23:e5db011bd410 77 BiQuad filterhigh3(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01);
relvorelvo 23:e5db011bd410 78 BiQuad filternotch3(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01);
relvorelvo 24:26659f1de039 79 BiQuad filterpeak3(1.0033,-1.984,9.852e-01,-1.9838,9.8855e-01);
relvorelvo 24:26659f1de039 80 BiQuad filterlow3(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
ralphg_92 27:ca07f895f999 81 // Right Lower Arm OR Triceps
ralphg_92 27:ca07f895f999 82 BiQuad filterhigh4(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01);
ralphg_92 27:ca07f895f999 83 BiQuad filternotch4(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01);
ralphg_92 27:ca07f895f999 84 BiQuad filterpeak4(1.0033,-1.984,9.852e-01,-1.9838,9.8855e-01);
ralphg_92 27:ca07f895f999 85 BiQuad filterlow4(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
relvorelvo 23:e5db011bd410 86 //
relvorelvo 21:136a1ab8163c 87
ralphg_92 29:09c1567d6148 88 // Finding max values for correct motor switch if the button is pressed
ralphg_92 29:09c1567d6148 89 void get_max1(){
ralphg_92 29:09c1567d6148 90 if (max_reader12==0){
ralphg_92 29:09c1567d6148 91 green = !green;
ralphg_92 29:09c1567d6148 92 red = 1;
ralphg_92 29:09c1567d6148 93 blue = 1;
ralphg_92 29:09c1567d6148 94 for(int n=0;n<2000;n++){ /* measure 2000 samples and filter it */
ralphg_92 29:09c1567d6148 95
ralphg_92 29:09c1567d6148 96 emg1 = emg1_in.read(); /* read out emg */
ralphg_92 29:09c1567d6148 97 emg1highfilter = filterhigh1.step(emg1); /* high pass filtered */
ralphg_92 29:09c1567d6148 98 emg1notchfilter = filternotch1.step(emg1highfilter); /* notch filtered */
ralphg_92 29:09c1567d6148 99 emg1abs = fabs(emg1notchfilter); /* take the absolute value */
ralphg_92 29:09c1567d6148 100 emg1lowfilter = filterlow1.step(emg1abs); /* low pass filtered */
ralphg_92 29:09c1567d6148 101 emg1peak = filterpeak1.step(emg1lowfilter); /* 4dB gain peak */
ralphg_92 29:09c1567d6148 102
ralphg_92 29:09c1567d6148 103 emg2 = emg2_in.read(); /* read out emg */
ralphg_92 29:09c1567d6148 104 emg2highfilter = filterhigh2.step(emg2); /* high pass filtered */
ralphg_92 29:09c1567d6148 105 emg2notchfilter = filternotch2.step(emg2highfilter); /* notch filtered */
ralphg_92 29:09c1567d6148 106 emg2abs = fabs(emg2notchfilter); /* take the absolute value */
ralphg_92 29:09c1567d6148 107 emg2lowfilter = filterlow2.step(emg2abs); /* low pass filtered */
ralphg_92 29:09c1567d6148 108 emg2peak = filterpeak2.step(emg2lowfilter); /* 4dB gain peak */
ralphg_92 29:09c1567d6148 109
ralphg_92 29:09c1567d6148 110 if (max1<emg1peak){
ralphg_92 29:09c1567d6148 111 max1 = emg1peak; /* set the max value at the highest measured value */
ralphg_92 29:09c1567d6148 112 }
ralphg_92 29:09c1567d6148 113 if (max2<emg2peak){
ralphg_92 29:09c1567d6148 114 max2 = emg2peak; /* set the max value at the highest measured value */
ralphg_92 29:09c1567d6148 115 }
ralphg_92 29:09c1567d6148 116 wait(0.001f); /* measure at 1000Hz */
ralphg_92 29:09c1567d6148 117 }
ralphg_92 29:09c1567d6148 118 wait(0.2f);
ralphg_92 29:09c1567d6148 119 green = 1;
ralphg_92 29:09c1567d6148 120 }
ralphg_92 29:09c1567d6148 121 maxpart1 = 0.12*max1; /* set cut off voltage at 15% of max for right biceps */
ralphg_92 29:09c1567d6148 122 maxpart2 = 0.12*max2; /* set cut off votage at 15% of max for left biceps */
ralphg_92 29:09c1567d6148 123 }
ralphg_92 29:09c1567d6148 124
relvorelvo 23:e5db011bd410 125 void get_max3(){
ralphg_92 27:ca07f895f999 126 if (max_reader34==0){
relvorelvo 23:e5db011bd410 127 green = 1;
relvorelvo 23:e5db011bd410 128 blue = 1;
relvorelvo 23:e5db011bd410 129 red = !red;
relvorelvo 23:e5db011bd410 130 for(int n=0;n<2000;n++){
relvorelvo 23:e5db011bd410 131
relvorelvo 23:e5db011bd410 132 emg3 = emg3_in.read();
relvorelvo 23:e5db011bd410 133 emg3highfilter = filterhigh3.step(emg3);
relvorelvo 23:e5db011bd410 134 emg3notchfilter = filternotch3.step(emg3highfilter);
relvorelvo 23:e5db011bd410 135 emg3abs = fabs(emg3notchfilter);
relvorelvo 23:e5db011bd410 136 emg3lowfilter = filterlow3.step(emg3abs);
relvorelvo 23:e5db011bd410 137 emg3peak = filterpeak3.step(emg3lowfilter);
relvorelvo 23:e5db011bd410 138
ralphg_92 27:ca07f895f999 139 emg4 = emg4_in.read();
ralphg_92 27:ca07f895f999 140 emg4highfilter = filterhigh4.step(emg4);
ralphg_92 27:ca07f895f999 141 emg4notchfilter = filternotch4.step(emg4highfilter);
ralphg_92 27:ca07f895f999 142 emg4abs = fabs(emg4notchfilter);
ralphg_92 27:ca07f895f999 143 emg4lowfilter = filterlow4.step(emg4abs);
ralphg_92 27:ca07f895f999 144 emg4peak = filterpeak4.step(emg4lowfilter);
ralphg_92 27:ca07f895f999 145
relvorelvo 23:e5db011bd410 146 if (max3<emg3peak){
relvorelvo 26:c9ba45bdd5c9 147 max3 = emg3peak; /* set the max value at the highest measured value */
relvorelvo 23:e5db011bd410 148 }
ralphg_92 27:ca07f895f999 149 if (max4<emg4peak){
ralphg_92 27:ca07f895f999 150 max4 = emg4peak; /* set the max value at the highest measured value */
ralphg_92 27:ca07f895f999 151 }
relvorelvo 23:e5db011bd410 152 wait(0.001f);
relvorelvo 23:e5db011bd410 153 }
relvorelvo 23:e5db011bd410 154 wait(0.2f);
relvorelvo 23:e5db011bd410 155 red = 1;
relvorelvo 23:e5db011bd410 156 }
ralphg_92 29:09c1567d6148 157 maxpart3 = 0.18*max3; /* set cut off voltage at 25% of max for left lower arm */
ralphg_92 29:09c1567d6148 158 maxpart4 = 0.18*max4; /* set cut off voltage at 25% of max for right lower arm */
vsluiter 2:e314bb3b2d99 159 }
vsluiter 0:32bb76391d89 160
relvorelvo 23:e5db011bd410 161 // Filtering & Scope
relvorelvo 23:e5db011bd410 162 void filter() {
relvorelvo 23:e5db011bd410 163 // Right Biceps
relvorelvo 23:e5db011bd410 164 emg1 = emg1_in.read();
relvorelvo 23:e5db011bd410 165 emg1highfilter = filterhigh1.step(emg1);
relvorelvo 23:e5db011bd410 166 emg1notchfilter = filternotch1.step(emg1highfilter);
relvorelvo 23:e5db011bd410 167 emg1abs = fabs(emg1notchfilter);
relvorelvo 23:e5db011bd410 168 emg1lowfilter = filterlow1.step(emg1abs);
relvorelvo 26:c9ba45bdd5c9 169 emg1peak = filterpeak1.step(emg1lowfilter); /* Final Right Biceps values to be sent */
relvorelvo 23:e5db011bd410 170 // Left Biceps
relvorelvo 23:e5db011bd410 171 emg2 = emg2_in.read();
relvorelvo 23:e5db011bd410 172 emg2highfilter = filterhigh2.step(emg2);
relvorelvo 23:e5db011bd410 173 emg2notchfilter = filternotch2.step(emg2highfilter);
relvorelvo 23:e5db011bd410 174 emg2abs = fabs(emg2notchfilter);
relvorelvo 23:e5db011bd410 175 emg2lowfilter = filterlow2.step(emg2abs);
relvorelvo 26:c9ba45bdd5c9 176 emg2peak = filterpeak2.step(emg2lowfilter); /* Final Left Biceps values to be sent */
relvorelvo 23:e5db011bd410 177 // Left Lower Arm OR Triceps
relvorelvo 23:e5db011bd410 178 emg3 = emg3_in.read();
relvorelvo 23:e5db011bd410 179 emg3highfilter = filterhigh3.step(emg3);
relvorelvo 23:e5db011bd410 180 emg3notchfilter = filternotch3.step(emg3highfilter);
relvorelvo 23:e5db011bd410 181 emg3abs = fabs(emg3notchfilter);
relvorelvo 23:e5db011bd410 182 emg3lowfilter = filterlow3.step(emg3abs);
relvorelvo 26:c9ba45bdd5c9 183 emg3peak = filterpeak3.step(emg3lowfilter); /* Final Lower Arm values to be sent */
ralphg_92 27:ca07f895f999 184 // Right Lower Arm OR Triceps
ralphg_92 27:ca07f895f999 185 emg4 = emg4_in.read();
ralphg_92 27:ca07f895f999 186 emg4highfilter = filterhigh4.step(emg4);
ralphg_92 27:ca07f895f999 187 emg4notchfilter = filternotch4.step(emg4highfilter);
ralphg_92 27:ca07f895f999 188 emg4abs = fabs(emg4notchfilter);
ralphg_92 27:ca07f895f999 189 emg4lowfilter = filterlow4.step(emg4abs);
ralphg_92 29:09c1567d6148 190 emg4peak = filterpeak4.step(emg4lowfilter); /* Final Lower Arm values to be sent */
ralphg_92 29:09c1567d6148 191
relvorelvo 23:e5db011bd410 192
ralphg_92 29:09c1567d6148 193 /* Compare measurement to the calibrated value to decide actions */
ralphg_92 29:09c1567d6148 194
ralphg_92 30:2c67abcdb892 195 /* This part checks for right biceps contractions only*/
ralphg_92 29:09c1567d6148 196 if (maxpart1<emg1peak && maxpart2>emg2peak && maxpart3>emg3peak && maxpart4>emg4peak){
ralphg_92 29:09c1567d6148 197 red = 1;
ralphg_92 29:09c1567d6148 198 blue = 1;
ralphg_92 29:09c1567d6148 199 green = 0;
ralphg_92 29:09c1567d6148 200 }
ralphg_92 30:2c67abcdb892 201 /* This part checks for left biceps contractions only*/
ralphg_92 29:09c1567d6148 202 else if (maxpart1>emg1peak && maxpart2<emg2peak && maxpart3>emg3peak && maxpart4>emg4peak){
ralphg_92 29:09c1567d6148 203 red = 0;
ralphg_92 29:09c1567d6148 204 blue = 1;
ralphg_92 29:09c1567d6148 205 green = 1;
relvorelvo 23:e5db011bd410 206 }
ralphg_92 30:2c67abcdb892 207 /* This part checks for left lower arm contractions only*/
ralphg_92 29:09c1567d6148 208 else if (maxpart1>emg1peak && maxpart2>emg2peak && maxpart3<emg3peak && maxpart4>emg4peak){
relvorelvo 23:e5db011bd410 209 red = 1;
relvorelvo 23:e5db011bd410 210 blue = 0;
relvorelvo 23:e5db011bd410 211 green = 1;
ralphg_92 29:09c1567d6148 212 }
ralphg_92 30:2c67abcdb892 213 /* This part checks for right lower arm contractions only */
ralphg_92 29:09c1567d6148 214 else if (maxpart1>emg1peak && maxpart2>emg2peak && maxpart3>emg3peak && maxpart4<emg4peak){
ralphg_92 29:09c1567d6148 215 red = 0;
relvorelvo 23:e5db011bd410 216 blue = 1;
relvorelvo 23:e5db011bd410 217 green = 0;
ralphg_92 30:2c67abcdb892 218 }
ralphg_92 30:2c67abcdb892 219 /* This part checks for both lower arm contractions only */
ralphg_92 30:2c67abcdb892 220 else if (maxpart1>emg1peak && maxpart2>emg2peak && maxpart3<emg3peak && maxpart4<emg4peak){
ralphg_92 30:2c67abcdb892 221 red = 0;
ralphg_92 30:2c67abcdb892 222 blue = 0;
ralphg_92 30:2c67abcdb892 223 green = 0;
ralphg_92 30:2c67abcdb892 224 }
ralphg_92 30:2c67abcdb892 225 /* This part checks for both biceps contractions only*/
ralphg_92 30:2c67abcdb892 226 else if (maxpart1<emg1peak && maxpart2<emg2peak && maxpart3>emg3peak && maxpart4>emg4peak){
ralphg_92 30:2c67abcdb892 227 red = 0;
ralphg_92 30:2c67abcdb892 228 blue = 0;
ralphg_92 30:2c67abcdb892 229 green = 0;
ralphg_92 30:2c67abcdb892 230 }
ralphg_92 30:2c67abcdb892 231 /* This part checks for right lower arm & left biceps contractions only*/
ralphg_92 30:2c67abcdb892 232 else if (maxpart1>emg1peak && maxpart2<emg2peak && maxpart3>emg3peak && maxpart4<emg4peak){
ralphg_92 30:2c67abcdb892 233 red = 0;
ralphg_92 30:2c67abcdb892 234 blue = 0;
ralphg_92 30:2c67abcdb892 235 green = 0;
ralphg_92 30:2c67abcdb892 236 }
ralphg_92 30:2c67abcdb892 237 /* This part checks for left lower arm & right biceps contractions only*/
ralphg_92 30:2c67abcdb892 238 else if (maxpart1<emg1peak && maxpart2>emg2peak && maxpart3<emg3peak && maxpart4>emg4peak){
ralphg_92 30:2c67abcdb892 239 red = 0;
ralphg_92 30:2c67abcdb892 240 blue = 0;
ralphg_92 30:2c67abcdb892 241 green = 0;
ralphg_92 30:2c67abcdb892 242 }
ralphg_92 29:09c1567d6148 243 else {
relvorelvo 26:c9ba45bdd5c9 244 red = 1; /* Shut down all led colors if no movement is registered */
relvorelvo 23:e5db011bd410 245 blue = 1;
relvorelvo 23:e5db011bd410 246 green = 1;
ralphg_92 29:09c1567d6148 247 //pc.printf( "No contraction registered\n");
ralphg_92 29:09c1567d6148 248 }
ralphg_92 29:09c1567d6148 249
relvorelvo 23:e5db011bd410 250 /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
relvorelvo 26:c9ba45bdd5c9 251 scope.set(0, emg1peak ); /* plot Right biceps voltage */
relvorelvo 26:c9ba45bdd5c9 252 scope.set(1, emg2peak ); /* Plot Left biceps voltage */
ralphg_92 27:ca07f895f999 253 scope.set(2, emg3peak ); /* Plot Lower Left Arm voltage */
ralphg_92 27:ca07f895f999 254 scope.set(3, emg4peak ); /* Plot Lower Right Arm Voltage */
relvorelvo 23:e5db011bd410 255
relvorelvo 25:07187cf76863 256 scope.send(); /* send everything to the HID scope */
relvorelvo 23:e5db011bd410 257 }
tomlankhorst 15:0da764eea774 258
relvorelvo 23:e5db011bd410 259 int main(){
relvorelvo 22:68ab712b62b2 260
relvorelvo 25:07187cf76863 261 main_timer.attach(&filter, 0.001); /* set frequency for the filters at 1000Hz */
relvorelvo 25:07187cf76863 262 max_read1.attach(&get_max1, 2); /* set the frequency of the calibration loop at 0.5Hz */
relvorelvo 23:e5db011bd410 263 max_read3.attach(&get_max3, 2);
tomlankhorst 15:0da764eea774 264 while(1) {}
vsluiter 0:32bb76391d89 265 }