Ralph Gerlings / Mbed 2 deprecated EMG_5older

try this for a change

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of EMG_5 by Ralph Gerlings

main.cpp@43:a6fbcada47aa, 2017-11-03 (annotated)

Committer:
ralphg_92
Date:
Fri Nov 03 08:38:14 2017 +0000
Revision:
43:a6fbcada47aa
Parent:
42:3aa03b5cefb0 
revisiting old memories

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 43:a6fbcada47aa 5 //#include "QEI.h"
vsluiter 0:32bb76391d89 6
ralphg_92 32:a779b1131977 7 //Define Objects
ralphg_92 32:a779b1131977 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 32:a779b1131977 12 DigitalIn max_reader12( SW2 ); // Define Button Press
ralphg_92 27:ca07f895f999 13 DigitalIn max_reader34( SW3 );
ralphg_92 31:d346f9244b4a 14
ralphg_92 31:d346f9244b4a 15 DigitalOut motor1direction( D4 );
ralphg_92 31:d346f9244b4a 16 PwmOut motor1pwm( D5);
ralphg_92 31:d346f9244b4a 17 PwmOut motor2pwm( D6 );
ralphg_92 31:d346f9244b4a 18 DigitalOut motor2direction( D7 );
ralphg_92 43:a6fbcada47aa 19 //QEI Encoder1(D10, D11, NC, 32); // Encoder
ralphg_92 43:a6fbcada47aa 20 //QEI Encoder2(D12, D13, NC, 32);
relvorelvo 23:e5db011bd410 21
relvorelvo 23:e5db011bd410 22 Ticker main_timer;
relvorelvo 23:e5db011bd410 23 Ticker max_read1;
relvorelvo 23:e5db011bd410 24 Ticker max_read3;
ralphg_92 31:d346f9244b4a 25 Ticker Motorcontrol;
ralphg_92 43:a6fbcada47aa 26 HIDScope scope( 6 );
relvorelvo 23:e5db011bd410 27 DigitalOut red(LED_RED);
relvorelvo 23:e5db011bd410 28 DigitalOut blue(LED_BLUE);
relvorelvo 23:e5db011bd410 29 DigitalOut green(LED_GREEN);
ralphg_92 43:a6fbcada47aa 30 MODSERIAL pc(USBTX, USBRX);
relvorelvo 21:136a1ab8163c 31
ralphg_92 43:a6fbcada47aa 32
ralphg_92 43:a6fbcada47aa 33 // EMG Variables
relvorelvo 23:e5db011bd410 34 //Right Biceps
relvorelvo 23:e5db011bd410 35 double emg1;
relvorelvo 23:e5db011bd410 36 double emg1highfilter;
relvorelvo 23:e5db011bd410 37 double emg1notchfilter;
relvorelvo 23:e5db011bd410 38 double emg1abs;
relvorelvo 23:e5db011bd410 39 double emg1lowfilter;
ralphg_92 27:ca07f895f999 40 double max1;
relvorelvo 24:26659f1de039 41 double maxpart1;
relvorelvo 23:e5db011bd410 42 // Left Biceps
relvorelvo 23:e5db011bd410 43 double emg2;
relvorelvo 23:e5db011bd410 44 double emg2highfilter;
relvorelvo 23:e5db011bd410 45 double emg2notchfilter;
relvorelvo 23:e5db011bd410 46 double emg2abs;
relvorelvo 23:e5db011bd410 47 double emg2lowfilter;
ralphg_92 27:ca07f895f999 48 double max2;
relvorelvo 24:26659f1de039 49 double maxpart2;
ralphg_92 27:ca07f895f999 50 // Left Lower Arm
relvorelvo 23:e5db011bd410 51 double emg3;
relvorelvo 23:e5db011bd410 52 double emg3highfilter;
relvorelvo 23:e5db011bd410 53 double emg3notchfilter;
relvorelvo 23:e5db011bd410 54 double emg3abs;
relvorelvo 23:e5db011bd410 55 double emg3lowfilter;
relvorelvo 23:e5db011bd410 56 double max3;
relvorelvo 23:e5db011bd410 57 double maxpart3;
ralphg_92 27:ca07f895f999 58 // Right Lower Arm
ralphg_92 27:ca07f895f999 59 double emg4;
ralphg_92 27:ca07f895f999 60 double emg4highfilter;
ralphg_92 27:ca07f895f999 61 double emg4notchfilter;
ralphg_92 27:ca07f895f999 62 double emg4abs;
ralphg_92 27:ca07f895f999 63 double emg4lowfilter;
ralphg_92 27:ca07f895f999 64 double max4;
ralphg_92 27:ca07f895f999 65 double maxpart4;
ralphg_92 43:a6fbcada47aa 66 // Moving Average Floats
ralphg_92 32:a779b1131977 67 // Right Biceps Movavg
ralphg_92 32:a779b1131977 68 float RB0, RB1, RB2, RB3, RB4, RB5, RB6, RB7, RB8, RB9, RB10, RB11, RB12,
ralphg_92 32:a779b1131977 69 RB13, RB14, RB15, RB16, RB17, RB18, RB19, RB20, RB21, RB22, RB23, RB24,
ralphg_92 32:a779b1131977 70 RB25, RB26, RB27, RB28, RB29, RB30, RB31, RB32, RB33, RB34, RB35, RB36,
ralphg_92 32:a779b1131977 71 RB37, RB38, RB39, RB40, RB41, RB42, RB43, RB44, RB45, RB46, RB47, RB48,
ralphg_92 32:a779b1131977 72 RB49, RB50, RB51, RB52, RB53, RB54, RB55, RB56, RB57, RB58, RB59, RB60,
ralphg_92 32:a779b1131977 73 RB61, RB62, RB63, RB64, RB65, RB66, RB67, RB68, RB69, RB70, RB71, RB72,
ralphg_92 32:a779b1131977 74 RB73, RB74, RB75, RB76, RB77, RB78, RB79, RB80, RB81, RB82, RB83, RB84,
ralphg_92 32:a779b1131977 75 RB85, RB86, RB87, MOVAVG_RB;
ralphg_92 32:a779b1131977 76 // Left Biceps Movavg
ralphg_92 32:a779b1131977 77 float LB0, LB1, LB2, LB3, LB4, LB5, LB6, LB7, LB8, LB9, LB10, LB11, LB12,
ralphg_92 32:a779b1131977 78 LB13, LB14, LB15, LB16, LB17, LB18, LB19, LB20, LB21, LB22, LB23, LB24,
ralphg_92 32:a779b1131977 79 LB25, LB26, LB27, LB28, LB29, LB30, LB31, LB32, LB33, LB34, LB35, LB36,
ralphg_92 32:a779b1131977 80 LB37, LB38, LB39, LB40, LB41, LB42, LB43, LB44, LB45, LB46, LB47, LB48,
ralphg_92 32:a779b1131977 81 LB49, LB50, LB51, LB52, LB53, LB54, LB55, LB56, LB57, LB58, LB59, LB60,
ralphg_92 32:a779b1131977 82 LB61, LB62, LB63, LB64, LB65, LB66, LB67, LB68, LB69, LB70, LB71, LB72,
ralphg_92 32:a779b1131977 83 LB73, LB74, LB75, LB76, LB77, LB78, LB79, LB80, LB81, LB82, LB83, LB84,
ralphg_92 32:a779b1131977 84 LB85, LB86, LB87, MOVAVG_LB;
ralphg_92 32:a779b1131977 85 // Left Lower Arm Movavg
ralphg_92 32:a779b1131977 86 float LL0, LL1, LL2, LL3, LL4, LL5, LL6, LL7, LL8, LL9, LL10, LL11, LL12,
ralphg_92 32:a779b1131977 87 LL13, LL14, LL15, LL16, LL17, LL18, LL19, LL20, LL21, LL22, LL23, LL24,
ralphg_92 32:a779b1131977 88 LL25, LL26, LL27, LL28, LL29, LL30, LL31, LL32, LL33, LL34, LL35, LL36,
ralphg_92 32:a779b1131977 89 LL37, LL38, LL39, LL40, LL41, LL42, LL43, LL44, LL45, LL46, LL47, LL48,
ralphg_92 32:a779b1131977 90 LL49, LL50, LL51, LL52, LL53, LL54, LL55, LL56, LL57, LL58, LL59, LL60,
ralphg_92 32:a779b1131977 91 LL61, LL62, LL63, LL64, LL65, LL66, LL67, LL68, LL69, LL70, LL71, LL72,
ralphg_92 32:a779b1131977 92 LL73, LL74, LL75, LL76, LL77, LL78, LL79, LL80, LL81, LL82, LL83, LL84,
ralphg_92 32:a779b1131977 93 LL85, LL86, LL87, MOVAVG_LL;
ralphg_92 32:a779b1131977 94 // Right Lower Arm Movavg
ralphg_92 32:a779b1131977 95 float RL0, RL1, RL2, RL3, RL4, RL5, RL6, RL7, RL8, RL9, RL10, RL11, RL12,
ralphg_92 32:a779b1131977 96 RL13, RL14, RL15, RL16, RL17, RL18, RL19, RL20, RL21, RL22, RL23, RL24,
ralphg_92 32:a779b1131977 97 RL25, RL26, RL27, RL28, RL29, RL30, RL31, RL32, RL33, RL34, RL35, RL36,
ralphg_92 32:a779b1131977 98 RL37, RL38, RL39, RL40, RL41, RL42, RL43, RL44, RL45, RL46, RL47, RL48,
ralphg_92 32:a779b1131977 99 RL49, RL50, RL51, RL52, RL53, RL54, RL55, RL56, RL57, RL58, RL59, RL60,
ralphg_92 32:a779b1131977 100 RL61, RL62, RL63, RL64, RL65, RL66, RL67, RL68, RL69, RL70, RL71, RL72,
ralphg_92 32:a779b1131977 101 RL73, RL74, RL75, RL76, RL77, RL78, RL79, RL80, RL81, RL82, RL83, RL84,
ralphg_92 32:a779b1131977 102 RL85, RL86, RL87, MOVAVG_RL;
ralphg_92 32:a779b1131977 103 float AV = 0.02; //multiplication value for adding each movavg value
ralphg_92 32:a779b1131977 104 // This value also adds a very slight gain to every value
ralphg_92 31:d346f9244b4a 105
ralphg_92 43:a6fbcada47aa 106 // Motor Variables
ralphg_92 43:a6fbcada47aa 107 float MV1 = 0;
ralphg_92 43:a6fbcada47aa 108 float MV2 = 0;
ralphg_92 29:09c1567d6148 109
relvorelvo 25:07187cf76863 110 // BiQuad Filter Settings
relvorelvo 23:e5db011bd410 111 // Right Biceps
ralphg_92 31:d346f9244b4a 112 BiQuad filterhigh1(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01); // Filter at 10 Hz
ralphg_92 31:d346f9244b4a 113 BiQuad filternotch1(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01); // Filter at 50 Hz
ralphg_92 31:d346f9244b4a 114 BiQuad filterlow1(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01); // Filter at 15 Hz
relvorelvo 23:e5db011bd410 115 // Left Biceps
relvorelvo 23:e5db011bd410 116 BiQuad filterhigh2(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01);
relvorelvo 23:e5db011bd410 117 BiQuad filternotch2(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01);
relvorelvo 24:26659f1de039 118 BiQuad filterlow2(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
relvorelvo 23:e5db011bd410 119 // Left Lower Arm OR Triceps
relvorelvo 23:e5db011bd410 120 BiQuad filterhigh3(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01);
relvorelvo 23:e5db011bd410 121 BiQuad filternotch3(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01);
relvorelvo 24:26659f1de039 122 BiQuad filterlow3(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
ralphg_92 27:ca07f895f999 123 // Right Lower Arm OR Triceps
ralphg_92 27:ca07f895f999 124 BiQuad filterhigh4(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01);
ralphg_92 27:ca07f895f999 125 BiQuad filternotch4(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01);
ralphg_92 27:ca07f895f999 126 BiQuad filterlow4(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
relvorelvo 23:e5db011bd410 127 //
relvorelvo 21:136a1ab8163c 128
ralphg_92 43:a6fbcada47aa 129 // Finding max values for correct motor switch if the button is pressed
ralphg_92 43:a6fbcada47aa 130 // calibration of both biceps
ralphg_92 33:97e69c32a768 131
ralphg_92 29:09c1567d6148 132 void get_max1(){
ralphg_92 29:09c1567d6148 133 if (max_reader12==0){
ralphg_92 29:09c1567d6148 134 green = !green;
ralphg_92 29:09c1567d6148 135 red = 1;
ralphg_92 29:09c1567d6148 136 blue = 1;
ralphg_92 31:d346f9244b4a 137 for(int n=0;n<2000;n++){ // measure 2000 samples and filter it
ralphg_92 29:09c1567d6148 138
ralphg_92 31:d346f9244b4a 139 emg1 = emg1_in.read(); // read out emg
ralphg_92 31:d346f9244b4a 140 emg1highfilter = filterhigh1.step(emg1); // high pass filtered
ralphg_92 31:d346f9244b4a 141 emg1notchfilter = filternotch1.step(emg1highfilter); // notch filtered
ralphg_92 31:d346f9244b4a 142 emg1abs = fabs(emg1notchfilter); // take the absolute value
ralphg_92 31:d346f9244b4a 143 emg1lowfilter = filterlow1.step(emg1abs); // low pass filtered
ralphg_92 32:a779b1131977 144 RB0 = emg1lowfilter;
ralphg_92 32:a779b1131977 145 // Movavg is defined by rb0-rb75 and rb1 to rb75 are redefined by the value of rbx-1
ralphg_92 32:a779b1131977 146 MOVAVG_RB = RB0*AV+RB1*AV+RB2*AV+RB3*AV+RB4*AV+RB5*AV+RB6*AV+RB7*AV+RB8*AV
ralphg_92 32:a779b1131977 147 +RB9*AV+RB10*AV+RB11*AV+RB12*AV+RB13*AV+RB14*AV+RB15*AV+RB16*AV+RB17*AV+RB18
ralphg_92 32:a779b1131977 148 *AV+RB19*AV+RB20*AV+RB21*AV+RB22*AV+RB23*AV+RB24*AV+RB25*AV+RB26*AV+RB27
ralphg_92 32:a779b1131977 149 *AV+RB28*AV+RB29*AV+RB30*AV+RB31*AV+RB32*AV+RB33*AV+RB34*AV+RB35*AV+RB36*AV
ralphg_92 32:a779b1131977 150 +RB37*AV+RB38*AV+RB39*AV+RB40*AV+RB41*AV+RB42*AV+RB43*AV+RB44*AV+RB45*AV
ralphg_92 32:a779b1131977 151 +RB46*AV+RB47*AV+RB48*AV+RB49*AV+RB50*AV+RB51*AV+RB52*AV+RB53*AV+RB54*AV
ralphg_92 32:a779b1131977 152 +RB55*AV+RB56*AV+RB57*AV+RB58*AV+RB59*AV+RB60*AV+RB61*AV+RB62*AV+RB63*AV
ralphg_92 32:a779b1131977 153 +RB64*AV+RB65*AV+RB66*AV+RB67*AV+RB68*AV+RB69*AV+RB70*AV+RB71*AV+RB72*AV
ralphg_92 32:a779b1131977 154 +RB73*AV+RB74*AV+RB75*AV+RB76*AV+RB77*AV+RB78*AV+RB79*AV+RB80*AV+RB81*AV
ralphg_92 32:a779b1131977 155 +RB82*AV+RB83*AV+RB84*AV+RB85*AV+RB86*AV+RB87*AV;
ralphg_92 32:a779b1131977 156 RB87 = RB86; RB86 = RB85; RB85 = RB84; RB84 = RB83; RB83 = RB82; RB82 = RB81;
ralphg_92 32:a779b1131977 157 RB81 = RB80; RB80 = RB79; RB79 = RB78; RB78 = RB77; RB77 = RB76; RB76 = RB75;
ralphg_92 32:a779b1131977 158 RB75 = RB74; RB74 = RB73; RB73 = RB72; RB72 = RB71; RB71 = RB70; RB70 = RB69;
ralphg_92 32:a779b1131977 159 RB69 = RB68; RB68 = RB67; RB67 = RB66; RB66 = RB65; RB65 = RB64; RB64 = RB63;
ralphg_92 32:a779b1131977 160 RB63 = RB62; RB62 = RB61; RB61 = RB60; RB60 = RB59; RB59 = RB58; RB58 = RB57;
ralphg_92 32:a779b1131977 161 RB57 = RB56; RB56 = RB55; RB55 = RB54; RB54 = RB53; RB53 = RB52; RB52 = RB51;
ralphg_92 32:a779b1131977 162 RB51 = RB50; RB50 = RB49; RB49 = RB48; RB48 = RB47; RB47 = RB46; RB46 = RB45;
ralphg_92 32:a779b1131977 163 RB45 = RB44; RB44 = RB43; RB43 = RB42; RB42 = RB41; RB41 = RB40; RB40 = RB39;
ralphg_92 32:a779b1131977 164 RB39 = RB38; RB38 = RB37; RB37 = RB36; RB36 = RB35; RB35 = RB34; RB34 = RB33;
ralphg_92 32:a779b1131977 165 RB33 = RB32; RB32 = RB31; RB31 = RB30; RB30 = RB29; RB29 = RB28; RB28 = RB27;
ralphg_92 32:a779b1131977 166 RB27 = RB26; RB26 = RB25; RB25 = RB24; RB24 = RB23; RB23 = RB22; RB22 = RB21;
ralphg_92 32:a779b1131977 167 RB21 = RB20; RB20 = RB19; RB19 = RB18; RB18 = RB17; RB17 = RB16; RB16 = RB15;
ralphg_92 32:a779b1131977 168 RB15 = RB14; RB14 = RB13; RB13 = RB12; RB12 = RB11; RB11 = RB10; RB10 = RB9;
ralphg_92 32:a779b1131977 169 RB9 = RB8; RB8 = RB7; RB7 = RB6; RB6 = RB5; RB5 = RB4; RB4 = RB3; RB3 = RB2;
ralphg_92 32:a779b1131977 170 RB2 = RB1; RB1 = RB0;
ralphg_92 29:09c1567d6148 171
ralphg_92 31:d346f9244b4a 172 emg2 = emg2_in.read(); // read out emg
ralphg_92 31:d346f9244b4a 173 emg2highfilter = filterhigh2.step(emg2); // high pass filtered
ralphg_92 31:d346f9244b4a 174 emg2notchfilter = filternotch2.step(emg2highfilter); // notch filtered
ralphg_92 31:d346f9244b4a 175 emg2abs = fabs(emg2notchfilter); // take the absolute value
ralphg_92 31:d346f9244b4a 176 emg2lowfilter = filterlow2.step(emg2abs); // low pass filtered
ralphg_92 32:a779b1131977 177 LB0 = emg2lowfilter;
ralphg_92 32:a779b1131977 178 // same story as with the right biceps
ralphg_92 32:a779b1131977 179 MOVAVG_LB = LB0*AV+LB1*AV+LB2*AV+LB3*AV+LB4*AV+LB5*AV+LB6*AV+LB7*AV+LB8*AV
ralphg_92 32:a779b1131977 180 +LB9*AV+LB10*AV+LB11*AV+LB12*AV+LB13*AV+LB14*AV+LB15*AV+LB16*AV+LB17*AV+LB18
ralphg_92 32:a779b1131977 181 *AV+LB19*AV+LB20*AV+LB21*AV+LB22*AV+LB23*AV+LB24*AV+LB25*AV+LB26*AV+LB27
ralphg_92 32:a779b1131977 182 *AV+LB28*AV+LB29*AV+LB30*AV+LB31*AV+LB32*AV+LB33*AV+LB34*AV+LB35*AV+LB36*AV
ralphg_92 32:a779b1131977 183 +LB37*AV+LB38*AV+LB39*AV+LB40*AV+LB41*AV+LB42*AV+LB43*AV+LB44*AV+LB45*AV
ralphg_92 32:a779b1131977 184 +LB46*AV+LB47*AV+LB48*AV+LB49*AV+LB50*AV+LB51*AV+LB52*AV+LB53*AV+LB54*AV
ralphg_92 32:a779b1131977 185 +LB55*AV+LB56*AV+LB57*AV+LB58*AV+LB59*AV+LB60*AV+LB61*AV+LB62*AV+LB63*AV
ralphg_92 32:a779b1131977 186 +LB64*AV+LB65*AV+LB66*AV+LB67*AV+LB68*AV+LB69*AV+LB70*AV+LB71*AV+LB72*AV
ralphg_92 32:a779b1131977 187 +LB73*AV+LB74*AV+LB75*AV+LB76*AV+LB77*AV+LB78*AV+LB79*AV+LB80*AV+LB81*AV
ralphg_92 32:a779b1131977 188 +LB82*AV+LB83*AV+LB84*AV+LB85*AV+LB86*AV+LB87*AV;
ralphg_92 32:a779b1131977 189 LB87 = LB86; LB86 = LB85; LB85 = LB84; LB84 = LB83; LB83 = LB82; LB82 = LB81;
ralphg_92 32:a779b1131977 190 LB81 = LB80; LB80 = LB79; LB79 = LB78; LB78 = LB77; LB77 = LB76; LB76 = LB75;
ralphg_92 32:a779b1131977 191 LB75 = LB74; LB74 = LB73; LB73 = LB72; LB72 = LB71; LB71 = LB70; LB70 = LB69;
ralphg_92 32:a779b1131977 192 LB69 = LB68; LB68 = LB67; LB67 = LB66; LB66 = LB65; LB65 = LB64; LB64 = LB63;
ralphg_92 32:a779b1131977 193 LB63 = LB62; LB62 = LB61; LB61 = LB60; LB60 = LB59; LB59 = LB58; LB58 = LB57;
ralphg_92 32:a779b1131977 194 LB57 = LB56; LB56 = LB55; LB55 = LB54; LB54 = LB53; LB53 = LB52; LB52 = LB51;
ralphg_92 32:a779b1131977 195 LB51 = LB50; LB50 = LB49; LB49 = LB48; LB48 = LB47; LB47 = LB46; LB46 = LB45;
ralphg_92 32:a779b1131977 196 LB45 = LB44; LB44 = LB43; LB43 = LB42; LB42 = LB41; LB41 = LB40; LB40 = LB39;
ralphg_92 32:a779b1131977 197 LB39 = LB38; LB38 = LB37; LB37 = LB36; LB36 = LB35; LB35 = LB34; LB34 = LB33;
ralphg_92 32:a779b1131977 198 LB33 = LB32; LB32 = LB31; LB31 = LB30; LB30 = LB29; LB29 = LB28; LB28 = LB27;
ralphg_92 32:a779b1131977 199 LB27 = LB26; LB26 = LB25; LB25 = LB24; LB24 = LB23; LB23 = LB22; LB22 = LB21;
ralphg_92 32:a779b1131977 200 LB21 = LB20; LB20 = LB19; LB19 = LB18; LB18 = LB17; LB17 = LB16; LB16 = LB15;
ralphg_92 32:a779b1131977 201 LB15 = LB14; LB14 = LB13; LB13 = LB12; LB12 = LB11; LB11 = LB10; LB10 = LB9;
ralphg_92 32:a779b1131977 202 LB9 = LB8; LB8 = LB7; LB7 = LB6; LB6 = LB5; LB5 = LB4; LB4 = LB3; LB3 = LB2;
ralphg_92 32:a779b1131977 203 LB2 = LB1; LB1 = LB0;
ralphg_92 29:09c1567d6148 204
ralphg_92 32:a779b1131977 205 if (max1<MOVAVG_RB){
ralphg_92 32:a779b1131977 206 max1 = MOVAVG_RB; // set the max value at the highest measured value
ralphg_92 29:09c1567d6148 207 }
ralphg_92 32:a779b1131977 208 if (max2<MOVAVG_LB){
ralphg_92 32:a779b1131977 209 max2 = MOVAVG_LB; // set the max value at the highest measured value
ralphg_92 29:09c1567d6148 210 }
ralphg_92 31:d346f9244b4a 211 wait(0.001f); // measure at 1000Hz
ralphg_92 29:09c1567d6148 212 }
ralphg_92 29:09c1567d6148 213 wait(0.2f);
ralphg_92 29:09c1567d6148 214 green = 1;
ralphg_92 29:09c1567d6148 215 }
ralphg_92 33:97e69c32a768 216 maxpart1 = 0.15*max1; // set cut off voltage at 13% of max for right biceps
ralphg_92 32:a779b1131977 217 maxpart2 = 0.15*max2; // set cut off voltage at 13% of max for left biceps
ralphg_92 29:09c1567d6148 218 }
ralphg_92 29:09c1567d6148 219
ralphg_92 31:d346f9244b4a 220 // calibration of both lower arms
ralphg_92 43:a6fbcada47aa 221
relvorelvo 23:e5db011bd410 222 void get_max3(){
ralphg_92 27:ca07f895f999 223 if (max_reader34==0){
relvorelvo 23:e5db011bd410 224 green = 1;
relvorelvo 23:e5db011bd410 225 blue = 1;
relvorelvo 23:e5db011bd410 226 red = !red;
relvorelvo 23:e5db011bd410 227 for(int n=0;n<2000;n++){
relvorelvo 23:e5db011bd410 228
relvorelvo 23:e5db011bd410 229 emg3 = emg3_in.read();
relvorelvo 23:e5db011bd410 230 emg3highfilter = filterhigh3.step(emg3);
relvorelvo 23:e5db011bd410 231 emg3notchfilter = filternotch3.step(emg3highfilter);
relvorelvo 23:e5db011bd410 232 emg3abs = fabs(emg3notchfilter);
relvorelvo 23:e5db011bd410 233 emg3lowfilter = filterlow3.step(emg3abs);
ralphg_92 32:a779b1131977 234 LL0 = emg3lowfilter;
ralphg_92 32:a779b1131977 235 // same story as with the right biceps
ralphg_92 32:a779b1131977 236 MOVAVG_LL = LL0*AV+LL1*AV+LL2*AV+LL3*AV+LL4*AV+LL5*AV+LL6*AV+LL7*AV+LL8*AV
ralphg_92 32:a779b1131977 237 +LL9*AV+LL10*AV+LL11*AV+LL12*AV+LL13*AV+LL14*AV+LL15*AV+LL16*AV+LL17*AV+LL18
ralphg_92 32:a779b1131977 238 *AV+LL19*AV+LL20*AV+LL21*AV+LL22*AV+LL23*AV+LL24*AV+LL25*AV+LL26*AV+LL27
ralphg_92 32:a779b1131977 239 *AV+LL28*AV+LL29*AV+LL30*AV+LL31*AV+LL32*AV+LL33*AV+LL34*AV+LL35*AV+LL36*AV
ralphg_92 32:a779b1131977 240 +LL37*AV+LL38*AV+LL39*AV+LL40*AV+LL41*AV+LL42*AV+LL43*AV+LL44*AV+LL45*AV
ralphg_92 32:a779b1131977 241 +LL46*AV+LL47*AV+LL48*AV+LL49*AV+LL50*AV+LL51*AV+LL52*AV+LL53*AV+LL54*AV
ralphg_92 32:a779b1131977 242 +LL55*AV+LL56*AV+LL57*AV+LL58*AV+LL59*AV+LL60*AV+LL61*AV+LL62*AV+LL63*AV
ralphg_92 32:a779b1131977 243 +LL64*AV+LL65*AV+LL66*AV+LL67*AV+LL68*AV+LL69*AV+LL70*AV+LL71*AV+LL72*AV
ralphg_92 32:a779b1131977 244 +LL73*AV+LL74*AV+LL75*AV+LL76*AV+LL77*AV+LL78*AV+LL79*AV+LL80*AV+LL81*AV
ralphg_92 32:a779b1131977 245 +LL82*AV+LL83*AV+LL84*AV+LL85*AV+LL86*AV+LL87*AV;
ralphg_92 32:a779b1131977 246 LL87 = LL86; LL86 = LL85; LL85 = LL84; LL84 = LL83; LL83 = LL82; LL82 = LL81;
ralphg_92 32:a779b1131977 247 LL81 = LL80; LL80 = LL79; LL79 = LL78; LL78 = LL77; LL77 = LL76; LL76 = LL75;
ralphg_92 32:a779b1131977 248 LL75 = LL74; LL74 = LL73; LL73 = LL72; LL72 = LL71; LL71 = LL70; LL70 = LL69;
ralphg_92 32:a779b1131977 249 LL69 = LL68; LL68 = LL67; LL67 = LL66; LL66 = LL65; LL65 = LL64; LL64 = LL63;
ralphg_92 32:a779b1131977 250 LL63 = LL62; LL62 = LL61; LL61 = LL60; LL60 = LL59; LL59 = LL58; LL58 = LL57;
ralphg_92 32:a779b1131977 251 LL57 = LL56; LL56 = LL55; LL55 = LL54; LL54 = LL53; LL53 = LL52; LL52 = LL51;
ralphg_92 32:a779b1131977 252 LL51 = LL50; LL50 = LL49; LL49 = LL48; LL48 = LL47; LL47 = LL46; LL46 = LL45;
ralphg_92 32:a779b1131977 253 LL45 = LL44; LL44 = LL43; LL43 = LL42; LL42 = LL41; LL41 = LL40; LL40 = LL39;
ralphg_92 32:a779b1131977 254 LL39 = LL38; LL38 = LL37; LL37 = LL36; LL36 = LL35; LL35 = LL34; LL34 = LL33;
ralphg_92 32:a779b1131977 255 LL33 = LL32; LL32 = LL31; LL31 = LL30; LL30 = LL29; LL29 = LL28; LL28 = LL27;
ralphg_92 32:a779b1131977 256 LL27 = LL26; LL26 = LL25; LL25 = LL24; LL24 = LL23; LL23 = LL22; LL22 = LL21;
ralphg_92 32:a779b1131977 257 LL21 = LL20; LL20 = LL19; LL19 = LL18; LL18 = LL17; LL17 = LL16; LL16 = LL15;
ralphg_92 32:a779b1131977 258 LL15 = LL14; LL14 = LL13; LL13 = LL12; LL12 = LL11; LL11 = LL10; LL10 = LL9;
ralphg_92 32:a779b1131977 259 LL9 = LL8; LL8 = LL7; LL7 = LL6; LL6 = LL5; LL5 = LL4; LL4 = LL3; LL3 = LL2;
ralphg_92 32:a779b1131977 260 LL2 = LL1; LL1 = LL0;
relvorelvo 23:e5db011bd410 261
ralphg_92 27:ca07f895f999 262 emg4 = emg4_in.read();
ralphg_92 27:ca07f895f999 263 emg4highfilter = filterhigh4.step(emg4);
ralphg_92 27:ca07f895f999 264 emg4notchfilter = filternotch4.step(emg4highfilter);
ralphg_92 27:ca07f895f999 265 emg4abs = fabs(emg4notchfilter);
ralphg_92 27:ca07f895f999 266 emg4lowfilter = filterlow4.step(emg4abs);
ralphg_92 32:a779b1131977 267 RL0 = emg4lowfilter;
ralphg_92 32:a779b1131977 268 // same story as with the right biceps
ralphg_92 32:a779b1131977 269 MOVAVG_RL = RL0*AV+RL1*AV+RL2*AV+RL3*AV+RL4*AV+RL5*AV+RL6*AV+RL7*AV+RL8*AV
ralphg_92 32:a779b1131977 270 +RL9*AV+RL10*AV+RL11*AV+RL12*AV+RL13*AV+RL14*AV+RL15*AV+RL16*AV+RL17*AV+RL18
ralphg_92 32:a779b1131977 271 *AV+RL19*AV+RL20*AV+RL21*AV+RL22*AV+RL23*AV+RL24*AV+RL25*AV+RL26*AV+RL27
ralphg_92 32:a779b1131977 272 *AV+RL28*AV+RL29*AV+RL30*AV+RL31*AV+RL32*AV+RL33*AV+RL34*AV+RL35*AV+RL36*AV
ralphg_92 32:a779b1131977 273 +RL37*AV+RL38*AV+RL39*AV+RL40*AV+RL41*AV+RL42*AV+RL43*AV+RL44*AV+RL45*AV
ralphg_92 32:a779b1131977 274 +RL46*AV+RL47*AV+RL48*AV+RL49*AV+RL50*AV+RL51*AV+RL52*AV+RL53*AV+RL54*AV
ralphg_92 32:a779b1131977 275 +RL55*AV+RL56*AV+RL57*AV+RL58*AV+RL59*AV+RL60*AV+RL61*AV+RL62*AV+RL63*AV
ralphg_92 32:a779b1131977 276 +RL64*AV+RL65*AV+RL66*AV+RL67*AV+RL68*AV+RL69*AV+RL70*AV+RL71*AV+RL72*AV
ralphg_92 32:a779b1131977 277 +RL73*AV+RL74*AV+RL75*AV+RL76*AV+RL77*AV+RL78*AV+RL79*AV+RL80*AV+RL81*AV
ralphg_92 32:a779b1131977 278 +RL82*AV+RL83*AV+RL84*AV+RL85*AV+RL86*AV+RL87*AV;
ralphg_92 32:a779b1131977 279 RL87 = RL86; RL86 = RL85; RL85 = RL84; RL84 = RL83; RL83 = RL82; RL82 = RL81;
ralphg_92 32:a779b1131977 280 RL81 = RL80; RL80 = RL79; RL79 = RL78; RL78 = RL77; RL77 = RL76; RL76 = RL75;
ralphg_92 32:a779b1131977 281 RL75 = RL74; RL74 = RL73; RL73 = RL72; RL72 = RL71; RL71 = RL70; RL70 = RL69;
ralphg_92 32:a779b1131977 282 RL69 = RL68; RL68 = RL67; RL67 = RL66; RL66 = RL65; RL65 = RL64; RL64 = RL63;
ralphg_92 32:a779b1131977 283 RL63 = RL62; RL62 = RL61; RL61 = RL60; RL60 = RL59; RL59 = RL58; RL58 = RL57;
ralphg_92 32:a779b1131977 284 RL57 = RL56; RL56 = RL55; RL55 = RL54; RL54 = RL53; RL53 = RL52; RL52 = RL51;
ralphg_92 32:a779b1131977 285 RL51 = RL50; RL50 = RL49; RL49 = RL48; RL48 = RL47; RL47 = RL46; RL46 = RL45;
ralphg_92 32:a779b1131977 286 RL45 = RL44; RL44 = RL43; RL43 = RL42; RL42 = RL41; RL41 = RL40; RL40 = RL39;
ralphg_92 32:a779b1131977 287 RL39 = RL38; RL38 = RL37; RL37 = RL36; RL36 = RL35; RL35 = RL34; RL34 = RL33;
ralphg_92 32:a779b1131977 288 RL33 = RL32; RL32 = RL31; RL31 = RL30; RL30 = RL29; RL29 = RL28; RL28 = RL27;
ralphg_92 32:a779b1131977 289 RL27 = RL26; RL26 = RL25; RL25 = RL24; RL24 = RL23; RL23 = RL22; RL22 = RL21;
ralphg_92 32:a779b1131977 290 RL21 = RL20; RL20 = RL19; RL19 = RL18; RL18 = RL17; RL17 = RL16; RL16 = RL15;
ralphg_92 32:a779b1131977 291 RL15 = RL14; RL14 = RL13; RL13 = RL12; RL12 = RL11; RL11 = RL10; RL10 = RL9;
ralphg_92 32:a779b1131977 292 RL9 = RL8; RL8 = RL7; RL7 = RL6; RL6 = RL5; RL5 = RL4; RL4 = RL3; RL3 = RL2;
ralphg_92 32:a779b1131977 293 RL2 = RL1; RL1 = RL0;
ralphg_92 27:ca07f895f999 294
ralphg_92 32:a779b1131977 295 if (max3<MOVAVG_LL){
ralphg_92 32:a779b1131977 296 max3 = MOVAVG_LL; // set the max value at the highest measured value
relvorelvo 23:e5db011bd410 297 }
ralphg_92 32:a779b1131977 298 if (max4<MOVAVG_RL){
ralphg_92 32:a779b1131977 299 max4 = MOVAVG_RL; // set the max value at the highest measured value
ralphg_92 27:ca07f895f999 300 }
relvorelvo 23:e5db011bd410 301 wait(0.001f);
relvorelvo 23:e5db011bd410 302 }
relvorelvo 23:e5db011bd410 303 wait(0.2f);
relvorelvo 23:e5db011bd410 304 red = 1;
relvorelvo 23:e5db011bd410 305 }
ralphg_92 32:a779b1131977 306 maxpart3 = 0.17*max3; // set cut off voltage at 15% of max for left lower arm
ralphg_92 32:a779b1131977 307 maxpart4 = 0.17*max4; // set cut off voltage at 15% of max for right lower arm
vsluiter 2:e314bb3b2d99 308 }
vsluiter 0:32bb76391d89 309
ralphg_92 43:a6fbcada47aa 310 // Filtering & Scope
ralphg_92 43:a6fbcada47aa 311 void filter() {
relvorelvo 23:e5db011bd410 312 // Right Biceps
relvorelvo 23:e5db011bd410 313 emg1 = emg1_in.read();
relvorelvo 23:e5db011bd410 314 emg1highfilter = filterhigh1.step(emg1);
relvorelvo 23:e5db011bd410 315 emg1notchfilter = filternotch1.step(emg1highfilter);
relvorelvo 23:e5db011bd410 316 emg1abs = fabs(emg1notchfilter);
ralphg_92 31:d346f9244b4a 317 emg1lowfilter = filterlow1.step(emg1abs); // Final Right Biceps values to be sent
ralphg_92 32:a779b1131977 318 RB0 = emg1lowfilter;
ralphg_92 32:a779b1131977 319 // Movavg is defined by rb0-rb75 and rb1 to rb75 are redefined by the value of rbx-1
ralphg_92 32:a779b1131977 320 MOVAVG_RB = RB0*AV+RB1*AV+RB2*AV+RB3*AV+RB4*AV+RB5*AV+RB6*AV+RB7*AV+RB8*AV
ralphg_92 32:a779b1131977 321 +RB9*AV+RB10*AV+RB11*AV+RB12*AV+RB13*AV+RB14*AV+RB15*AV+RB16*AV+RB17*AV+RB18
ralphg_92 32:a779b1131977 322 *AV+RB19*AV+RB20*AV+RB21*AV+RB22*AV+RB23*AV+RB24*AV+RB25*AV+RB26*AV+RB27
ralphg_92 32:a779b1131977 323 *AV+RB28*AV+RB29*AV+RB30*AV+RB31*AV+RB32*AV+RB33*AV+RB34*AV+RB35*AV+RB36*AV
ralphg_92 32:a779b1131977 324 +RB37*AV+RB38*AV+RB39*AV+RB40*AV+RB41*AV+RB42*AV+RB43*AV+RB44*AV+RB45*AV
ralphg_92 32:a779b1131977 325 +RB46*AV+RB47*AV+RB48*AV+RB49*AV+RB50*AV+RB51*AV+RB52*AV+RB53*AV+RB54*AV
ralphg_92 32:a779b1131977 326 +RB55*AV+RB56*AV+RB57*AV+RB58*AV+RB59*AV+RB60*AV+RB61*AV+RB62*AV+RB63*AV
ralphg_92 32:a779b1131977 327 +RB64*AV+RB65*AV+RB66*AV+RB67*AV+RB68*AV+RB69*AV+RB70*AV+RB71*AV+RB72*AV
ralphg_92 32:a779b1131977 328 +RB73*AV+RB74*AV+RB75*AV+RB76*AV+RB77*AV+RB78*AV+RB79*AV+RB80*AV+RB81*AV
ralphg_92 32:a779b1131977 329 +RB82*AV+RB83*AV+RB84*AV+RB85*AV+RB86*AV+RB87*AV;
ralphg_92 32:a779b1131977 330 RB87 = RB86; RB86 = RB85; RB85 = RB84; RB84 = RB83; RB83 = RB82; RB82 = RB81;
ralphg_92 32:a779b1131977 331 RB81 = RB80; RB80 = RB79; RB79 = RB78; RB78 = RB77; RB77 = RB76; RB76 = RB75;
ralphg_92 32:a779b1131977 332 RB75 = RB74; RB74 = RB73; RB73 = RB72; RB72 = RB71; RB71 = RB70; RB70 = RB69;
ralphg_92 32:a779b1131977 333 RB69 = RB68; RB68 = RB67; RB67 = RB66; RB66 = RB65; RB65 = RB64; RB64 = RB63;
ralphg_92 32:a779b1131977 334 RB63 = RB62; RB62 = RB61; RB61 = RB60; RB60 = RB59; RB59 = RB58; RB58 = RB57;
ralphg_92 32:a779b1131977 335 RB57 = RB56; RB56 = RB55; RB55 = RB54; RB54 = RB53; RB53 = RB52; RB52 = RB51;
ralphg_92 32:a779b1131977 336 RB51 = RB50; RB50 = RB49; RB49 = RB48; RB48 = RB47; RB47 = RB46; RB46 = RB45;
ralphg_92 32:a779b1131977 337 RB45 = RB44; RB44 = RB43; RB43 = RB42; RB42 = RB41; RB41 = RB40; RB40 = RB39;
ralphg_92 32:a779b1131977 338 RB39 = RB38; RB38 = RB37; RB37 = RB36; RB36 = RB35; RB35 = RB34; RB34 = RB33;
ralphg_92 32:a779b1131977 339 RB33 = RB32; RB32 = RB31; RB31 = RB30; RB30 = RB29; RB29 = RB28; RB28 = RB27;
ralphg_92 32:a779b1131977 340 RB27 = RB26; RB26 = RB25; RB25 = RB24; RB24 = RB23; RB23 = RB22; RB22 = RB21;
ralphg_92 32:a779b1131977 341 RB21 = RB20; RB20 = RB19; RB19 = RB18; RB18 = RB17; RB17 = RB16; RB16 = RB15;
ralphg_92 32:a779b1131977 342 RB15 = RB14; RB14 = RB13; RB13 = RB12; RB12 = RB11; RB11 = RB10; RB10 = RB9;
ralphg_92 32:a779b1131977 343 RB9 = RB8; RB8 = RB7; RB7 = RB6; RB6 = RB5; RB5 = RB4; RB4 = RB3; RB3 = RB2;
ralphg_92 32:a779b1131977 344 RB2 = RB1; RB1 = RB0;
relvorelvo 23:e5db011bd410 345 // Left Biceps
relvorelvo 23:e5db011bd410 346 emg2 = emg2_in.read();
relvorelvo 23:e5db011bd410 347 emg2highfilter = filterhigh2.step(emg2);
relvorelvo 23:e5db011bd410 348 emg2notchfilter = filternotch2.step(emg2highfilter);
relvorelvo 23:e5db011bd410 349 emg2abs = fabs(emg2notchfilter);
ralphg_92 31:d346f9244b4a 350 emg2lowfilter = filterlow2.step(emg2abs); // Final Left Biceps values to be sent
ralphg_92 32:a779b1131977 351 LB0 = emg2lowfilter;
ralphg_92 32:a779b1131977 352 // same story as with the right biceps
ralphg_92 32:a779b1131977 353 MOVAVG_LB = LB0*AV+LB1*AV+LB2*AV+LB3*AV+LB4*AV+LB5*AV+LB6*AV+LB7*AV+LB8*AV
ralphg_92 32:a779b1131977 354 +LB9*AV+LB10*AV+LB11*AV+LB12*AV+LB13*AV+LB14*AV+LB15*AV+LB16*AV+LB17*AV+LB18
ralphg_92 32:a779b1131977 355 *AV+LB19*AV+LB20*AV+LB21*AV+LB22*AV+LB23*AV+LB24*AV+LB25*AV+LB26*AV+LB27
ralphg_92 32:a779b1131977 356 *AV+LB28*AV+LB29*AV+LB30*AV+LB31*AV+LB32*AV+LB33*AV+LB34*AV+LB35*AV+LB36*AV
ralphg_92 32:a779b1131977 357 +LB37*AV+LB38*AV+LB39*AV+LB40*AV+LB41*AV+LB42*AV+LB43*AV+LB44*AV+LB45*AV
ralphg_92 32:a779b1131977 358 +LB46*AV+LB47*AV+LB48*AV+LB49*AV+LB50*AV+LB51*AV+LB52*AV+LB53*AV+LB54*AV
ralphg_92 32:a779b1131977 359 +LB55*AV+LB56*AV+LB57*AV+LB58*AV+LB59*AV+LB60*AV+LB61*AV+LB62*AV+LB63*AV
ralphg_92 32:a779b1131977 360 +LB64*AV+LB65*AV+LB66*AV+LB67*AV+LB68*AV+LB69*AV+LB70*AV+LB71*AV+LB72*AV
ralphg_92 32:a779b1131977 361 +LB73*AV+LB74*AV+LB75*AV+LB76*AV+LB77*AV+LB78*AV+LB79*AV+LB80*AV+LB81*AV
ralphg_92 32:a779b1131977 362 +LB82*AV+LB83*AV+LB84*AV+LB85*AV+LB86*AV+LB87*AV;
ralphg_92 32:a779b1131977 363 LB87 = LB86; LB86 = LB85; LB85 = LB84; LB84 = LB83; LB83 = LB82; LB82 = LB81;
ralphg_92 32:a779b1131977 364 LB81 = LB80; LB80 = LB79; LB79 = LB78; LB78 = LB77; LB77 = LB76; LB76 = LB75;
ralphg_92 32:a779b1131977 365 LB75 = LB74; LB74 = LB73; LB73 = LB72; LB72 = LB71; LB71 = LB70; LB70 = LB69;
ralphg_92 32:a779b1131977 366 LB69 = LB68; LB68 = LB67; LB67 = LB66; LB66 = LB65; LB65 = LB64; LB64 = LB63;
ralphg_92 32:a779b1131977 367 LB63 = LB62; LB62 = LB61; LB61 = LB60; LB60 = LB59; LB59 = LB58; LB58 = LB57;
ralphg_92 32:a779b1131977 368 LB57 = LB56; LB56 = LB55; LB55 = LB54; LB54 = LB53; LB53 = LB52; LB52 = LB51;
ralphg_92 32:a779b1131977 369 LB51 = LB50; LB50 = LB49; LB49 = LB48; LB48 = LB47; LB47 = LB46; LB46 = LB45;
ralphg_92 32:a779b1131977 370 LB45 = LB44; LB44 = LB43; LB43 = LB42; LB42 = LB41; LB41 = LB40; LB40 = LB39;
ralphg_92 32:a779b1131977 371 LB39 = LB38; LB38 = LB37; LB37 = LB36; LB36 = LB35; LB35 = LB34; LB34 = LB33;
ralphg_92 32:a779b1131977 372 LB33 = LB32; LB32 = LB31; LB31 = LB30; LB30 = LB29; LB29 = LB28; LB28 = LB27;
ralphg_92 32:a779b1131977 373 LB27 = LB26; LB26 = LB25; LB25 = LB24; LB24 = LB23; LB23 = LB22; LB22 = LB21;
ralphg_92 32:a779b1131977 374 LB21 = LB20; LB20 = LB19; LB19 = LB18; LB18 = LB17; LB17 = LB16; LB16 = LB15;
ralphg_92 32:a779b1131977 375 LB15 = LB14; LB14 = LB13; LB13 = LB12; LB12 = LB11; LB11 = LB10; LB10 = LB9;
ralphg_92 32:a779b1131977 376 LB9 = LB8; LB8 = LB7; LB7 = LB6; LB6 = LB5; LB5 = LB4; LB4 = LB3; LB3 = LB2;
ralphg_92 32:a779b1131977 377 LB2 = LB1; LB1 = LB0;
relvorelvo 23:e5db011bd410 378 // Left Lower Arm OR Triceps
relvorelvo 23:e5db011bd410 379 emg3 = emg3_in.read();
relvorelvo 23:e5db011bd410 380 emg3highfilter = filterhigh3.step(emg3);
relvorelvo 23:e5db011bd410 381 emg3notchfilter = filternotch3.step(emg3highfilter);
relvorelvo 23:e5db011bd410 382 emg3abs = fabs(emg3notchfilter);
ralphg_92 31:d346f9244b4a 383 emg3lowfilter = filterlow3.step(emg3abs); // Final Lower Arm values to be sent
ralphg_92 32:a779b1131977 384 LL0 = emg3lowfilter;
ralphg_92 32:a779b1131977 385 // same story as with the right biceps
ralphg_92 32:a779b1131977 386 MOVAVG_LL = LL0*AV+LL1*AV+LL2*AV+LL3*AV+LL4*AV+LL5*AV+LL6*AV+LL7*AV+LL8*AV
ralphg_92 32:a779b1131977 387 +LL9*AV+LL10*AV+LL11*AV+LL12*AV+LL13*AV+LL14*AV+LL15*AV+LL16*AV+LL17*AV+LL18
ralphg_92 32:a779b1131977 388 *AV+LL19*AV+LL20*AV+LL21*AV+LL22*AV+LL23*AV+LL24*AV+LL25*AV+LL26*AV+LL27
ralphg_92 32:a779b1131977 389 *AV+LL28*AV+LL29*AV+LL30*AV+LL31*AV+LL32*AV+LL33*AV+LL34*AV+LL35*AV+LL36*AV
ralphg_92 32:a779b1131977 390 +LL37*AV+LL38*AV+LL39*AV+LL40*AV+LL41*AV+LL42*AV+LL43*AV+LL44*AV+LL45*AV
ralphg_92 32:a779b1131977 391 +LL46*AV+LL47*AV+LL48*AV+LL49*AV+LL50*AV+LL51*AV+LL52*AV+LL53*AV+LL54*AV
ralphg_92 32:a779b1131977 392 +LL55*AV+LL56*AV+LL57*AV+LL58*AV+LL59*AV+LL60*AV+LL61*AV+LL62*AV+LL63*AV
ralphg_92 32:a779b1131977 393 +LL64*AV+LL65*AV+LL66*AV+LL67*AV+LL68*AV+LL69*AV+LL70*AV+LL71*AV+LL72*AV
ralphg_92 32:a779b1131977 394 +LL73*AV+LL74*AV+LL75*AV+LL76*AV+LL77*AV+LL78*AV+LL79*AV+LL80*AV+LL81*AV
ralphg_92 32:a779b1131977 395 +LL82*AV+LL83*AV+LL84*AV+LL85*AV+LL86*AV+LL87*AV;
ralphg_92 32:a779b1131977 396 LL87 = LL86; LL86 = LL85; LL85 = LL84; LL84 = LL83; LL83 = LL82; LL82 = LL81;
ralphg_92 32:a779b1131977 397 LL81 = LL80; LL80 = LL79; LL79 = LL78; LL78 = LL77; LL77 = LL76; LL76 = LL75;
ralphg_92 32:a779b1131977 398 LL75 = LL74; LL74 = LL73; LL73 = LL72; LL72 = LL71; LL71 = LL70; LL70 = LL69;
ralphg_92 32:a779b1131977 399 LL69 = LL68; LL68 = LL67; LL67 = LL66; LL66 = LL65; LL65 = LL64; LL64 = LL63;
ralphg_92 32:a779b1131977 400 LL63 = LL62; LL62 = LL61; LL61 = LL60; LL60 = LL59; LL59 = LL58; LL58 = LL57;
ralphg_92 32:a779b1131977 401 LL57 = LL56; LL56 = LL55; LL55 = LL54; LL54 = LL53; LL53 = LL52; LL52 = LL51;
ralphg_92 32:a779b1131977 402 LL51 = LL50; LL50 = LL49; LL49 = LL48; LL48 = LL47; LL47 = LL46; LL46 = LL45;
ralphg_92 32:a779b1131977 403 LL45 = LL44; LL44 = LL43; LL43 = LL42; LL42 = LL41; LL41 = LL40; LL40 = LL39;
ralphg_92 32:a779b1131977 404 LL39 = LL38; LL38 = LL37; LL37 = LL36; LL36 = LL35; LL35 = LL34; LL34 = LL33;
ralphg_92 32:a779b1131977 405 LL33 = LL32; LL32 = LL31; LL31 = LL30; LL30 = LL29; LL29 = LL28; LL28 = LL27;
ralphg_92 32:a779b1131977 406 LL27 = LL26; LL26 = LL25; LL25 = LL24; LL24 = LL23; LL23 = LL22; LL22 = LL21;
ralphg_92 32:a779b1131977 407 LL21 = LL20; LL20 = LL19; LL19 = LL18; LL18 = LL17; LL17 = LL16; LL16 = LL15;
ralphg_92 32:a779b1131977 408 LL15 = LL14; LL14 = LL13; LL13 = LL12; LL12 = LL11; LL11 = LL10; LL10 = LL9;
ralphg_92 32:a779b1131977 409 LL9 = LL8; LL8 = LL7; LL7 = LL6; LL6 = LL5; LL5 = LL4; LL4 = LL3; LL3 = LL2;
ralphg_92 32:a779b1131977 410 LL2 = LL1; LL1 = LL0;
ralphg_92 27:ca07f895f999 411 // Right Lower Arm OR Triceps
ralphg_92 27:ca07f895f999 412 emg4 = emg4_in.read();
ralphg_92 27:ca07f895f999 413 emg4highfilter = filterhigh4.step(emg4);
ralphg_92 27:ca07f895f999 414 emg4notchfilter = filternotch4.step(emg4highfilter);
ralphg_92 27:ca07f895f999 415 emg4abs = fabs(emg4notchfilter);
ralphg_92 32:a779b1131977 416 emg4lowfilter = filterlow4.step(emg4abs); // Final Lower Arm values to be sent
ralphg_92 32:a779b1131977 417 RL0 = emg4lowfilter;
ralphg_92 32:a779b1131977 418 // same story as with the right biceps
ralphg_92 32:a779b1131977 419 MOVAVG_RL = RL0*AV+RL1*AV+RL2*AV+RL3*AV+RL4*AV+RL5*AV+RL6*AV+RL7*AV+RL8*AV
ralphg_92 32:a779b1131977 420 +RL9*AV+RL10*AV+RL11*AV+RL12*AV+RL13*AV+RL14*AV+RL15*AV+RL16*AV+RL17*AV+RL18
ralphg_92 32:a779b1131977 421 *AV+RL19*AV+RL20*AV+RL21*AV+RL22*AV+RL23*AV+RL24*AV+RL25*AV+RL26*AV+RL27
ralphg_92 32:a779b1131977 422 *AV+RL28*AV+RL29*AV+RL30*AV+RL31*AV+RL32*AV+RL33*AV+RL34*AV+RL35*AV+RL36*AV
ralphg_92 32:a779b1131977 423 +RL37*AV+RL38*AV+RL39*AV+RL40*AV+RL41*AV+RL42*AV+RL43*AV+RL44*AV+RL45*AV
ralphg_92 32:a779b1131977 424 +RL46*AV+RL47*AV+RL48*AV+RL49*AV+RL50*AV+RL51*AV+RL52*AV+RL53*AV+RL54*AV
ralphg_92 32:a779b1131977 425 +RL55*AV+RL56*AV+RL57*AV+RL58*AV+RL59*AV+RL60*AV+RL61*AV+RL62*AV+RL63*AV
ralphg_92 32:a779b1131977 426 +RL64*AV+RL65*AV+RL66*AV+RL67*AV+RL68*AV+RL69*AV+RL70*AV+RL71*AV+RL72*AV
ralphg_92 32:a779b1131977 427 +RL73*AV+RL74*AV+RL75*AV+RL76*AV+RL77*AV+RL78*AV+RL79*AV+RL80*AV+RL81*AV
ralphg_92 32:a779b1131977 428 +RL82*AV+RL83*AV+RL84*AV+RL85*AV+RL86*AV+RL87*AV;
ralphg_92 32:a779b1131977 429 RL87 = RL86; RL86 = RL85; RL85 = RL84; RL84 = RL83; RL83 = RL82; RL82 = RL81;
ralphg_92 32:a779b1131977 430 RL81 = RL80; RL80 = RL79; RL79 = RL78; RL78 = RL77; RL77 = RL76; RL76 = RL75;
ralphg_92 32:a779b1131977 431 RL75 = RL74; RL74 = RL73; RL73 = RL72; RL72 = RL71; RL71 = RL70; RL70 = RL69;
ralphg_92 32:a779b1131977 432 RL69 = RL68; RL68 = RL67; RL67 = RL66; RL66 = RL65; RL65 = RL64; RL64 = RL63;
ralphg_92 32:a779b1131977 433 RL63 = RL62; RL62 = RL61; RL61 = RL60; RL60 = RL59; RL59 = RL58; RL58 = RL57;
ralphg_92 32:a779b1131977 434 RL57 = RL56; RL56 = RL55; RL55 = RL54; RL54 = RL53; RL53 = RL52; RL52 = RL51;
ralphg_92 32:a779b1131977 435 RL51 = RL50; RL50 = RL49; RL49 = RL48; RL48 = RL47; RL47 = RL46; RL46 = RL45;
ralphg_92 32:a779b1131977 436 RL45 = RL44; RL44 = RL43; RL43 = RL42; RL42 = RL41; RL41 = RL40; RL40 = RL39;
ralphg_92 32:a779b1131977 437 RL39 = RL38; RL38 = RL37; RL37 = RL36; RL36 = RL35; RL35 = RL34; RL34 = RL33;
ralphg_92 32:a779b1131977 438 RL33 = RL32; RL32 = RL31; RL31 = RL30; RL30 = RL29; RL29 = RL28; RL28 = RL27;
ralphg_92 32:a779b1131977 439 RL27 = RL26; RL26 = RL25; RL25 = RL24; RL24 = RL23; RL23 = RL22; RL22 = RL21;
ralphg_92 32:a779b1131977 440 RL21 = RL20; RL20 = RL19; RL19 = RL18; RL18 = RL17; RL17 = RL16; RL16 = RL15;
ralphg_92 32:a779b1131977 441 RL15 = RL14; RL14 = RL13; RL13 = RL12; RL12 = RL11; RL11 = RL10; RL10 = RL9;
ralphg_92 32:a779b1131977 442 RL9 = RL8; RL8 = RL7; RL7 = RL6; RL6 = RL5; RL5 = RL4; RL4 = RL3; RL3 = RL2;
ralphg_92 32:a779b1131977 443 RL2 = RL1; RL1 = RL0;
ralphg_92 29:09c1567d6148 444
relvorelvo 23:e5db011bd410 445
ralphg_92 31:d346f9244b4a 446 // Compare measurement to the calibrated value to decide actions
ralphg_92 31:d346f9244b4a 447 // more options could be added if the callibration is well defined enough
ralphg_92 31:d346f9244b4a 448 // This part checks for right biceps contractions only
ralphg_92 43:a6fbcada47aa 449 if (maxpart1<MOVAVG_RB || maxpart2<MOVAVG_LB || maxpart3<MOVAVG_LL || maxpart4<MOVAVG_RL){
ralphg_92 43:a6fbcada47aa 450
ralphg_92 43:a6fbcada47aa 451 if (maxpart1<MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
ralphg_92 29:09c1567d6148 452 red = 1;
ralphg_92 29:09c1567d6148 453 blue = 1;
ralphg_92 29:09c1567d6148 454 green = 0;
ralphg_92 43:a6fbcada47aa 455 MV1 = 0.08;
ralphg_92 43:a6fbcada47aa 456 MV2 = 0;
ralphg_92 43:a6fbcada47aa 457 }
ralphg_92 43:a6fbcada47aa 458 // This part checks for left biceps contractions only
ralphg_92 43:a6fbcada47aa 459 else if (maxpart1>MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
ralphg_92 29:09c1567d6148 460 red = 0;
ralphg_92 29:09c1567d6148 461 blue = 1;
ralphg_92 29:09c1567d6148 462 green = 1;
ralphg_92 43:a6fbcada47aa 463 MV1 = -0.08;
ralphg_92 43:a6fbcada47aa 464 MV2 = 0;
ralphg_92 43:a6fbcada47aa 465 }
ralphg_92 43:a6fbcada47aa 466 // This part checks for left lower arm contractions only
ralphg_92 43:a6fbcada47aa 467 else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4>MOVAVG_RL){
relvorelvo 23:e5db011bd410 468 red = 1;
relvorelvo 23:e5db011bd410 469 blue = 0;
relvorelvo 23:e5db011bd410 470 green = 1;
ralphg_92 43:a6fbcada47aa 471 MV1 = 0;
ralphg_92 43:a6fbcada47aa 472 MV2 = 0.08;
ralphg_92 43:a6fbcada47aa 473 }
ralphg_92 43:a6fbcada47aa 474 // This part checks for right lower arm contractions only
ralphg_92 43:a6fbcada47aa 475 else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4<MOVAVG_RL){
ralphg_92 29:09c1567d6148 476 red = 0;
relvorelvo 23:e5db011bd410 477 blue = 1;
relvorelvo 23:e5db011bd410 478 green = 0;
ralphg_92 43:a6fbcada47aa 479 MV1 = 0;
ralphg_92 43:a6fbcada47aa 480 MV2 = -0.08;
ralphg_92 43:a6fbcada47aa 481 }
ralphg_92 43:a6fbcada47aa 482 // This part checks for both lower arm contractions only
ralphg_92 43:a6fbcada47aa 483 else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4<MOVAVG_RL){
ralphg_92 43:a6fbcada47aa 484 red = 0;
ralphg_92 43:a6fbcada47aa 485 blue = 0;
ralphg_92 43:a6fbcada47aa 486 green = 0;
ralphg_92 43:a6fbcada47aa 487 MV1 = -0.08;
ralphg_92 43:a6fbcada47aa 488 MV2 = -0.08;
ralphg_92 43:a6fbcada47aa 489 }
ralphg_92 43:a6fbcada47aa 490 // This part checks for both biceps contractions only
ralphg_92 43:a6fbcada47aa 491 else if (maxpart1<MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
ralphg_92 31:d346f9244b4a 492 red = 0;
ralphg_92 31:d346f9244b4a 493 blue = 0;
ralphg_92 31:d346f9244b4a 494 green = 0;
ralphg_92 43:a6fbcada47aa 495 MV1 = 0.08;
ralphg_92 43:a6fbcada47aa 496 MV2 = 0.08;
ralphg_92 43:a6fbcada47aa 497 }
ralphg_92 43:a6fbcada47aa 498 // This part checks for right lower arm & left biceps contractions only
ralphg_92 43:a6fbcada47aa 499 else if (maxpart1>MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4<MOVAVG_RL){
ralphg_92 30:2c67abcdb892 500 red = 0;
ralphg_92 30:2c67abcdb892 501 blue = 0;
ralphg_92 30:2c67abcdb892 502 green = 0;
ralphg_92 43:a6fbcada47aa 503 MV1 = 0.08;
ralphg_92 43:a6fbcada47aa 504 MV2 = -0.08;
ralphg_92 43:a6fbcada47aa 505 }
ralphg_92 43:a6fbcada47aa 506 // This part checks for left lower arm & right biceps contractions only
ralphg_92 43:a6fbcada47aa 507 else if (maxpart1<MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4>MOVAVG_RL){
ralphg_92 30:2c67abcdb892 508 red = 0;
ralphg_92 30:2c67abcdb892 509 blue = 0;
ralphg_92 30:2c67abcdb892 510 green = 0;
ralphg_92 43:a6fbcada47aa 511 MV1 = -0.08;
ralphg_92 43:a6fbcada47aa 512 MV2 = 0.08;
ralphg_92 43:a6fbcada47aa 513 }
ralphg_92 43:a6fbcada47aa 514 }
ralphg_92 43:a6fbcada47aa 515 else {
ralphg_92 43:a6fbcada47aa 516 red = 1; // Shut down all led colors if no movement is registered
ralphg_92 43:a6fbcada47aa 517 blue = 1;
ralphg_92 43:a6fbcada47aa 518 green = 1;
ralphg_92 43:a6fbcada47aa 519 MV1 = 0;
ralphg_92 43:a6fbcada47aa 520 MV2 = 0;
ralphg_92 43:a6fbcada47aa 521 //pc.printf( "No contraction registered\n");
ralphg_92 43:a6fbcada47aa 522 }
ralphg_92 43:a6fbcada47aa 523
ralphg_92 31:d346f9244b4a 524 // Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope'
ralphg_92 32:a779b1131977 525 scope.set(0, MOVAVG_RB ); // plot Right biceps voltage
ralphg_92 32:a779b1131977 526 scope.set(1, MOVAVG_LB ); // Plot Left biceps voltage
ralphg_92 32:a779b1131977 527 scope.set(2, MOVAVG_LL ); // Plot Lower Left Arm voltage
ralphg_92 32:a779b1131977 528 scope.set(3, MOVAVG_RL ); // Plot Lower Right Arm Voltage
ralphg_92 31:d346f9244b4a 529 scope.send(); // send everything to the HID scope
ralphg_92 31:d346f9244b4a 530
relvorelvo 23:e5db011bd410 531 }
ralphg_92 41:08d41bb622bb 532
ralphg_92 43:a6fbcada47aa 533 // check MV1 to see if motor1 needs to be activated
ralphg_92 43:a6fbcada47aa 534 void SetMotor1(float MV1) {
ralphg_92 43:a6fbcada47aa 535 motor1pwm = fabs(MV1); // motor speed
ralphg_92 43:a6fbcada47aa 536 if (MV1 >=0) {
ralphg_92 43:a6fbcada47aa 537 motor1direction = 1; // clockwise rotation
ralphg_92 33:97e69c32a768 538 }
ralphg_92 43:a6fbcada47aa 539 else {
ralphg_92 43:a6fbcada47aa 540 motor1direction = 0; // counterclockwise rotation
ralphg_92 31:d346f9244b4a 541 }
ralphg_92 31:d346f9244b4a 542 }
ralphg_92 43:a6fbcada47aa 543 // check MV2 if motor1 needs to be activated
ralphg_92 43:a6fbcada47aa 544 void SetMotor2(float MV2) {
ralphg_92 43:a6fbcada47aa 545 motor2pwm = fabs(MV2); // motor speed
ralphg_92 43:a6fbcada47aa 546 if (MV2 >=0) {
ralphg_92 43:a6fbcada47aa 547 motor2direction = 1; // clockwise rotation
ralphg_92 43:a6fbcada47aa 548 }
ralphg_92 43:a6fbcada47aa 549 else {
ralphg_92 43:a6fbcada47aa 550 motor2direction = 0; // counterclockwise rotation
ralphg_92 31:d346f9244b4a 551 }
ralphg_92 31:d346f9244b4a 552 }
ralphg_92 41:08d41bb622bb 553
ralphg_92 43:a6fbcada47aa 554 void MeasureAndControl(void)
ralphg_92 43:a6fbcada47aa 555 {
ralphg_92 43:a6fbcada47aa 556 // This function measures the potmeter position, extracts a
ralphg_92 43:a6fbcada47aa 557 // reference velocity from it, and controls the motor with
ralphg_92 43:a6fbcada47aa 558 // a simple FeedForward controller. Call this from a Ticker.
ralphg_92 43:a6fbcada47aa 559 SetMotor1(MV1);
ralphg_92 43:a6fbcada47aa 560 SetMotor2(MV2);
ralphg_92 31:d346f9244b4a 561 }
ralphg_92 43:a6fbcada47aa 562
ralphg_92 31:d346f9244b4a 563
relvorelvo 23:e5db011bd410 564 int main(){
ralphg_92 43:a6fbcada47aa 565
ralphg_92 31:d346f9244b4a 566 main_timer.attach(&filter, 0.001); // set frequency for the filters at 1000Hz
ralphg_92 31:d346f9244b4a 567 max_read1.attach(&get_max1, 2); // set the frequency of the calibration loop at 0.5Hz
relvorelvo 23:e5db011bd410 568 max_read3.attach(&get_max3, 2);
ralphg_92 43:a6fbcada47aa 569 Motorcontrol.attach(&MeasureAndControl,0.5);
tomlankhorst 15:0da764eea774 570 while(1) {}
vsluiter 0:32bb76391d89 571 }