Dan August
/
EMG2spierJorick
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
EMG2spier
by 
Jorick Leferink
Revision 5:6fd237516c27, committed 2013-10-30
- Comitter:
- DanAuhust
- Date:
- Wed Oct 30 15:05:06 2013 +0000
- Parent:
- 4:41222da4a577
- Commit message:
- asddf
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Mon Oct 28 12:52:33 2013 +0000
+++ b/main.cpp Wed Oct 30 15:05:06 2013 +0000
@@ -17,13 +17,15 @@
#define inertia 4
#define gain_biceps 1
-#define threshold_biceps 0.04
+#define threshold_biceps 0.06
#define border_biceps 0.1125 //25% van .57 - .12
+#define cap_biceps 2
#define gain_triceps 1
-#define threshold_triceps 0.04
+#define threshold_triceps 0.06
#define border_triceps 0.1237 //25% van .605 - .11 = .12375
-
+#define cap_triceps 2
+// constanten butterworth in aantekeningen EMG op dropbox
#define NUM0 0.8841 // constante
#define NUM1 -3.53647 // z^-1
#define NUM2 5.3046 // z^-2etc.
@@ -42,7 +44,8 @@
//signal_number=3 --> flexoren filteren
//signal_number=4 --> extensoren filteren
-float filter(int signal_number){
+float filter(int signal_number)
+{
//static variables keep their values between function calls
//the assignents are only executed the first iteration.
@@ -69,15 +72,16 @@
static float square_biceps = 0, square_triceps = 0, square_flexoren = 0, square_extensoren = 0;
static float sum_biceps = 0, sum_triceps = 0, sum_flexoren = 0, sum_extensoren = 0;
static float mean_biceps = 0.1, mean_triceps = 0.1, mean_flexoren = 0.1, mean_extensoren = 0.1;
-// static float emg_biceps, emg_triceps, emg_flexoren, emg_extensoren; // output ruwe EMG
+ //static float emg_biceps, emg_triceps, emg_flexoren, emg_extensoren; // output ruwe EMG
static float sig_in_biceps, sig_in_triceps, sig_in_extensoren, sig_in_flexoren; // naam gewijzigd, output StDev
static float sig_out_biceps, sig_out_triceps, sig_out_extensoren, sig_out_flexoren;
- float emg_abs, sig_out, deltaV;
+ float sig_out, deltaV;
static float sig_prev_biceps = 0, sig_prev_triceps = 0, sig_prev_extensoren = 0, sig_prev_flexoren = 0;
static int stat0_biceps, stat0_triceps, stat0_extensoren, stat0_flexoren;
static int stat1_biceps = 0, stat1_triceps = 0, stat1_extensoren = 0, stat1_flexoren = 0;
- switch (signal_number){
+ switch (signal_number)
+ {
case 1:
//biceps filteren
in4_biceps = in3_biceps; in3_biceps = in2_biceps; in2_biceps = in1_biceps; in1_biceps = in0_biceps;
@@ -86,16 +90,15 @@
out0_biceps = (NUM0*in0_biceps + NUM1*in1_biceps + NUM2*in2_biceps + NUM3*in3_biceps + NUM4*in4_biceps - DEN1*out1_biceps - DEN2*out2_biceps - DEN3*out3_biceps - DEN4*out4_biceps ) / DEN0;
//std deviatie bepalen, om de N metingen
- emg_abs = fabs(out0_biceps);
sum_biceps += out0_biceps;
- square_biceps += (emg_abs - mean_biceps)*(emg_abs - mean_biceps); //neem absolute waarde, kwadrateer, voeg toe aan vorige square
+ square_biceps += (out0_biceps - mean_biceps)*(out0_biceps - mean_biceps); //neem absolute waarde, kwadrateer, voeg toe aan vorige square
// voeg rest EMG's toe, variabelen alleen _spier geven als het nodig is.
count_biceps += 1; // hou bij hoeveel squares er zijn opgeteld
if (count_biceps >= MAXCOUNT)
{ sig_in_biceps = sqrt(square_biceps/count_biceps);
mean_biceps = sum_biceps/count_biceps;
count_biceps = 0; square_biceps = 0; sum_biceps = 0; // en neem de RMS als er genoeg zijn geteld, stuur die door, en reset sqaure en count
- // nieuw:
+
deltaV = sig_in_biceps - sig_prev_biceps;
if (sig_in_biceps <= threshold_biceps) // threshold
{ stat0_biceps = 0;
@@ -103,6 +106,10 @@
sig_out_biceps = sig_prev_biceps + deltaV / inertia;
else sig_out_biceps = 0;
}
+ else if (sig_in_biceps >= cap_biceps) // cap
+ { stat0_biceps = 0;
+ sig_out_biceps = cap_biceps;
+ }
else if ( deltaV >= border_biceps ) // stijging
{ stat0_biceps = 1;
if (stat1_biceps == -1)
@@ -118,10 +125,9 @@
else { stat0_biceps = 0;
sig_out_biceps = sig_in_biceps;
}
- sig_prev_biceps = sig_in_biceps;
+ sig_prev_biceps = sig_out_biceps;
stat1_biceps = stat0_biceps;
sig_out = sig_out_biceps;
- //red = sig_out_biceps;
}
else sig_out = -1;
break;
@@ -130,30 +136,33 @@
in4_triceps = in3_triceps; in3_triceps = in2_triceps; in2_triceps = in1_triceps; in1_triceps = in0_triceps;
in0_triceps = emg_triceps.read();
out4_triceps = out3_triceps; out3_triceps = out2_triceps; out2_triceps = out1_triceps; out1_triceps = out0_triceps;
- out0_biceps = (NUM0*in0_triceps + NUM1*in1_triceps + NUM2*in2_triceps + NUM3*in3_triceps + NUM4*in4_triceps - DEN1*out1_triceps - DEN2*out2_triceps - DEN3*out3_triceps - DEN4*out4_triceps ) / DEN0;
-
+ out0_triceps = (NUM0*in0_triceps + NUM1*in1_triceps + NUM2*in2_triceps + NUM3*in3_triceps + NUM4*in4_triceps - DEN1*out1_triceps - DEN2*out2_triceps - DEN3*out3_triceps - DEN4*out4_triceps ) / DEN0;
//std deviatie bepalen om de N metingen
- emg_abs = fabs(out0_triceps);
sum_triceps += out0_triceps;
- square_triceps += (emg_abs - mean_triceps)*(emg_abs - mean_triceps); //neem absolute waarde, kwadrateer, voeg toe aan vorige square
+ square_triceps += (out0_triceps - mean_triceps)*(out0_triceps - mean_triceps); //neem absolute waarde, kwadrateer, voeg toe aan vorige square
// voeg rest EMG's toe, variabelen alleen _spier geven als het nodig is.
count_triceps += 1; // hou bij hoeveel squares er zijn opgeteld
-
if (count_triceps >= MAXCOUNT)
{ sig_in_triceps = sqrt(square_triceps/count_triceps);
mean_triceps = sum_triceps/count_triceps;
count_triceps = 0; square_triceps = 0; sum_triceps = 0;// en neem de RMS als er genoeg zijn geteld, stuur die door, en reset sqaure en count
deltaV = sig_in_triceps - sig_prev_triceps;
+ if(pc.txBufferGetSize(0)-pc.txBufferGetCount() > 30) // in sommige versies staat rx, dit klopt niet
+ pc.printf("%.6f\n",sig_in_triceps);
if (sig_in_triceps <= threshold_triceps) // threshold
{ stat0_triceps = 0;
if (stat1_triceps == 1)
sig_out_triceps = sig_prev_triceps + deltaV / inertia;
else sig_out_triceps = 0;
}
+ else if (sig_in_triceps >= cap_triceps) // cap
+ { stat0_triceps = 0;
+ sig_out_triceps = cap_triceps;
+ }
else if ( deltaV >= border_triceps ) // stijging
{ stat0_triceps = 1;
if (stat1_triceps == -1)
- sig_out_triceps = sig_prev_triceps + deltaV / inertia;
+ sig_out_triceps = sig_prev_triceps + deltaV / inertia;
else sig_out_triceps = sig_in_triceps;
}
else if ( deltaV <= -border_triceps ) // daling
@@ -165,10 +174,9 @@
else { stat0_triceps = 0;
sig_out_triceps = sig_in_triceps;
}
- sig_prev_triceps = sig_in_triceps;
+ sig_prev_triceps = sig_out_triceps;
stat1_triceps = stat0_triceps;
sig_out = sig_out_triceps;
- //blue = sig_out_triceps;
}
else sig_out = -1;
break;
@@ -180,9 +188,8 @@
out0_flexoren = (NUM0*in0_flexoren + NUM1*in1_flexoren + NUM2*in2_flexoren + NUM3*in3_flexoren + NUM4*in4_flexoren - DEN1*out1_flexoren - DEN2*out2_flexoren - DEN3*out3_flexoren - DEN4*out4_flexoren) / DEN0;
//std deviatie bepalen om de N metingen
- emg_abs = fabs(out0_flexoren);
sum_flexoren += out0_flexoren;
- square_flexoren += (emg_abs - mean_flexoren)*(emg_abs - mean_flexoren); //neem absolute waarde, kwadrateer, voeg toe aan vorige square
+ square_flexoren += (out0_flexoren - mean_flexoren)*(out0_flexoren - mean_flexoren); //neem absolute waarde, kwadrateer, voeg toe aan vorige square
// voeg rest EMG's toe, variabelen alleen _spier geven als het nodig is.
count_flexoren += 1; // hou bij hoeveel squares er zijn opgeteld
if (count_flexoren >= MAXCOUNT)
@@ -202,9 +209,8 @@
out0_extensoren = (NUM0*in0_extensoren + NUM1*in1_extensoren + NUM2*in2_extensoren + NUM3*in3_extensoren + NUM4*in4_extensoren - DEN1*out1_extensoren - DEN2*out2_extensoren - DEN3*out3_extensoren - DEN4*out4_extensoren) / DEN0;
//std deviatie bepalen om de 50 metingen
- emg_abs = fabs(out0_extensoren);
sum_extensoren += out0_extensoren;
- square_extensoren += (emg_abs - mean_extensoren)*(emg_abs - mean_extensoren); //neem absolute waarde, kwadrateer, voeg toe aan vorige square
+ square_extensoren += (out0_extensoren - mean_extensoren)*(out0_extensoren - mean_extensoren); //neem absolute waarde, kwadrateer, voeg toe aan vorige square
// voeg rest EMG's toe, variabelen alleen _spier geven als het nodig is.
count_extensoren += 1; // hou bij hoeveel squares er zijn opgeteld
if (count_extensoren >= MAXCOUNT)
@@ -227,18 +233,30 @@
static float biceps, triceps, extensoren, flexoren, emg_filter_test, dy, dx;
emg_filter_test = filter(1);
- if (emg_filter_test != -1) {
- biceps = emg_filter_test;
- red = biceps;
- }
+ if (emg_filter_test != -1)
+ {biceps = emg_filter_test;
+ }
emg_filter_test = filter(2);
- if (emg_filter_test != -1){
- triceps = emg_filter_test;
- blue = triceps;
- }
- dx=biceps-triceps;
- if(pc.rxBufferGetSize(0)-pc.rxBufferGetCount() > 30)
- pc.printf("%.6f\n",biceps);
+ if (emg_filter_test != -1)
+ {triceps = emg_filter_test;
+ }
+
+ dx = biceps - triceps;
+
+ if (biceps > triceps)
+ {//dx = biceps;
+ red = biceps; blue = 0;
+ }
+ else if (biceps < triceps)
+ {//dx = -triceps;
+ blue = gain_triceps*triceps; red = 0;
+ }
+ else {
+ dx = 0; red = 0; blue = 0;
+ }
+ /*if(pc.txBufferGetSize(0)-pc.txBufferGetCount() > 30) // in sommige versies staat rx, dit klopt niet
+ pc.printf("%.6f\n",biceps); */
+
/*emg_filter_test = filter(3);
if (emg_filter_test != -1) flexoren = emg_filter_test;
emg_filter_test = filter(4);
@@ -251,18 +269,18 @@
/*setup baudrate. Choose the same in your program on PC side*/
pc.baud(115200);
/*set the period for the PWM to the red LED*/
- red.period_ms(MAXCOUNT); // periode pwm = 2*Fs , blijkbaar.
- blue.period_ms(MAXCOUNT);
+ red.period_ms(MAXCOUNT*2); // periode pwm = 2*Fs , blijkbaar.
+ blue.period_ms(MAXCOUNT*2);
/**Here you attach the 'void looper(void)' function to the Ticker object0
* The looper() function will be called every 0.001 seconds.
* Please mind that the parentheses after looper are omitted when using attach.
*/
- timer.attach(looper, 0.002);
+ timer.attach(looper, 0.001); // programma zou sneller lopen als niet alle 4 de vensters tegelijk aflopen. Hiervoor moeten meer timers gebruikt worden.
while(1) // Loop
{
// zie 'encoder' , timer bepaalt wanneer een if in de while-loop berekeningen doet voor de motor.
-
+ // loop is leeg voor enkel meten EMG
}
-}
+}
\ No newline at end of file