Ralph Gerlings / Mbed 2 deprecated cpfromralph

new fork sure

Dependencies:   HIDScope MODSERIAL PID QEI biquadFilter mbed

Fork of cpfromralph by Remi van Veen

main.cpp@32:a779b1131977, 2017-11-02 (annotated)

Committer:
ralphg_92
Date:
Thu Nov 02 13:10:28 2017 +0000
Revision:
32:a779b1131977
Parent:
31:d346f9244b4a
Child:
33:97e69c32a768
now with moving average, working fine, maybe slightly alter the setpoints where motor activation is done but eh

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 31:d346f9244b4a 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 32:a779b1131977 19 //QEI Encoder1(D10, D11, NC, 32); // Encoder
ralphg_92 31:d346f9244b4a 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 32:a779b1131977 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);
relvorelvo 23:e5db011bd410 30 MODSERIAL pc(USBTX, USBRX);
relvorelvo 21:136a1ab8163c 31
relvorelvo 21:136a1ab8163c 32
ralphg_92 32:a779b1131977 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 32:a779b1131977 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 32:a779b1131977 106 // Motor Variables
ralphg_92 31:d346f9244b4a 107 float MV1 = 0;
ralphg_92 31:d346f9244b4a 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 29:09c1567d6148 129 // Finding max values for correct motor switch if the button is pressed
ralphg_92 31:d346f9244b4a 130 // calibration of both biceps
ralphg_92 31:d346f9244b4a 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 32:a779b1131977 216 maxpart1 = 0.14*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 31:d346f9244b4a 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
relvorelvo 23:e5db011bd410 310 // Filtering & Scope
relvorelvo 23:e5db011bd410 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 32:a779b1131977 449 if (maxpart1<MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
ralphg_92 29:09c1567d6148 450 red = 1;
ralphg_92 29:09c1567d6148 451 blue = 1;
ralphg_92 29:09c1567d6148 452 green = 0;
ralphg_92 31:d346f9244b4a 453 MV1 = 0.5;
ralphg_92 31:d346f9244b4a 454 MV2 = 0;
ralphg_92 31:d346f9244b4a 455 }
ralphg_92 31:d346f9244b4a 456 // This part checks for left biceps contractions only
ralphg_92 32:a779b1131977 457 else if (maxpart1>MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
ralphg_92 29:09c1567d6148 458 red = 0;
ralphg_92 29:09c1567d6148 459 blue = 1;
ralphg_92 29:09c1567d6148 460 green = 1;
ralphg_92 31:d346f9244b4a 461 MV1 = -0.5;
ralphg_92 31:d346f9244b4a 462 MV2 = 0;
ralphg_92 31:d346f9244b4a 463 }
ralphg_92 31:d346f9244b4a 464 // This part checks for left lower arm contractions only
ralphg_92 32:a779b1131977 465 else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4>MOVAVG_RL){
relvorelvo 23:e5db011bd410 466 red = 1;
relvorelvo 23:e5db011bd410 467 blue = 0;
relvorelvo 23:e5db011bd410 468 green = 1;
ralphg_92 31:d346f9244b4a 469 MV1 = 0;
ralphg_92 31:d346f9244b4a 470 MV2 = 0.5;
ralphg_92 31:d346f9244b4a 471 }
ralphg_92 31:d346f9244b4a 472 // This part checks for right lower arm contractions only
ralphg_92 32:a779b1131977 473 else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4<MOVAVG_RL){
ralphg_92 29:09c1567d6148 474 red = 0;
relvorelvo 23:e5db011bd410 475 blue = 1;
relvorelvo 23:e5db011bd410 476 green = 0;
ralphg_92 31:d346f9244b4a 477 MV1 = 0;
ralphg_92 31:d346f9244b4a 478 MV2 = -0.5;
ralphg_92 31:d346f9244b4a 479 }
ralphg_92 31:d346f9244b4a 480 // This part checks for both lower arm contractions only
ralphg_92 32:a779b1131977 481 else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4<MOVAVG_RL){
ralphg_92 31:d346f9244b4a 482 red = 0;
ralphg_92 31:d346f9244b4a 483 blue = 0;
ralphg_92 31:d346f9244b4a 484 green = 0;
ralphg_92 31:d346f9244b4a 485 MV1 = -0.5;
ralphg_92 31:d346f9244b4a 486 MV2 = -0.5;
ralphg_92 31:d346f9244b4a 487 }
ralphg_92 31:d346f9244b4a 488 // This part checks for both biceps contractions only
ralphg_92 32:a779b1131977 489 else if (maxpart1<MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
ralphg_92 31:d346f9244b4a 490 red = 0;
ralphg_92 31:d346f9244b4a 491 blue = 0;
ralphg_92 31:d346f9244b4a 492 green = 0;
ralphg_92 31:d346f9244b4a 493 MV1 = 0.5;
ralphg_92 31:d346f9244b4a 494 MV2 = 0.5;
ralphg_92 31:d346f9244b4a 495 }
ralphg_92 31:d346f9244b4a 496 // This part checks for right lower arm & left biceps contractions only
ralphg_92 32:a779b1131977 497 else if (maxpart1>MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4<MOVAVG_RL){
ralphg_92 30:2c67abcdb892 498 red = 0;
ralphg_92 30:2c67abcdb892 499 blue = 0;
ralphg_92 30:2c67abcdb892 500 green = 0;
ralphg_92 31:d346f9244b4a 501 MV1 = 0.5;
ralphg_92 31:d346f9244b4a 502 MV2 = -0.5;
ralphg_92 31:d346f9244b4a 503 }
ralphg_92 31:d346f9244b4a 504 // This part checks for left lower arm & right biceps contractions only
ralphg_92 32:a779b1131977 505 else if (maxpart1<MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4>MOVAVG_RL){
ralphg_92 30:2c67abcdb892 506 red = 0;
ralphg_92 30:2c67abcdb892 507 blue = 0;
ralphg_92 30:2c67abcdb892 508 green = 0;
ralphg_92 31:d346f9244b4a 509 MV1 = -0.5;
ralphg_92 31:d346f9244b4a 510 MV2 = 0.5;
ralphg_92 31:d346f9244b4a 511 }
ralphg_92 31:d346f9244b4a 512 else {
ralphg_92 31:d346f9244b4a 513 red = 1; // Shut down all led colors if no movement is registered
relvorelvo 23:e5db011bd410 514 blue = 1;
relvorelvo 23:e5db011bd410 515 green = 1;
ralphg_92 31:d346f9244b4a 516 MV1 = 0;
ralphg_92 31:d346f9244b4a 517 MV2 = 0;
ralphg_92 29:09c1567d6148 518 //pc.printf( "No contraction registered\n");
ralphg_92 29:09c1567d6148 519 }
ralphg_92 29:09c1567d6148 520
ralphg_92 31:d346f9244b4a 521 // 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 522 scope.set(0, MOVAVG_RB ); // plot Right biceps voltage
ralphg_92 32:a779b1131977 523 scope.set(1, MOVAVG_LB ); // Plot Left biceps voltage
ralphg_92 32:a779b1131977 524 scope.set(2, MOVAVG_LL ); // Plot Lower Left Arm voltage
ralphg_92 32:a779b1131977 525 scope.set(3, MOVAVG_RL ); // Plot Lower Right Arm Voltage
ralphg_92 31:d346f9244b4a 526 scope.send(); // send everything to the HID scope
ralphg_92 31:d346f9244b4a 527
relvorelvo 23:e5db011bd410 528 }
tomlankhorst 15:0da764eea774 529
ralphg_92 31:d346f9244b4a 530 // check MV1 to see if motor1 needs to be activated
ralphg_92 31:d346f9244b4a 531 void SetMotor1(float MV1) {
ralphg_92 31:d346f9244b4a 532 motor1pwm = fabs(MV1); // motor speed
ralphg_92 31:d346f9244b4a 533 if (MV1 >=0) {
ralphg_92 31:d346f9244b4a 534 motor1direction = 1; // clockwise rotation
ralphg_92 31:d346f9244b4a 535 }
ralphg_92 31:d346f9244b4a 536 else {
ralphg_92 31:d346f9244b4a 537 motor1direction = 0; // counterclockwise rotation
ralphg_92 31:d346f9244b4a 538 }
ralphg_92 31:d346f9244b4a 539 }
ralphg_92 31:d346f9244b4a 540 // check MV2 if motor1 needs to be activated
ralphg_92 31:d346f9244b4a 541 void SetMotor2(float MV2) {
ralphg_92 31:d346f9244b4a 542 motor2pwm = fabs(MV2); // motor speed
ralphg_92 31:d346f9244b4a 543 if (MV2 >=0) {
ralphg_92 31:d346f9244b4a 544 motor2direction = 1; // clockwise rotation
ralphg_92 31:d346f9244b4a 545 }
ralphg_92 31:d346f9244b4a 546 else {
ralphg_92 31:d346f9244b4a 547 motor2direction = 0; // counterclockwise rotation
ralphg_92 31:d346f9244b4a 548 }
ralphg_92 31:d346f9244b4a 549 }
ralphg_92 31:d346f9244b4a 550
ralphg_92 31:d346f9244b4a 551 void MeasureAndControl(void)
ralphg_92 31:d346f9244b4a 552 {
ralphg_92 31:d346f9244b4a 553 // This function measures the potmeter position, extracts a
ralphg_92 31:d346f9244b4a 554 // reference velocity from it, and controls the motor with
ralphg_92 31:d346f9244b4a 555 // a simple FeedForward controller. Call this from a Ticker.
ralphg_92 31:d346f9244b4a 556 SetMotor1(MV1);
ralphg_92 31:d346f9244b4a 557 SetMotor2(MV2);
ralphg_92 31:d346f9244b4a 558 }
ralphg_92 31:d346f9244b4a 559
ralphg_92 31:d346f9244b4a 560
relvorelvo 23:e5db011bd410 561 int main(){
relvorelvo 22:68ab712b62b2 562
ralphg_92 31:d346f9244b4a 563 main_timer.attach(&filter, 0.001); // set frequency for the filters at 1000Hz
ralphg_92 31:d346f9244b4a 564 max_read1.attach(&get_max1, 2); // set the frequency of the calibration loop at 0.5Hz
relvorelvo 23:e5db011bd410 565 max_read3.attach(&get_max3, 2);
ralphg_92 32:a779b1131977 566 Motorcontrol.attach(&MeasureAndControl,0.5);
tomlankhorst 15:0da764eea774 567 while(1) {}
vsluiter 0:32bb76391d89 568 }