8 option EMG
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
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?
User | Revision | Line number | New 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 | } |