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@28:ec0763106a2e, 2017-10-10 (annotated)

Committer:
ralphg_92
Date:
Tue Oct 10 14:17:03 2017 +0000
Revision:
28:ec0763106a2e
Parent:
27:ca07f895f999
Child:
29:09c1567d6148
nu nog onoverzichtelijker

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"
ralphg_92 28:ec0763106a2e 5 #include "Calibration.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 27:ca07f895f999 12 DigitalIn max_reader12( SW2 );
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
relvorelvo 23:e5db011bd410 27 double emg1;
relvorelvo 23:e5db011bd410 28 double emg1highfilter;
relvorelvo 23:e5db011bd410 29 double emg1notchfilter;
relvorelvo 23:e5db011bd410 30 double emg1abs;
relvorelvo 23:e5db011bd410 31 double emg1lowfilter;
relvorelvo 23:e5db011bd410 32 double emg1peak;
ralphg_92 27:ca07f895f999 33 double max1;
relvorelvo 24:26659f1de039 34 double maxpart1;
relvorelvo 23:e5db011bd410 35 // Left Biceps
relvorelvo 23:e5db011bd410 36 double emg2;
relvorelvo 23:e5db011bd410 37 double emg2highfilter;
relvorelvo 23:e5db011bd410 38 double emg2notchfilter;
relvorelvo 23:e5db011bd410 39 double emg2abs;
relvorelvo 23:e5db011bd410 40 double emg2lowfilter;
relvorelvo 23:e5db011bd410 41 double emg2peak;
ralphg_92 27:ca07f895f999 42 double max2;
relvorelvo 24:26659f1de039 43 double maxpart2;
ralphg_92 27:ca07f895f999 44 // Left Lower Arm
relvorelvo 23:e5db011bd410 45 double emg3;
relvorelvo 23:e5db011bd410 46 double emg3highfilter;
relvorelvo 23:e5db011bd410 47 double emg3notchfilter;
relvorelvo 23:e5db011bd410 48 double emg3abs;
relvorelvo 23:e5db011bd410 49 double emg3lowfilter;
relvorelvo 23:e5db011bd410 50 double emg3peak;
relvorelvo 23:e5db011bd410 51 double max3;
relvorelvo 23:e5db011bd410 52 double maxpart3;
ralphg_92 27:ca07f895f999 53 // Right Lower Arm
ralphg_92 27:ca07f895f999 54 double emg4;
ralphg_92 27:ca07f895f999 55 double emg4highfilter;
ralphg_92 27:ca07f895f999 56 double emg4notchfilter;
ralphg_92 27:ca07f895f999 57 double emg4abs;
ralphg_92 27:ca07f895f999 58 double emg4lowfilter;
ralphg_92 27:ca07f895f999 59 double emg4peak;
ralphg_92 27:ca07f895f999 60 double max4;
ralphg_92 27:ca07f895f999 61 double maxpart4;
vsluiter 2:e314bb3b2d99 62
relvorelvo 25:07187cf76863 63 // BiQuad Filter Settings
relvorelvo 23:e5db011bd410 64 // Right Biceps
relvorelvo 23:e5db011bd410 65 BiQuad filterhigh1(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01); /* Filter at 10 Hz */
relvorelvo 23:e5db011bd410 66 BiQuad filternotch1(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01); /* Filter at 50 Hz */
relvorelvo 25:07187cf76863 67 BiQuad filterpeak1(1.0033,-1.984,9.852e-01,-1.9838,9.8855e-01); /* 4dB Gain peak at 11 Hz */
relvorelvo 24:26659f1de039 68 BiQuad filterlow1(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01); /* Filter at 15 Hz */
relvorelvo 23:e5db011bd410 69 // Left Biceps
relvorelvo 23:e5db011bd410 70 BiQuad filterhigh2(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01);
relvorelvo 23:e5db011bd410 71 BiQuad filternotch2(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01);
relvorelvo 24:26659f1de039 72 BiQuad filterpeak2(1.0033,-1.984,9.852e-01,-1.9838,9.8855e-01);
relvorelvo 24:26659f1de039 73 BiQuad filterlow2(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
relvorelvo 23:e5db011bd410 74 // Left Lower Arm OR Triceps
relvorelvo 23:e5db011bd410 75 BiQuad filterhigh3(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01);
relvorelvo 23:e5db011bd410 76 BiQuad filternotch3(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01);
relvorelvo 24:26659f1de039 77 BiQuad filterpeak3(1.0033,-1.984,9.852e-01,-1.9838,9.8855e-01);
relvorelvo 24:26659f1de039 78 BiQuad filterlow3(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
ralphg_92 27:ca07f895f999 79 // Right Lower Arm OR Triceps
ralphg_92 27:ca07f895f999 80 BiQuad filterhigh4(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01);
ralphg_92 27:ca07f895f999 81 BiQuad filternotch4(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01);
ralphg_92 27:ca07f895f999 82 BiQuad filterpeak4(1.0033,-1.984,9.852e-01,-1.9838,9.8855e-01);
ralphg_92 27:ca07f895f999 83 BiQuad filterlow4(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
relvorelvo 23:e5db011bd410 84 //
relvorelvo 21:136a1ab8163c 85
relvorelvo 23:e5db011bd410 86 void get_max3(){
ralphg_92 27:ca07f895f999 87 if (max_reader34==0){
relvorelvo 23:e5db011bd410 88 green = 1;
relvorelvo 23:e5db011bd410 89 blue = 1;
relvorelvo 23:e5db011bd410 90 red = !red;
relvorelvo 23:e5db011bd410 91 for(int n=0;n<2000;n++){
relvorelvo 23:e5db011bd410 92
relvorelvo 23:e5db011bd410 93 emg3 = emg3_in.read();
relvorelvo 23:e5db011bd410 94 emg3highfilter = filterhigh3.step(emg3);
relvorelvo 23:e5db011bd410 95 emg3notchfilter = filternotch3.step(emg3highfilter);
relvorelvo 23:e5db011bd410 96 emg3abs = fabs(emg3notchfilter);
relvorelvo 23:e5db011bd410 97 emg3lowfilter = filterlow3.step(emg3abs);
relvorelvo 23:e5db011bd410 98 emg3peak = filterpeak3.step(emg3lowfilter);
relvorelvo 23:e5db011bd410 99
ralphg_92 27:ca07f895f999 100 emg4 = emg4_in.read();
ralphg_92 27:ca07f895f999 101 emg4highfilter = filterhigh4.step(emg4);
ralphg_92 27:ca07f895f999 102 emg4notchfilter = filternotch4.step(emg4highfilter);
ralphg_92 27:ca07f895f999 103 emg4abs = fabs(emg4notchfilter);
ralphg_92 27:ca07f895f999 104 emg4lowfilter = filterlow4.step(emg4abs);
ralphg_92 27:ca07f895f999 105 emg4peak = filterpeak4.step(emg4lowfilter);
ralphg_92 27:ca07f895f999 106
relvorelvo 23:e5db011bd410 107 if (max3<emg3peak){
relvorelvo 26:c9ba45bdd5c9 108 max3 = emg3peak; /* set the max value at the highest measured value */
relvorelvo 23:e5db011bd410 109 }
ralphg_92 27:ca07f895f999 110 if (max4<emg4peak){
ralphg_92 27:ca07f895f999 111 max4 = emg4peak; /* set the max value at the highest measured value */
ralphg_92 27:ca07f895f999 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 }
ralphg_92 28:ec0763106a2e 118 maxpart3 = 0.2*max3; /* set cut off voltage at 25% of max for left lower arm */
ralphg_92 28:ec0763106a2e 119 maxpart4 = 0.2*max4; /* set cut off voltage at 25% of max for right lower arm */
vsluiter 2:e314bb3b2d99 120 }
vsluiter 0:32bb76391d89 121
relvorelvo 23:e5db011bd410 122 // Filtering & Scope
relvorelvo 23:e5db011bd410 123 void filter() {
relvorelvo 23:e5db011bd410 124 // Right Biceps
relvorelvo 23:e5db011bd410 125 emg1 = emg1_in.read();
relvorelvo 23:e5db011bd410 126 emg1highfilter = filterhigh1.step(emg1);
relvorelvo 23:e5db011bd410 127 emg1notchfilter = filternotch1.step(emg1highfilter);
relvorelvo 23:e5db011bd410 128 emg1abs = fabs(emg1notchfilter);
relvorelvo 23:e5db011bd410 129 emg1lowfilter = filterlow1.step(emg1abs);
relvorelvo 26:c9ba45bdd5c9 130 emg1peak = filterpeak1.step(emg1lowfilter); /* Final Right Biceps values to be sent */
relvorelvo 23:e5db011bd410 131 // Left Biceps
relvorelvo 23:e5db011bd410 132 emg2 = emg2_in.read();
relvorelvo 23:e5db011bd410 133 emg2highfilter = filterhigh2.step(emg2);
relvorelvo 23:e5db011bd410 134 emg2notchfilter = filternotch2.step(emg2highfilter);
relvorelvo 23:e5db011bd410 135 emg2abs = fabs(emg2notchfilter);
relvorelvo 23:e5db011bd410 136 emg2lowfilter = filterlow2.step(emg2abs);
relvorelvo 26:c9ba45bdd5c9 137 emg2peak = filterpeak2.step(emg2lowfilter); /* Final Left Biceps values to be sent */
relvorelvo 23:e5db011bd410 138 // Left Lower Arm OR Triceps
relvorelvo 23:e5db011bd410 139 emg3 = emg3_in.read();
relvorelvo 23:e5db011bd410 140 emg3highfilter = filterhigh3.step(emg3);
relvorelvo 23:e5db011bd410 141 emg3notchfilter = filternotch3.step(emg3highfilter);
relvorelvo 23:e5db011bd410 142 emg3abs = fabs(emg3notchfilter);
relvorelvo 23:e5db011bd410 143 emg3lowfilter = filterlow3.step(emg3abs);
relvorelvo 26:c9ba45bdd5c9 144 emg3peak = filterpeak3.step(emg3lowfilter); /* Final Lower Arm values to be sent */
ralphg_92 27:ca07f895f999 145 // Right Lower Arm OR Triceps
ralphg_92 27:ca07f895f999 146 emg4 = emg4_in.read();
ralphg_92 27:ca07f895f999 147 emg4highfilter = filterhigh4.step(emg4);
ralphg_92 27:ca07f895f999 148 emg4notchfilter = filternotch4.step(emg4highfilter);
ralphg_92 27:ca07f895f999 149 emg4abs = fabs(emg4notchfilter);
ralphg_92 27:ca07f895f999 150 emg4lowfilter = filterlow4.step(emg4abs);
ralphg_92 27:ca07f895f999 151 emg4peak = filterpeak4.step(emg4lowfilter); /* Final Lower Arm values to be sent */
relvorelvo 23:e5db011bd410 152
relvorelvo 23:e5db011bd410 153
relvorelvo 23:e5db011bd410 154 /* Compare measurement to the calibrated value to decide actions */
relvorelvo 26:c9ba45bdd5c9 155 if (maxpart1<emg1peak){ /* See if right biceps is contracting */
relvorelvo 23:e5db011bd410 156 red = 0;
relvorelvo 23:e5db011bd410 157 blue = 1;
relvorelvo 23:e5db011bd410 158 green = 1;
relvorelvo 23:e5db011bd410 159 }
relvorelvo 23:e5db011bd410 160 else {
relvorelvo 26:c9ba45bdd5c9 161 if (maxpart2<emg2peak){ /* See if left biceps is contracting */
relvorelvo 23:e5db011bd410 162 red = 1;
relvorelvo 23:e5db011bd410 163 blue = 0;
relvorelvo 23:e5db011bd410 164 green = 1;
relvorelvo 23:e5db011bd410 165 }
relvorelvo 23:e5db011bd410 166
relvorelvo 23:e5db011bd410 167 else {
ralphg_92 27:ca07f895f999 168 if (maxpart3<emg3peak){ /* See if lower left arm is contracting */
relvorelvo 23:e5db011bd410 169 red = 1;
relvorelvo 23:e5db011bd410 170 blue = 1;
relvorelvo 23:e5db011bd410 171 green = 0;
relvorelvo 23:e5db011bd410 172 }
ralphg_92 27:ca07f895f999 173 else {
ralphg_92 27:ca07f895f999 174 if (maxpart4<emg3peak){ /* See if lower right arm is contracting */
ralphg_92 27:ca07f895f999 175 red = 0;
ralphg_92 27:ca07f895f999 176 blue = 0;
ralphg_92 27:ca07f895f999 177 green = 0;
ralphg_92 27:ca07f895f999 178 }
relvorelvo 23:e5db011bd410 179 else {
relvorelvo 26:c9ba45bdd5c9 180 red = 1; /* Shut down all led colors if no movement is registered */
relvorelvo 23:e5db011bd410 181 blue = 1;
relvorelvo 23:e5db011bd410 182 green = 1;
relvorelvo 23:e5db011bd410 183 }
relvorelvo 23:e5db011bd410 184 }
relvorelvo 23:e5db011bd410 185 }
ralphg_92 27:ca07f895f999 186 }
relvorelvo 23:e5db011bd410 187 /* 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 188 scope.set(0, emg1peak ); /* plot Right biceps voltage */
relvorelvo 26:c9ba45bdd5c9 189 scope.set(1, emg2peak ); /* Plot Left biceps voltage */
ralphg_92 27:ca07f895f999 190 scope.set(2, emg3peak ); /* Plot Lower Left Arm voltage */
ralphg_92 27:ca07f895f999 191 scope.set(3, emg4peak ); /* Plot Lower Right Arm Voltage */
relvorelvo 23:e5db011bd410 192
relvorelvo 25:07187cf76863 193 scope.send(); /* send everything to the HID scope */
relvorelvo 23:e5db011bd410 194 }
tomlankhorst 15:0da764eea774 195
relvorelvo 23:e5db011bd410 196 int main(){
relvorelvo 22:68ab712b62b2 197
relvorelvo 25:07187cf76863 198 main_timer.attach(&filter, 0.001); /* set frequency for the filters at 1000Hz */
relvorelvo 25:07187cf76863 199 max_read1.attach(&get_max1, 2); /* set the frequency of the calibration loop at 0.5Hz */
relvorelvo 23:e5db011bd410 200 max_read3.attach(&get_max3, 2);
tomlankhorst 15:0da764eea774 201 while(1) {}
vsluiter 0:32bb76391d89 202 }