Remi van Veen / Mbed 2 deprecated EMG4

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

Dependencies:   FastPWM HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@27:0dbd4fd88593, 2016-11-04 (annotated)

Committer:
Revave
Date:
Fri Nov 04 04:32:35 2016 +0000
Revision:
27:0dbd4fd88593
Parent:
26:c9ba45bdd5c9 
not checked for errors because compiler is not working atm, but this should do everything

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
Revave 27:0dbd4fd88593 6 #include "FastPWM.h"
Revave 27:0dbd4fd88593 7 #include "QEI.h"
Revave 27:0dbd4fd88593 8 float pwmprocent1 = 15; // introduce duty cycle variable translational motor
Revave 27:0dbd4fd88593 9 float pwmprocent2 = 15; // introduce duty cycle for rotational motor (SLOW DOWN TO PREVENT OVERSHOOT?)
Revave 27:0dbd4fd88593 10 int modesw; // introduce mode to switch from translational to rotational control
Revave 27:0dbd4fd88593 11 FastPWM motorpwm1(D5); // define PWM Pin
Revave 27:0dbd4fd88593 12 FastPWM motorpwm2(D6); // define motor 2 pwm pin
Revave 27:0dbd4fd88593 13 float totalpwm1 = pwmprocent1/100;
Revave 27:0dbd4fd88593 14 float totalpwm2 = pwmprocent2/100;
Revave 27:0dbd4fd88593 15 int upperlimit = 1200;
Revave 27:0dbd4fd88593 16 int lowerlimit = 3920;
Revave 27:0dbd4fd88593 17 DigitalOut m1direction(D4); // direction translational motor
Revave 27:0dbd4fd88593 18 DigitalOut m2direction(D7); // direction
Revave 27:0dbd4fd88593 19
vsluiter 4:8b298dfada81 20 //Define objects
relvorelvo 25:07187cf76863 21 AnalogIn emg1_in( A0 ); /* read out the signal */
relvorelvo 23:e5db011bd410 22 AnalogIn emg2_in( A1 );
relvorelvo 23:e5db011bd410 23 AnalogIn emg3_in( A2 );
relvorelvo 23:e5db011bd410 24 DigitalIn max_reader1( SW2 );
relvorelvo 23:e5db011bd410 25 DigitalIn max_reader3( SW3 );
relvorelvo 23:e5db011bd410 26
relvorelvo 23:e5db011bd410 27 Ticker main_timer;
relvorelvo 23:e5db011bd410 28 Ticker max_read1;
relvorelvo 23:e5db011bd410 29 Ticker max_read3;
relvorelvo 23:e5db011bd410 30 HIDScope scope( 5 );
relvorelvo 23:e5db011bd410 31 DigitalOut red(LED_RED);
relvorelvo 23:e5db011bd410 32 DigitalOut blue(LED_BLUE);
relvorelvo 23:e5db011bd410 33 DigitalOut green(LED_GREEN);
relvorelvo 23:e5db011bd410 34 MODSERIAL pc(USBTX, USBRX);
relvorelvo 21:136a1ab8163c 35
relvorelvo 21:136a1ab8163c 36
relvorelvo 21:136a1ab8163c 37 // EMG variables
relvorelvo 23:e5db011bd410 38 //Right Biceps
relvorelvo 23:e5db011bd410 39 double emg1;
relvorelvo 23:e5db011bd410 40 double emg1highfilter;
relvorelvo 23:e5db011bd410 41 double emg1notchfilter;
relvorelvo 23:e5db011bd410 42 double emg1abs;
relvorelvo 23:e5db011bd410 43 double emg1lowfilter;
relvorelvo 23:e5db011bd410 44 double emg1peak;
relvorelvo 24:26659f1de039 45 double maxpart1;
relvorelvo 23:e5db011bd410 46 // Left Biceps
relvorelvo 23:e5db011bd410 47 double emg2;
relvorelvo 23:e5db011bd410 48 double emg2highfilter;
relvorelvo 23:e5db011bd410 49 double emg2notchfilter;
relvorelvo 23:e5db011bd410 50 double emg2abs;
relvorelvo 23:e5db011bd410 51 double emg2lowfilter;
relvorelvo 23:e5db011bd410 52 double emg2peak;
relvorelvo 23:e5db011bd410 53 double max1;
relvorelvo 24:26659f1de039 54 double maxpart2;
relvorelvo 23:e5db011bd410 55 // Left Lower Arm OR Triceps
relvorelvo 23:e5db011bd410 56 double emg3;
relvorelvo 23:e5db011bd410 57 double emg3highfilter;
relvorelvo 23:e5db011bd410 58 double emg3notchfilter;
relvorelvo 23:e5db011bd410 59 double emg3abs;
relvorelvo 23:e5db011bd410 60 double emg3lowfilter;
relvorelvo 23:e5db011bd410 61 double emg3peak;
relvorelvo 23:e5db011bd410 62 double max3;
relvorelvo 23:e5db011bd410 63 double maxpart3;
vsluiter 2:e314bb3b2d99 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);
relvorelvo 23:e5db011bd410 81 //
relvorelvo 21:136a1ab8163c 82
Revave 27:0dbd4fd88593 83 // stopfunction
Revave 27:0dbd4fd88593 84 void superstop()
Revave 27:0dbd4fd88593 85 {
Revave 27:0dbd4fd88593 86 int stoptimes;
Revave 27:0dbd4fd88593 87 if (modesw==0)
Revave 27:0dbd4fd88593 88 {
Revave 27:0dbd4fd88593 89 motorpwm1.write(0);
Revave 27:0dbd4fd88593 90 for (int a = 0; a < pwmprocent1; a = a + 1)
Revave 27:0dbd4fd88593 91 {
Revave 27:0dbd4fd88593 92 if (m1direction = 0)
Revave 27:0dbd4fd88593 93 {
Revave 27:0dbd4fd88593 94 m1direction = 1;
Revave 27:0dbd4fd88593 95 }
Revave 27:0dbd4fd88593 96 else
Revave 27:0dbd4fd88593 97 {
Revave 27:0dbd4fd88593 98 m1direction = 0;
Revave 27:0dbd4fd88593 99 }
Revave 27:0dbd4fd88593 100 }
Revave 27:0dbd4fd88593 101 else
Revave 27:0dbd4fd88593 102 {
Revave 27:0dbd4fd88593 103 motorpwm2.write(0);
Revave 27:0dbd4fd88593 104 for (int a = 0; a < pwmprocent2; a = a+1)
Revave 27:0dbd4fd88593 105 {
Revave 27:0dbd4fd88593 106 if (m2direction = 0)
Revave 27:0dbd4fd88593 107 {
Revave 27:0dbd4fd88593 108 m2direction = 1;
Revave 27:0dbd4fd88593 109 }
Revave 27:0dbd4fd88593 110 else
Revave 27:0dbd4fd88593 111 {
Revave 27:0dbd4fd88593 112 m2direction = 0;
Revave 27:0dbd4fd88593 113 }
Revave 27:0dbd4fd88593 114 }
Revave 27:0dbd4fd88593 115 }
Revave 27:0dbd4fd88593 116 }
Revave 27:0dbd4fd88593 117
relvorelvo 25:07187cf76863 118 // Finding max values for correct motor switch if the button is pressed
relvorelvo 23:e5db011bd410 119 void get_max1(){
relvorelvo 23:e5db011bd410 120 if (max_reader1==0){
relvorelvo 23:e5db011bd410 121 green = !green;
relvorelvo 23:e5db011bd410 122 red = 1;
relvorelvo 23:e5db011bd410 123 blue = 1;
relvorelvo 25:07187cf76863 124
relvorelvo 25:07187cf76863 125 for(int n=0;n<2000;n++){ /* measure 2000 samples and filter it */
relvorelvo 25:07187cf76863 126 emg1 = emg1_in.read(); /* read out emg */
relvorelvo 25:07187cf76863 127 emg1highfilter = filterhigh1.step(emg1); /* high pass filtered */
relvorelvo 25:07187cf76863 128 emg1notchfilter = filternotch1.step(emg1highfilter); /* notch filtered */
relvorelvo 25:07187cf76863 129 emg1abs = fabs(emg1notchfilter); /* take the absolute value */
relvorelvo 25:07187cf76863 130 emg1lowfilter = filterlow1.step(emg1abs); /* low pass filtered */
relvorelvo 25:07187cf76863 131 emg1peak = filterpeak1.step(emg1lowfilter); /* 4dB gain peak */
relvorelvo 23:e5db011bd410 132
relvorelvo 23:e5db011bd410 133 if (max1<emg1peak){
relvorelvo 26:c9ba45bdd5c9 134 max1 = emg1peak; /* set the max value at the highest measured value */
relvorelvo 23:e5db011bd410 135 }
relvorelvo 26:c9ba45bdd5c9 136 wait(0.001f); /* measure at 1000Hz */
relvorelvo 23:e5db011bd410 137 }
relvorelvo 23:e5db011bd410 138 wait(0.2f);
relvorelvo 23:e5db011bd410 139 green = 1;
relvorelvo 23:e5db011bd410 140 }
relvorelvo 26:c9ba45bdd5c9 141 maxpart1 = 0.25*max1; /* set cut off voltage at 25% of max for right biceps */
relvorelvo 26:c9ba45bdd5c9 142 maxpart2 = 0.15*max1; /* set cut off votage at 15% of max for left biceps */
relvorelvo 23:e5db011bd410 143 }
relvorelvo 21:136a1ab8163c 144
relvorelvo 23:e5db011bd410 145 void get_max3(){
relvorelvo 23:e5db011bd410 146 if (max_reader3==0){
relvorelvo 23:e5db011bd410 147 green = 1;
relvorelvo 23:e5db011bd410 148 blue = 1;
relvorelvo 23:e5db011bd410 149 red = !red;
relvorelvo 23:e5db011bd410 150 for(int n=0;n<2000;n++){
relvorelvo 23:e5db011bd410 151
relvorelvo 23:e5db011bd410 152 emg3 = emg3_in.read();
relvorelvo 23:e5db011bd410 153 emg3highfilter = filterhigh3.step(emg3);
relvorelvo 23:e5db011bd410 154 emg3notchfilter = filternotch3.step(emg3highfilter);
relvorelvo 23:e5db011bd410 155 emg3abs = fabs(emg3notchfilter);
relvorelvo 23:e5db011bd410 156 emg3lowfilter = filterlow3.step(emg3abs);
relvorelvo 23:e5db011bd410 157 emg3peak = filterpeak3.step(emg3lowfilter);
relvorelvo 23:e5db011bd410 158
relvorelvo 23:e5db011bd410 159 if (max3<emg3peak){
relvorelvo 26:c9ba45bdd5c9 160 max3 = emg3peak; /* set the max value at the highest measured value */
relvorelvo 23:e5db011bd410 161 }
relvorelvo 23:e5db011bd410 162 wait(0.001f);
relvorelvo 23:e5db011bd410 163 }
relvorelvo 23:e5db011bd410 164 wait(0.2f);
relvorelvo 23:e5db011bd410 165 red = 1;
relvorelvo 23:e5db011bd410 166 }
relvorelvo 26:c9ba45bdd5c9 167 maxpart3 = 0.25*max3; /* set cut off voltage at 25% of max for right biceps */
vsluiter 2:e314bb3b2d99 168 }
vsluiter 0:32bb76391d89 169
relvorelvo 23:e5db011bd410 170 // Filtering & Scope
relvorelvo 23:e5db011bd410 171 void filter() {
relvorelvo 23:e5db011bd410 172 // Right Biceps
relvorelvo 23:e5db011bd410 173 emg1 = emg1_in.read();
relvorelvo 23:e5db011bd410 174 emg1highfilter = filterhigh1.step(emg1);
relvorelvo 23:e5db011bd410 175 emg1notchfilter = filternotch1.step(emg1highfilter);
relvorelvo 23:e5db011bd410 176 emg1abs = fabs(emg1notchfilter);
relvorelvo 23:e5db011bd410 177 emg1lowfilter = filterlow1.step(emg1abs);
relvorelvo 26:c9ba45bdd5c9 178 emg1peak = filterpeak1.step(emg1lowfilter); /* Final Right Biceps values to be sent */
relvorelvo 23:e5db011bd410 179 // Left Biceps
relvorelvo 23:e5db011bd410 180 emg2 = emg2_in.read();
relvorelvo 23:e5db011bd410 181 emg2highfilter = filterhigh2.step(emg2);
relvorelvo 23:e5db011bd410 182 emg2notchfilter = filternotch2.step(emg2highfilter);
relvorelvo 23:e5db011bd410 183 emg2abs = fabs(emg2notchfilter);
relvorelvo 23:e5db011bd410 184 emg2lowfilter = filterlow2.step(emg2abs);
relvorelvo 26:c9ba45bdd5c9 185 emg2peak = filterpeak2.step(emg2lowfilter); /* Final Left Biceps values to be sent */
relvorelvo 23:e5db011bd410 186 // Left Lower Arm OR Triceps
relvorelvo 23:e5db011bd410 187 emg3 = emg3_in.read();
relvorelvo 23:e5db011bd410 188 emg3highfilter = filterhigh3.step(emg3);
relvorelvo 23:e5db011bd410 189 emg3notchfilter = filternotch3.step(emg3highfilter);
relvorelvo 23:e5db011bd410 190 emg3abs = fabs(emg3notchfilter);
relvorelvo 23:e5db011bd410 191 emg3lowfilter = filterlow3.step(emg3abs);
relvorelvo 26:c9ba45bdd5c9 192 emg3peak = filterpeak3.step(emg3lowfilter); /* Final Lower Arm values to be sent */
Revave 27:0dbd4fd88593 193 int count;
Revave 27:0dbd4fd88593 194 QEI Encoder(D12,D13,NC,64);
relvorelvo 23:e5db011bd410 195 /* Compare measurement to the calibrated value to decide actions */
relvorelvo 26:c9ba45bdd5c9 196 if (maxpart1<emg1peak){ /* See if right biceps is contracting */
relvorelvo 23:e5db011bd410 197 red = 0;
relvorelvo 23:e5db011bd410 198 blue = 1;
relvorelvo 23:e5db011bd410 199 green = 1;
Revave 27:0dbd4fd88593 200
Revave 27:0dbd4fd88593 201 switch (modesw)
Revave 27:0dbd4fd88593 202 {
Revave 27:0dbd4fd88593 203 case 0:
Revave 27:0dbd4fd88593 204 {
Revave 27:0dbd4fd88593 205 modesw = 1;
Revave 27:0dbd4fd88593 206 break;
Revave 27:0dbd4fd88593 207 }
Revave 27:0dbd4fd88593 208 case 1:
Revave 27:0dbd4fd88593 209 {
Revave 27:0dbd4fd88593 210 modesw = 0;
Revave 27:0dbd4fd88593 211 break;
Revave 27:0dbd4fd88593 212 }
Revave 27:0dbd4fd88593 213
relvorelvo 23:e5db011bd410 214 }
relvorelvo 23:e5db011bd410 215 else {
relvorelvo 26:c9ba45bdd5c9 216 if (maxpart2<emg2peak){ /* See if left biceps is contracting */
relvorelvo 23:e5db011bd410 217 red = 1;
relvorelvo 23:e5db011bd410 218 blue = 0;
relvorelvo 23:e5db011bd410 219 green = 1;
Revave 27:0dbd4fd88593 220
Revave 27:0dbd4fd88593 221 switch (modesw)
Revave 27:0dbd4fd88593 222 {
Revave 27:0dbd4fd88593 223 case 0:
Revave 27:0dbd4fd88593 224 {
Revave 27:0dbd4fd88593 225 m1direction = 1;
Revave 27:0dbd4fd88593 226 motorpwm1.write(totalpwm1);
Revave 27:0dbd4fd88593 227 break;
Revave 27:0dbd4fd88593 228 }
Revave 27:0dbd4fd88593 229 case 1:
Revave 27:0dbd4fd88593 230 {
Revave 27:0dbd4fd88593 231 count = Encoder.getPulses();
Revave 27:0dbd4fd88593 232 m2direction = 1;
Revave 27:0dbd4fd88593 233 if (count<lowerlimit
Revave 27:0dbd4fd88593 234 motorpwm2.write(totalpwm2);
Revave 27:0dbd4fd88593 235 break;
Revave 27:0dbd4fd88593 236 }
Revave 27:0dbd4fd88593 237 }
Revave 27:0dbd4fd88593 238
relvorelvo 23:e5db011bd410 239 }
relvorelvo 23:e5db011bd410 240
relvorelvo 23:e5db011bd410 241 else {
relvorelvo 26:c9ba45bdd5c9 242 if (maxpart3<emg3peak){ /* See if lower arm is contracting */
relvorelvo 23:e5db011bd410 243 red = 1;
relvorelvo 23:e5db011bd410 244 blue = 1;
relvorelvo 23:e5db011bd410 245 green = 0;
Revave 27:0dbd4fd88593 246
Revave 27:0dbd4fd88593 247 switch (modesw)
Revave 27:0dbd4fd88593 248 {
Revave 27:0dbd4fd88593 249 case 0:
Revave 27:0dbd4fd88593 250 {
Revave 27:0dbd4fd88593 251 m1direction = 0;
Revave 27:0dbd4fd88593 252 motorpwm1.write(totalpwm1);
Revave 27:0dbd4fd88593 253 break;
Revave 27:0dbd4fd88593 254 }
Revave 27:0dbd4fd88593 255 case 1:
Revave 27:0dbd4fd88593 256 {
Revave 27:0dbd4fd88593 257 m1direction = 0;
Revave 27:0dbd4fd88593 258 motorpwm2.write(totalpwm1);
Revave 27:0dbd4fd88593 259 break;
Revave 27:0dbd4fd88593 260 }
relvorelvo 23:e5db011bd410 261 }
relvorelvo 23:e5db011bd410 262
relvorelvo 23:e5db011bd410 263 else {
relvorelvo 26:c9ba45bdd5c9 264 red = 1; /* Shut down all led colors if no movement is registered */
relvorelvo 23:e5db011bd410 265 blue = 1;
relvorelvo 23:e5db011bd410 266 green = 1;
relvorelvo 23:e5db011bd410 267 }
relvorelvo 23:e5db011bd410 268 }
relvorelvo 23:e5db011bd410 269 }
relvorelvo 23:e5db011bd410 270 /* 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 271 scope.set(0, emg1peak ); /* plot Right biceps voltage */
relvorelvo 26:c9ba45bdd5c9 272 scope.set(1, emg2peak ); /* Plot Left biceps voltage */
relvorelvo 26:c9ba45bdd5c9 273 scope.set(2, maxpart1 ); /* Show the line above which the motor should run for right biceps */
relvorelvo 26:c9ba45bdd5c9 274 scope.set(3, emg3peak ); /* Plot Lower Arm voltage */
relvorelvo 26:c9ba45bdd5c9 275 scope.set(4, maxpart3 ); /* Plot the line above which the motor should run for lower arm */
relvorelvo 23:e5db011bd410 276
relvorelvo 25:07187cf76863 277 scope.send(); /* send everything to the HID scope */
relvorelvo 23:e5db011bd410 278 }
tomlankhorst 15:0da764eea774 279
relvorelvo 23:e5db011bd410 280 int main(){
relvorelvo 22:68ab712b62b2 281
relvorelvo 25:07187cf76863 282 main_timer.attach(&filter, 0.001); /* set frequency for the filters at 1000Hz */
relvorelvo 25:07187cf76863 283 max_read1.attach(&get_max1, 2); /* set the frequency of the calibration loop at 0.5Hz */
relvorelvo 23:e5db011bd410 284 max_read3.attach(&get_max3, 2);
tomlankhorst 15:0da764eea774 285 while(1) {}
vsluiter 0:32bb76391d89 286 }