Ralph Gerlings
even versimpelen
Dependencies: HIDScope MODSERIAL PID QEI biquadFilter mbed
Fork of
cpfromralph1
by 
Remi van Veen
Diff: main.cpp
- Revision:
- 32:a779b1131977
- Parent:
- 31:d346f9244b4a
- Child:
- 33:97e69c32a768
--- a/main.cpp Wed Nov 01 11:34:46 2017 +0000
+++ b/main.cpp Thu Nov 02 13:10:28 2017 +0000
@@ -4,33 +4,33 @@
#include "BiQuad.h"
//#include "QEI.h"
-//Define objects
- AnalogIn emg1_in( A0 ); // read out the signal
+//Define Objects
+ AnalogIn emg1_in( A0 ); // Read Out The Signal
AnalogIn emg2_in( A1 );
AnalogIn emg3_in( A2 );
AnalogIn emg4_in( A3 );
- DigitalIn max_reader12( SW2 ); // define button press
+ DigitalIn max_reader12( SW2 ); // Define Button Press
DigitalIn max_reader34( SW3 );
DigitalOut motor1direction( D4 );
PwmOut motor1pwm( D5);
PwmOut motor2pwm( D6 );
DigitalOut motor2direction( D7 );
- //QEI Encoder1(D10, D11, NC, 32); // Encoder reminder
+ //QEI Encoder1(D10, D11, NC, 32); // Encoder
//QEI Encoder2(D12, D13, NC, 32);
Ticker main_timer;
Ticker max_read1;
Ticker max_read3;
Ticker Motorcontrol;
- HIDScope scope( 4 );
+ HIDScope scope( 6 );
DigitalOut red(LED_RED);
DigitalOut blue(LED_BLUE);
DigitalOut green(LED_GREEN);
MODSERIAL pc(USBTX, USBRX);
-// EMG variables
+// EMG Variables
//Right Biceps
double emg1;
double emg1highfilter;
@@ -63,10 +63,47 @@
double emg4lowfilter;
double max4;
double maxpart4;
+// Moving Average Floats
+ // Right Biceps Movavg
+ float RB0, RB1, RB2, RB3, RB4, RB5, RB6, RB7, RB8, RB9, RB10, RB11, RB12,
+ RB13, RB14, RB15, RB16, RB17, RB18, RB19, RB20, RB21, RB22, RB23, RB24,
+ RB25, RB26, RB27, RB28, RB29, RB30, RB31, RB32, RB33, RB34, RB35, RB36,
+ RB37, RB38, RB39, RB40, RB41, RB42, RB43, RB44, RB45, RB46, RB47, RB48,
+ RB49, RB50, RB51, RB52, RB53, RB54, RB55, RB56, RB57, RB58, RB59, RB60,
+ RB61, RB62, RB63, RB64, RB65, RB66, RB67, RB68, RB69, RB70, RB71, RB72,
+ RB73, RB74, RB75, RB76, RB77, RB78, RB79, RB80, RB81, RB82, RB83, RB84,
+ RB85, RB86, RB87, MOVAVG_RB;
+ // Left Biceps Movavg
+ float LB0, LB1, LB2, LB3, LB4, LB5, LB6, LB7, LB8, LB9, LB10, LB11, LB12,
+ LB13, LB14, LB15, LB16, LB17, LB18, LB19, LB20, LB21, LB22, LB23, LB24,
+ LB25, LB26, LB27, LB28, LB29, LB30, LB31, LB32, LB33, LB34, LB35, LB36,
+ LB37, LB38, LB39, LB40, LB41, LB42, LB43, LB44, LB45, LB46, LB47, LB48,
+ LB49, LB50, LB51, LB52, LB53, LB54, LB55, LB56, LB57, LB58, LB59, LB60,
+ LB61, LB62, LB63, LB64, LB65, LB66, LB67, LB68, LB69, LB70, LB71, LB72,
+ LB73, LB74, LB75, LB76, LB77, LB78, LB79, LB80, LB81, LB82, LB83, LB84,
+ LB85, LB86, LB87, MOVAVG_LB;
+ // Left Lower Arm Movavg
+ float LL0, LL1, LL2, LL3, LL4, LL5, LL6, LL7, LL8, LL9, LL10, LL11, LL12,
+ LL13, LL14, LL15, LL16, LL17, LL18, LL19, LL20, LL21, LL22, LL23, LL24,
+ LL25, LL26, LL27, LL28, LL29, LL30, LL31, LL32, LL33, LL34, LL35, LL36,
+ LL37, LL38, LL39, LL40, LL41, LL42, LL43, LL44, LL45, LL46, LL47, LL48,
+ LL49, LL50, LL51, LL52, LL53, LL54, LL55, LL56, LL57, LL58, LL59, LL60,
+ LL61, LL62, LL63, LL64, LL65, LL66, LL67, LL68, LL69, LL70, LL71, LL72,
+ LL73, LL74, LL75, LL76, LL77, LL78, LL79, LL80, LL81, LL82, LL83, LL84,
+ LL85, LL86, LL87, MOVAVG_LL;
+ // Right Lower Arm Movavg
+ float RL0, RL1, RL2, RL3, RL4, RL5, RL6, RL7, RL8, RL9, RL10, RL11, RL12,
+ RL13, RL14, RL15, RL16, RL17, RL18, RL19, RL20, RL21, RL22, RL23, RL24,
+ RL25, RL26, RL27, RL28, RL29, RL30, RL31, RL32, RL33, RL34, RL35, RL36,
+ RL37, RL38, RL39, RL40, RL41, RL42, RL43, RL44, RL45, RL46, RL47, RL48,
+ RL49, RL50, RL51, RL52, RL53, RL54, RL55, RL56, RL57, RL58, RL59, RL60,
+ RL61, RL62, RL63, RL64, RL65, RL66, RL67, RL68, RL69, RL70, RL71, RL72,
+ RL73, RL74, RL75, RL76, RL77, RL78, RL79, RL80, RL81, RL82, RL83, RL84,
+ RL85, RL86, RL87, MOVAVG_RL;
+ float AV = 0.02; //multiplication value for adding each movavg value
+ // This value also adds a very slight gain to every value
-// Motor variables
- float referenceVelocity;
- float potMeterIn;
+// Motor Variables
float MV1 = 0;
float MV2 = 0;
@@ -104,26 +141,80 @@
emg1notchfilter = filternotch1.step(emg1highfilter); // notch filtered
emg1abs = fabs(emg1notchfilter); // take the absolute value
emg1lowfilter = filterlow1.step(emg1abs); // low pass filtered
+ RB0 = emg1lowfilter;
+ // Movavg is defined by rb0-rb75 and rb1 to rb75 are redefined by the value of rbx-1
+ MOVAVG_RB = RB0*AV+RB1*AV+RB2*AV+RB3*AV+RB4*AV+RB5*AV+RB6*AV+RB7*AV+RB8*AV
+ +RB9*AV+RB10*AV+RB11*AV+RB12*AV+RB13*AV+RB14*AV+RB15*AV+RB16*AV+RB17*AV+RB18
+ *AV+RB19*AV+RB20*AV+RB21*AV+RB22*AV+RB23*AV+RB24*AV+RB25*AV+RB26*AV+RB27
+ *AV+RB28*AV+RB29*AV+RB30*AV+RB31*AV+RB32*AV+RB33*AV+RB34*AV+RB35*AV+RB36*AV
+ +RB37*AV+RB38*AV+RB39*AV+RB40*AV+RB41*AV+RB42*AV+RB43*AV+RB44*AV+RB45*AV
+ +RB46*AV+RB47*AV+RB48*AV+RB49*AV+RB50*AV+RB51*AV+RB52*AV+RB53*AV+RB54*AV
+ +RB55*AV+RB56*AV+RB57*AV+RB58*AV+RB59*AV+RB60*AV+RB61*AV+RB62*AV+RB63*AV
+ +RB64*AV+RB65*AV+RB66*AV+RB67*AV+RB68*AV+RB69*AV+RB70*AV+RB71*AV+RB72*AV
+ +RB73*AV+RB74*AV+RB75*AV+RB76*AV+RB77*AV+RB78*AV+RB79*AV+RB80*AV+RB81*AV
+ +RB82*AV+RB83*AV+RB84*AV+RB85*AV+RB86*AV+RB87*AV;
+ RB87 = RB86; RB86 = RB85; RB85 = RB84; RB84 = RB83; RB83 = RB82; RB82 = RB81;
+ RB81 = RB80; RB80 = RB79; RB79 = RB78; RB78 = RB77; RB77 = RB76; RB76 = RB75;
+ RB75 = RB74; RB74 = RB73; RB73 = RB72; RB72 = RB71; RB71 = RB70; RB70 = RB69;
+ RB69 = RB68; RB68 = RB67; RB67 = RB66; RB66 = RB65; RB65 = RB64; RB64 = RB63;
+ RB63 = RB62; RB62 = RB61; RB61 = RB60; RB60 = RB59; RB59 = RB58; RB58 = RB57;
+ RB57 = RB56; RB56 = RB55; RB55 = RB54; RB54 = RB53; RB53 = RB52; RB52 = RB51;
+ RB51 = RB50; RB50 = RB49; RB49 = RB48; RB48 = RB47; RB47 = RB46; RB46 = RB45;
+ RB45 = RB44; RB44 = RB43; RB43 = RB42; RB42 = RB41; RB41 = RB40; RB40 = RB39;
+ RB39 = RB38; RB38 = RB37; RB37 = RB36; RB36 = RB35; RB35 = RB34; RB34 = RB33;
+ RB33 = RB32; RB32 = RB31; RB31 = RB30; RB30 = RB29; RB29 = RB28; RB28 = RB27;
+ RB27 = RB26; RB26 = RB25; RB25 = RB24; RB24 = RB23; RB23 = RB22; RB22 = RB21;
+ RB21 = RB20; RB20 = RB19; RB19 = RB18; RB18 = RB17; RB17 = RB16; RB16 = RB15;
+ RB15 = RB14; RB14 = RB13; RB13 = RB12; RB12 = RB11; RB11 = RB10; RB10 = RB9;
+ RB9 = RB8; RB8 = RB7; RB7 = RB6; RB6 = RB5; RB5 = RB4; RB4 = RB3; RB3 = RB2;
+ RB2 = RB1; RB1 = RB0;
emg2 = emg2_in.read(); // read out emg
emg2highfilter = filterhigh2.step(emg2); // high pass filtered
emg2notchfilter = filternotch2.step(emg2highfilter); // notch filtered
emg2abs = fabs(emg2notchfilter); // take the absolute value
emg2lowfilter = filterlow2.step(emg2abs); // low pass filtered
+ LB0 = emg2lowfilter;
+ // same story as with the right biceps
+ MOVAVG_LB = LB0*AV+LB1*AV+LB2*AV+LB3*AV+LB4*AV+LB5*AV+LB6*AV+LB7*AV+LB8*AV
+ +LB9*AV+LB10*AV+LB11*AV+LB12*AV+LB13*AV+LB14*AV+LB15*AV+LB16*AV+LB17*AV+LB18
+ *AV+LB19*AV+LB20*AV+LB21*AV+LB22*AV+LB23*AV+LB24*AV+LB25*AV+LB26*AV+LB27
+ *AV+LB28*AV+LB29*AV+LB30*AV+LB31*AV+LB32*AV+LB33*AV+LB34*AV+LB35*AV+LB36*AV
+ +LB37*AV+LB38*AV+LB39*AV+LB40*AV+LB41*AV+LB42*AV+LB43*AV+LB44*AV+LB45*AV
+ +LB46*AV+LB47*AV+LB48*AV+LB49*AV+LB50*AV+LB51*AV+LB52*AV+LB53*AV+LB54*AV
+ +LB55*AV+LB56*AV+LB57*AV+LB58*AV+LB59*AV+LB60*AV+LB61*AV+LB62*AV+LB63*AV
+ +LB64*AV+LB65*AV+LB66*AV+LB67*AV+LB68*AV+LB69*AV+LB70*AV+LB71*AV+LB72*AV
+ +LB73*AV+LB74*AV+LB75*AV+LB76*AV+LB77*AV+LB78*AV+LB79*AV+LB80*AV+LB81*AV
+ +LB82*AV+LB83*AV+LB84*AV+LB85*AV+LB86*AV+LB87*AV;
+ LB87 = LB86; LB86 = LB85; LB85 = LB84; LB84 = LB83; LB83 = LB82; LB82 = LB81;
+ LB81 = LB80; LB80 = LB79; LB79 = LB78; LB78 = LB77; LB77 = LB76; LB76 = LB75;
+ LB75 = LB74; LB74 = LB73; LB73 = LB72; LB72 = LB71; LB71 = LB70; LB70 = LB69;
+ LB69 = LB68; LB68 = LB67; LB67 = LB66; LB66 = LB65; LB65 = LB64; LB64 = LB63;
+ LB63 = LB62; LB62 = LB61; LB61 = LB60; LB60 = LB59; LB59 = LB58; LB58 = LB57;
+ LB57 = LB56; LB56 = LB55; LB55 = LB54; LB54 = LB53; LB53 = LB52; LB52 = LB51;
+ LB51 = LB50; LB50 = LB49; LB49 = LB48; LB48 = LB47; LB47 = LB46; LB46 = LB45;
+ LB45 = LB44; LB44 = LB43; LB43 = LB42; LB42 = LB41; LB41 = LB40; LB40 = LB39;
+ LB39 = LB38; LB38 = LB37; LB37 = LB36; LB36 = LB35; LB35 = LB34; LB34 = LB33;
+ LB33 = LB32; LB32 = LB31; LB31 = LB30; LB30 = LB29; LB29 = LB28; LB28 = LB27;
+ LB27 = LB26; LB26 = LB25; LB25 = LB24; LB24 = LB23; LB23 = LB22; LB22 = LB21;
+ LB21 = LB20; LB20 = LB19; LB19 = LB18; LB18 = LB17; LB17 = LB16; LB16 = LB15;
+ LB15 = LB14; LB14 = LB13; LB13 = LB12; LB12 = LB11; LB11 = LB10; LB10 = LB9;
+ LB9 = LB8; LB8 = LB7; LB7 = LB6; LB6 = LB5; LB5 = LB4; LB4 = LB3; LB3 = LB2;
+ LB2 = LB1; LB1 = LB0;
- if (max1<emg1lowfilter){
- max1 = emg1lowfilter; // set the max value at the highest measured value
+ if (max1<MOVAVG_RB){
+ max1 = MOVAVG_RB; // set the max value at the highest measured value
}
- if (max2<emg2lowfilter){
- max2 = emg2lowfilter; // set the max value at the highest measured value
+ if (max2<MOVAVG_LB){
+ max2 = MOVAVG_LB; // set the max value at the highest measured value
}
wait(0.001f); // measure at 1000Hz
}
wait(0.2f);
green = 1;
}
- maxpart1 = 0.11*max1; // set cut off voltage at 13% of max for right biceps
- maxpart2 = 0.13*max2; // set cut off voltage at 13% of max for left biceps
+ maxpart1 = 0.14*max1; // set cut off voltage at 13% of max for right biceps
+ maxpart2 = 0.15*max2; // set cut off voltage at 13% of max for left biceps
}
// calibration of both lower arms
@@ -140,26 +231,80 @@
emg3notchfilter = filternotch3.step(emg3highfilter);
emg3abs = fabs(emg3notchfilter);
emg3lowfilter = filterlow3.step(emg3abs);
+ LL0 = emg3lowfilter;
+ // same story as with the right biceps
+ MOVAVG_LL = LL0*AV+LL1*AV+LL2*AV+LL3*AV+LL4*AV+LL5*AV+LL6*AV+LL7*AV+LL8*AV
+ +LL9*AV+LL10*AV+LL11*AV+LL12*AV+LL13*AV+LL14*AV+LL15*AV+LL16*AV+LL17*AV+LL18
+ *AV+LL19*AV+LL20*AV+LL21*AV+LL22*AV+LL23*AV+LL24*AV+LL25*AV+LL26*AV+LL27
+ *AV+LL28*AV+LL29*AV+LL30*AV+LL31*AV+LL32*AV+LL33*AV+LL34*AV+LL35*AV+LL36*AV
+ +LL37*AV+LL38*AV+LL39*AV+LL40*AV+LL41*AV+LL42*AV+LL43*AV+LL44*AV+LL45*AV
+ +LL46*AV+LL47*AV+LL48*AV+LL49*AV+LL50*AV+LL51*AV+LL52*AV+LL53*AV+LL54*AV
+ +LL55*AV+LL56*AV+LL57*AV+LL58*AV+LL59*AV+LL60*AV+LL61*AV+LL62*AV+LL63*AV
+ +LL64*AV+LL65*AV+LL66*AV+LL67*AV+LL68*AV+LL69*AV+LL70*AV+LL71*AV+LL72*AV
+ +LL73*AV+LL74*AV+LL75*AV+LL76*AV+LL77*AV+LL78*AV+LL79*AV+LL80*AV+LL81*AV
+ +LL82*AV+LL83*AV+LL84*AV+LL85*AV+LL86*AV+LL87*AV;
+ LL87 = LL86; LL86 = LL85; LL85 = LL84; LL84 = LL83; LL83 = LL82; LL82 = LL81;
+ LL81 = LL80; LL80 = LL79; LL79 = LL78; LL78 = LL77; LL77 = LL76; LL76 = LL75;
+ LL75 = LL74; LL74 = LL73; LL73 = LL72; LL72 = LL71; LL71 = LL70; LL70 = LL69;
+ LL69 = LL68; LL68 = LL67; LL67 = LL66; LL66 = LL65; LL65 = LL64; LL64 = LL63;
+ LL63 = LL62; LL62 = LL61; LL61 = LL60; LL60 = LL59; LL59 = LL58; LL58 = LL57;
+ LL57 = LL56; LL56 = LL55; LL55 = LL54; LL54 = LL53; LL53 = LL52; LL52 = LL51;
+ LL51 = LL50; LL50 = LL49; LL49 = LL48; LL48 = LL47; LL47 = LL46; LL46 = LL45;
+ LL45 = LL44; LL44 = LL43; LL43 = LL42; LL42 = LL41; LL41 = LL40; LL40 = LL39;
+ LL39 = LL38; LL38 = LL37; LL37 = LL36; LL36 = LL35; LL35 = LL34; LL34 = LL33;
+ LL33 = LL32; LL32 = LL31; LL31 = LL30; LL30 = LL29; LL29 = LL28; LL28 = LL27;
+ LL27 = LL26; LL26 = LL25; LL25 = LL24; LL24 = LL23; LL23 = LL22; LL22 = LL21;
+ LL21 = LL20; LL20 = LL19; LL19 = LL18; LL18 = LL17; LL17 = LL16; LL16 = LL15;
+ LL15 = LL14; LL14 = LL13; LL13 = LL12; LL12 = LL11; LL11 = LL10; LL10 = LL9;
+ LL9 = LL8; LL8 = LL7; LL7 = LL6; LL6 = LL5; LL5 = LL4; LL4 = LL3; LL3 = LL2;
+ LL2 = LL1; LL1 = LL0;
emg4 = emg4_in.read();
emg4highfilter = filterhigh4.step(emg4);
emg4notchfilter = filternotch4.step(emg4highfilter);
emg4abs = fabs(emg4notchfilter);
emg4lowfilter = filterlow4.step(emg4abs);
+ RL0 = emg4lowfilter;
+ // same story as with the right biceps
+ MOVAVG_RL = RL0*AV+RL1*AV+RL2*AV+RL3*AV+RL4*AV+RL5*AV+RL6*AV+RL7*AV+RL8*AV
+ +RL9*AV+RL10*AV+RL11*AV+RL12*AV+RL13*AV+RL14*AV+RL15*AV+RL16*AV+RL17*AV+RL18
+ *AV+RL19*AV+RL20*AV+RL21*AV+RL22*AV+RL23*AV+RL24*AV+RL25*AV+RL26*AV+RL27
+ *AV+RL28*AV+RL29*AV+RL30*AV+RL31*AV+RL32*AV+RL33*AV+RL34*AV+RL35*AV+RL36*AV
+ +RL37*AV+RL38*AV+RL39*AV+RL40*AV+RL41*AV+RL42*AV+RL43*AV+RL44*AV+RL45*AV
+ +RL46*AV+RL47*AV+RL48*AV+RL49*AV+RL50*AV+RL51*AV+RL52*AV+RL53*AV+RL54*AV
+ +RL55*AV+RL56*AV+RL57*AV+RL58*AV+RL59*AV+RL60*AV+RL61*AV+RL62*AV+RL63*AV
+ +RL64*AV+RL65*AV+RL66*AV+RL67*AV+RL68*AV+RL69*AV+RL70*AV+RL71*AV+RL72*AV
+ +RL73*AV+RL74*AV+RL75*AV+RL76*AV+RL77*AV+RL78*AV+RL79*AV+RL80*AV+RL81*AV
+ +RL82*AV+RL83*AV+RL84*AV+RL85*AV+RL86*AV+RL87*AV;
+ RL87 = RL86; RL86 = RL85; RL85 = RL84; RL84 = RL83; RL83 = RL82; RL82 = RL81;
+ RL81 = RL80; RL80 = RL79; RL79 = RL78; RL78 = RL77; RL77 = RL76; RL76 = RL75;
+ RL75 = RL74; RL74 = RL73; RL73 = RL72; RL72 = RL71; RL71 = RL70; RL70 = RL69;
+ RL69 = RL68; RL68 = RL67; RL67 = RL66; RL66 = RL65; RL65 = RL64; RL64 = RL63;
+ RL63 = RL62; RL62 = RL61; RL61 = RL60; RL60 = RL59; RL59 = RL58; RL58 = RL57;
+ RL57 = RL56; RL56 = RL55; RL55 = RL54; RL54 = RL53; RL53 = RL52; RL52 = RL51;
+ RL51 = RL50; RL50 = RL49; RL49 = RL48; RL48 = RL47; RL47 = RL46; RL46 = RL45;
+ RL45 = RL44; RL44 = RL43; RL43 = RL42; RL42 = RL41; RL41 = RL40; RL40 = RL39;
+ RL39 = RL38; RL38 = RL37; RL37 = RL36; RL36 = RL35; RL35 = RL34; RL34 = RL33;
+ RL33 = RL32; RL32 = RL31; RL31 = RL30; RL30 = RL29; RL29 = RL28; RL28 = RL27;
+ RL27 = RL26; RL26 = RL25; RL25 = RL24; RL24 = RL23; RL23 = RL22; RL22 = RL21;
+ RL21 = RL20; RL20 = RL19; RL19 = RL18; RL18 = RL17; RL17 = RL16; RL16 = RL15;
+ RL15 = RL14; RL14 = RL13; RL13 = RL12; RL12 = RL11; RL11 = RL10; RL10 = RL9;
+ RL9 = RL8; RL8 = RL7; RL7 = RL6; RL6 = RL5; RL5 = RL4; RL4 = RL3; RL3 = RL2;
+ RL2 = RL1; RL1 = RL0;
- if (max3<emg3lowfilter){
- max3 = emg3lowfilter; // set the max value at the highest measured value
+ if (max3<MOVAVG_LL){
+ max3 = MOVAVG_LL; // set the max value at the highest measured value
}
- if (max4<emg4lowfilter){
- max4 = emg4lowfilter; // set the max value at the highest measured value
+ if (max4<MOVAVG_RL){
+ max4 = MOVAVG_RL; // set the max value at the highest measured value
}
wait(0.001f);
}
wait(0.2f);
red = 1;
}
- maxpart3 = 0.15*max3; // set cut off voltage at 15% of max for left lower arm
- maxpart4 = 0.15*max4; // set cut off voltage at 15% of max for right lower arm
+ maxpart3 = 0.17*max3; // set cut off voltage at 15% of max for left lower arm
+ maxpart4 = 0.17*max4; // set cut off voltage at 15% of max for right lower arm
}
// Filtering & Scope
@@ -170,30 +315,138 @@
emg1notchfilter = filternotch1.step(emg1highfilter);
emg1abs = fabs(emg1notchfilter);
emg1lowfilter = filterlow1.step(emg1abs); // Final Right Biceps values to be sent
+ RB0 = emg1lowfilter;
+ // Movavg is defined by rb0-rb75 and rb1 to rb75 are redefined by the value of rbx-1
+ MOVAVG_RB = RB0*AV+RB1*AV+RB2*AV+RB3*AV+RB4*AV+RB5*AV+RB6*AV+RB7*AV+RB8*AV
+ +RB9*AV+RB10*AV+RB11*AV+RB12*AV+RB13*AV+RB14*AV+RB15*AV+RB16*AV+RB17*AV+RB18
+ *AV+RB19*AV+RB20*AV+RB21*AV+RB22*AV+RB23*AV+RB24*AV+RB25*AV+RB26*AV+RB27
+ *AV+RB28*AV+RB29*AV+RB30*AV+RB31*AV+RB32*AV+RB33*AV+RB34*AV+RB35*AV+RB36*AV
+ +RB37*AV+RB38*AV+RB39*AV+RB40*AV+RB41*AV+RB42*AV+RB43*AV+RB44*AV+RB45*AV
+ +RB46*AV+RB47*AV+RB48*AV+RB49*AV+RB50*AV+RB51*AV+RB52*AV+RB53*AV+RB54*AV
+ +RB55*AV+RB56*AV+RB57*AV+RB58*AV+RB59*AV+RB60*AV+RB61*AV+RB62*AV+RB63*AV
+ +RB64*AV+RB65*AV+RB66*AV+RB67*AV+RB68*AV+RB69*AV+RB70*AV+RB71*AV+RB72*AV
+ +RB73*AV+RB74*AV+RB75*AV+RB76*AV+RB77*AV+RB78*AV+RB79*AV+RB80*AV+RB81*AV
+ +RB82*AV+RB83*AV+RB84*AV+RB85*AV+RB86*AV+RB87*AV;
+ RB87 = RB86; RB86 = RB85; RB85 = RB84; RB84 = RB83; RB83 = RB82; RB82 = RB81;
+ RB81 = RB80; RB80 = RB79; RB79 = RB78; RB78 = RB77; RB77 = RB76; RB76 = RB75;
+ RB75 = RB74; RB74 = RB73; RB73 = RB72; RB72 = RB71; RB71 = RB70; RB70 = RB69;
+ RB69 = RB68; RB68 = RB67; RB67 = RB66; RB66 = RB65; RB65 = RB64; RB64 = RB63;
+ RB63 = RB62; RB62 = RB61; RB61 = RB60; RB60 = RB59; RB59 = RB58; RB58 = RB57;
+ RB57 = RB56; RB56 = RB55; RB55 = RB54; RB54 = RB53; RB53 = RB52; RB52 = RB51;
+ RB51 = RB50; RB50 = RB49; RB49 = RB48; RB48 = RB47; RB47 = RB46; RB46 = RB45;
+ RB45 = RB44; RB44 = RB43; RB43 = RB42; RB42 = RB41; RB41 = RB40; RB40 = RB39;
+ RB39 = RB38; RB38 = RB37; RB37 = RB36; RB36 = RB35; RB35 = RB34; RB34 = RB33;
+ RB33 = RB32; RB32 = RB31; RB31 = RB30; RB30 = RB29; RB29 = RB28; RB28 = RB27;
+ RB27 = RB26; RB26 = RB25; RB25 = RB24; RB24 = RB23; RB23 = RB22; RB22 = RB21;
+ RB21 = RB20; RB20 = RB19; RB19 = RB18; RB18 = RB17; RB17 = RB16; RB16 = RB15;
+ RB15 = RB14; RB14 = RB13; RB13 = RB12; RB12 = RB11; RB11 = RB10; RB10 = RB9;
+ RB9 = RB8; RB8 = RB7; RB7 = RB6; RB6 = RB5; RB5 = RB4; RB4 = RB3; RB3 = RB2;
+ RB2 = RB1; RB1 = RB0;
// Left Biceps
emg2 = emg2_in.read();
emg2highfilter = filterhigh2.step(emg2);
emg2notchfilter = filternotch2.step(emg2highfilter);
emg2abs = fabs(emg2notchfilter);
emg2lowfilter = filterlow2.step(emg2abs); // Final Left Biceps values to be sent
+ LB0 = emg2lowfilter;
+ // same story as with the right biceps
+ MOVAVG_LB = LB0*AV+LB1*AV+LB2*AV+LB3*AV+LB4*AV+LB5*AV+LB6*AV+LB7*AV+LB8*AV
+ +LB9*AV+LB10*AV+LB11*AV+LB12*AV+LB13*AV+LB14*AV+LB15*AV+LB16*AV+LB17*AV+LB18
+ *AV+LB19*AV+LB20*AV+LB21*AV+LB22*AV+LB23*AV+LB24*AV+LB25*AV+LB26*AV+LB27
+ *AV+LB28*AV+LB29*AV+LB30*AV+LB31*AV+LB32*AV+LB33*AV+LB34*AV+LB35*AV+LB36*AV
+ +LB37*AV+LB38*AV+LB39*AV+LB40*AV+LB41*AV+LB42*AV+LB43*AV+LB44*AV+LB45*AV
+ +LB46*AV+LB47*AV+LB48*AV+LB49*AV+LB50*AV+LB51*AV+LB52*AV+LB53*AV+LB54*AV
+ +LB55*AV+LB56*AV+LB57*AV+LB58*AV+LB59*AV+LB60*AV+LB61*AV+LB62*AV+LB63*AV
+ +LB64*AV+LB65*AV+LB66*AV+LB67*AV+LB68*AV+LB69*AV+LB70*AV+LB71*AV+LB72*AV
+ +LB73*AV+LB74*AV+LB75*AV+LB76*AV+LB77*AV+LB78*AV+LB79*AV+LB80*AV+LB81*AV
+ +LB82*AV+LB83*AV+LB84*AV+LB85*AV+LB86*AV+LB87*AV;
+ LB87 = LB86; LB86 = LB85; LB85 = LB84; LB84 = LB83; LB83 = LB82; LB82 = LB81;
+ LB81 = LB80; LB80 = LB79; LB79 = LB78; LB78 = LB77; LB77 = LB76; LB76 = LB75;
+ LB75 = LB74; LB74 = LB73; LB73 = LB72; LB72 = LB71; LB71 = LB70; LB70 = LB69;
+ LB69 = LB68; LB68 = LB67; LB67 = LB66; LB66 = LB65; LB65 = LB64; LB64 = LB63;
+ LB63 = LB62; LB62 = LB61; LB61 = LB60; LB60 = LB59; LB59 = LB58; LB58 = LB57;
+ LB57 = LB56; LB56 = LB55; LB55 = LB54; LB54 = LB53; LB53 = LB52; LB52 = LB51;
+ LB51 = LB50; LB50 = LB49; LB49 = LB48; LB48 = LB47; LB47 = LB46; LB46 = LB45;
+ LB45 = LB44; LB44 = LB43; LB43 = LB42; LB42 = LB41; LB41 = LB40; LB40 = LB39;
+ LB39 = LB38; LB38 = LB37; LB37 = LB36; LB36 = LB35; LB35 = LB34; LB34 = LB33;
+ LB33 = LB32; LB32 = LB31; LB31 = LB30; LB30 = LB29; LB29 = LB28; LB28 = LB27;
+ LB27 = LB26; LB26 = LB25; LB25 = LB24; LB24 = LB23; LB23 = LB22; LB22 = LB21;
+ LB21 = LB20; LB20 = LB19; LB19 = LB18; LB18 = LB17; LB17 = LB16; LB16 = LB15;
+ LB15 = LB14; LB14 = LB13; LB13 = LB12; LB12 = LB11; LB11 = LB10; LB10 = LB9;
+ LB9 = LB8; LB8 = LB7; LB7 = LB6; LB6 = LB5; LB5 = LB4; LB4 = LB3; LB3 = LB2;
+ LB2 = LB1; LB1 = LB0;
// Left Lower Arm OR Triceps
emg3 = emg3_in.read();
emg3highfilter = filterhigh3.step(emg3);
emg3notchfilter = filternotch3.step(emg3highfilter);
emg3abs = fabs(emg3notchfilter);
emg3lowfilter = filterlow3.step(emg3abs); // Final Lower Arm values to be sent
+ LL0 = emg3lowfilter;
+ // same story as with the right biceps
+ MOVAVG_LL = LL0*AV+LL1*AV+LL2*AV+LL3*AV+LL4*AV+LL5*AV+LL6*AV+LL7*AV+LL8*AV
+ +LL9*AV+LL10*AV+LL11*AV+LL12*AV+LL13*AV+LL14*AV+LL15*AV+LL16*AV+LL17*AV+LL18
+ *AV+LL19*AV+LL20*AV+LL21*AV+LL22*AV+LL23*AV+LL24*AV+LL25*AV+LL26*AV+LL27
+ *AV+LL28*AV+LL29*AV+LL30*AV+LL31*AV+LL32*AV+LL33*AV+LL34*AV+LL35*AV+LL36*AV
+ +LL37*AV+LL38*AV+LL39*AV+LL40*AV+LL41*AV+LL42*AV+LL43*AV+LL44*AV+LL45*AV
+ +LL46*AV+LL47*AV+LL48*AV+LL49*AV+LL50*AV+LL51*AV+LL52*AV+LL53*AV+LL54*AV
+ +LL55*AV+LL56*AV+LL57*AV+LL58*AV+LL59*AV+LL60*AV+LL61*AV+LL62*AV+LL63*AV
+ +LL64*AV+LL65*AV+LL66*AV+LL67*AV+LL68*AV+LL69*AV+LL70*AV+LL71*AV+LL72*AV
+ +LL73*AV+LL74*AV+LL75*AV+LL76*AV+LL77*AV+LL78*AV+LL79*AV+LL80*AV+LL81*AV
+ +LL82*AV+LL83*AV+LL84*AV+LL85*AV+LL86*AV+LL87*AV;
+ LL87 = LL86; LL86 = LL85; LL85 = LL84; LL84 = LL83; LL83 = LL82; LL82 = LL81;
+ LL81 = LL80; LL80 = LL79; LL79 = LL78; LL78 = LL77; LL77 = LL76; LL76 = LL75;
+ LL75 = LL74; LL74 = LL73; LL73 = LL72; LL72 = LL71; LL71 = LL70; LL70 = LL69;
+ LL69 = LL68; LL68 = LL67; LL67 = LL66; LL66 = LL65; LL65 = LL64; LL64 = LL63;
+ LL63 = LL62; LL62 = LL61; LL61 = LL60; LL60 = LL59; LL59 = LL58; LL58 = LL57;
+ LL57 = LL56; LL56 = LL55; LL55 = LL54; LL54 = LL53; LL53 = LL52; LL52 = LL51;
+ LL51 = LL50; LL50 = LL49; LL49 = LL48; LL48 = LL47; LL47 = LL46; LL46 = LL45;
+ LL45 = LL44; LL44 = LL43; LL43 = LL42; LL42 = LL41; LL41 = LL40; LL40 = LL39;
+ LL39 = LL38; LL38 = LL37; LL37 = LL36; LL36 = LL35; LL35 = LL34; LL34 = LL33;
+ LL33 = LL32; LL32 = LL31; LL31 = LL30; LL30 = LL29; LL29 = LL28; LL28 = LL27;
+ LL27 = LL26; LL26 = LL25; LL25 = LL24; LL24 = LL23; LL23 = LL22; LL22 = LL21;
+ LL21 = LL20; LL20 = LL19; LL19 = LL18; LL18 = LL17; LL17 = LL16; LL16 = LL15;
+ LL15 = LL14; LL14 = LL13; LL13 = LL12; LL12 = LL11; LL11 = LL10; LL10 = LL9;
+ LL9 = LL8; LL8 = LL7; LL7 = LL6; LL6 = LL5; LL5 = LL4; LL4 = LL3; LL3 = LL2;
+ LL2 = LL1; LL1 = LL0;
// Right Lower Arm OR Triceps
emg4 = emg4_in.read();
emg4highfilter = filterhigh4.step(emg4);
emg4notchfilter = filternotch4.step(emg4highfilter);
emg4abs = fabs(emg4notchfilter);
- emg4lowfilter = filterlow4.step(emg4abs); // Final Lower Arm values to be sent
+ emg4lowfilter = filterlow4.step(emg4abs); // Final Lower Arm values to be sent
+ RL0 = emg4lowfilter;
+ // same story as with the right biceps
+ MOVAVG_RL = RL0*AV+RL1*AV+RL2*AV+RL3*AV+RL4*AV+RL5*AV+RL6*AV+RL7*AV+RL8*AV
+ +RL9*AV+RL10*AV+RL11*AV+RL12*AV+RL13*AV+RL14*AV+RL15*AV+RL16*AV+RL17*AV+RL18
+ *AV+RL19*AV+RL20*AV+RL21*AV+RL22*AV+RL23*AV+RL24*AV+RL25*AV+RL26*AV+RL27
+ *AV+RL28*AV+RL29*AV+RL30*AV+RL31*AV+RL32*AV+RL33*AV+RL34*AV+RL35*AV+RL36*AV
+ +RL37*AV+RL38*AV+RL39*AV+RL40*AV+RL41*AV+RL42*AV+RL43*AV+RL44*AV+RL45*AV
+ +RL46*AV+RL47*AV+RL48*AV+RL49*AV+RL50*AV+RL51*AV+RL52*AV+RL53*AV+RL54*AV
+ +RL55*AV+RL56*AV+RL57*AV+RL58*AV+RL59*AV+RL60*AV+RL61*AV+RL62*AV+RL63*AV
+ +RL64*AV+RL65*AV+RL66*AV+RL67*AV+RL68*AV+RL69*AV+RL70*AV+RL71*AV+RL72*AV
+ +RL73*AV+RL74*AV+RL75*AV+RL76*AV+RL77*AV+RL78*AV+RL79*AV+RL80*AV+RL81*AV
+ +RL82*AV+RL83*AV+RL84*AV+RL85*AV+RL86*AV+RL87*AV;
+ RL87 = RL86; RL86 = RL85; RL85 = RL84; RL84 = RL83; RL83 = RL82; RL82 = RL81;
+ RL81 = RL80; RL80 = RL79; RL79 = RL78; RL78 = RL77; RL77 = RL76; RL76 = RL75;
+ RL75 = RL74; RL74 = RL73; RL73 = RL72; RL72 = RL71; RL71 = RL70; RL70 = RL69;
+ RL69 = RL68; RL68 = RL67; RL67 = RL66; RL66 = RL65; RL65 = RL64; RL64 = RL63;
+ RL63 = RL62; RL62 = RL61; RL61 = RL60; RL60 = RL59; RL59 = RL58; RL58 = RL57;
+ RL57 = RL56; RL56 = RL55; RL55 = RL54; RL54 = RL53; RL53 = RL52; RL52 = RL51;
+ RL51 = RL50; RL50 = RL49; RL49 = RL48; RL48 = RL47; RL47 = RL46; RL46 = RL45;
+ RL45 = RL44; RL44 = RL43; RL43 = RL42; RL42 = RL41; RL41 = RL40; RL40 = RL39;
+ RL39 = RL38; RL38 = RL37; RL37 = RL36; RL36 = RL35; RL35 = RL34; RL34 = RL33;
+ RL33 = RL32; RL32 = RL31; RL31 = RL30; RL30 = RL29; RL29 = RL28; RL28 = RL27;
+ RL27 = RL26; RL26 = RL25; RL25 = RL24; RL24 = RL23; RL23 = RL22; RL22 = RL21;
+ RL21 = RL20; RL20 = RL19; RL19 = RL18; RL18 = RL17; RL17 = RL16; RL16 = RL15;
+ RL15 = RL14; RL14 = RL13; RL13 = RL12; RL12 = RL11; RL11 = RL10; RL10 = RL9;
+ RL9 = RL8; RL8 = RL7; RL7 = RL6; RL6 = RL5; RL5 = RL4; RL4 = RL3; RL3 = RL2;
+ RL2 = RL1; RL1 = RL0;
// Compare measurement to the calibrated value to decide actions
// more options could be added if the callibration is well defined enough
// This part checks for right biceps contractions only
- if (maxpart1<emg1lowfilter && maxpart2>emg2lowfilter && maxpart3>emg3lowfilter && maxpart4>emg4lowfilter){
+ if (maxpart1<MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
red = 1;
blue = 1;
green = 0;
@@ -201,7 +454,7 @@
MV2 = 0;
}
// This part checks for left biceps contractions only
- else if (maxpart1>emg1lowfilter && maxpart2<emg2lowfilter && maxpart3>emg3lowfilter && maxpart4>emg4lowfilter){
+ else if (maxpart1>MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
red = 0;
blue = 1;
green = 1;
@@ -209,7 +462,7 @@
MV2 = 0;
}
// This part checks for left lower arm contractions only
- else if (maxpart1>emg1lowfilter && maxpart2>emg2lowfilter && maxpart3<emg3lowfilter && maxpart4>emg4lowfilter){
+ else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4>MOVAVG_RL){
red = 1;
blue = 0;
green = 1;
@@ -217,7 +470,7 @@
MV2 = 0.5;
}
// This part checks for right lower arm contractions only
- else if (maxpart1>emg1lowfilter && maxpart2>emg2lowfilter && maxpart3>emg3lowfilter && maxpart4<emg4lowfilter){
+ else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4<MOVAVG_RL){
red = 0;
blue = 1;
green = 0;
@@ -225,7 +478,7 @@
MV2 = -0.5;
}
// This part checks for both lower arm contractions only
- else if (maxpart1>emg1lowfilter && maxpart2>emg2lowfilter && maxpart3<emg3lowfilter && maxpart4<emg4lowfilter){
+ else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4<MOVAVG_RL){
red = 0;
blue = 0;
green = 0;
@@ -233,7 +486,7 @@
MV2 = -0.5;
}
// This part checks for both biceps contractions only
- else if (maxpart1<emg1lowfilter && maxpart2<emg2lowfilter && maxpart3>emg3lowfilter && maxpart4>emg4lowfilter){
+ else if (maxpart1<MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
red = 0;
blue = 0;
green = 0;
@@ -241,7 +494,7 @@
MV2 = 0.5;
}
// This part checks for right lower arm & left biceps contractions only
- else if (maxpart1>emg1lowfilter && maxpart2<emg2lowfilter && maxpart3>emg3lowfilter && maxpart4<emg4lowfilter){
+ else if (maxpart1>MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4<MOVAVG_RL){
red = 0;
blue = 0;
green = 0;
@@ -249,7 +502,7 @@
MV2 = -0.5;
}
// This part checks for left lower arm & right biceps contractions only
- else if (maxpart1<emg1lowfilter && maxpart2>emg2lowfilter && maxpart3<emg3lowfilter && maxpart4>emg4lowfilter){
+ else if (maxpart1<MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4>MOVAVG_RL){
red = 0;
blue = 0;
green = 0;
@@ -266,10 +519,10 @@
}
// Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope'
- scope.set(0, emg1lowfilter ); // plot Right biceps voltage
- scope.set(1, emg2lowfilter ); // Plot Left biceps voltage
- scope.set(2, emg3lowfilter ); // Plot Lower Left Arm voltage
- scope.set(3, emg4lowfilter ); // Plot Lower Right Arm Voltage
+ scope.set(0, MOVAVG_RB ); // plot Right biceps voltage
+ scope.set(1, MOVAVG_LB ); // Plot Left biceps voltage
+ scope.set(2, MOVAVG_LL ); // Plot Lower Left Arm voltage
+ scope.set(3, MOVAVG_RL ); // Plot Lower Right Arm Voltage
scope.send(); // send everything to the HID scope
}
@@ -310,6 +563,6 @@
main_timer.attach(&filter, 0.001); // set frequency for the filters at 1000Hz
max_read1.attach(&get_max1, 2); // set the frequency of the calibration loop at 0.5Hz
max_read3.attach(&get_max3, 2);
- Motorcontrol.attach(MeasureAndControl,0.5);
+ Motorcontrol.attach(&MeasureAndControl,0.5);
while(1) {}
}
\ No newline at end of file