Dan August / Mbed 2 deprecated EMGtest

Meting EMG met alle filters voor biceps.

Dependencies:   MODSERIAL mbed

main.cpp@1:04e5611501f6, 2013-10-25 (annotated)

Committer:
DanAuhust
Date:
Fri Oct 25 07:25:14 2013 +0000
Revision:
1:04e5611501f6
Parent:
0:49ab9907d70d 
Alle filters voor biceps

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DanAuhust 0:49ab9907d70d 1 #include "mbed.h"
DanAuhust 0:49ab9907d70d 2 #include "MODSERIAL.h"
DanAuhust 0:49ab9907d70d 3
DanAuhust 0:49ab9907d70d 4 //Define objects
DanAuhust 0:49ab9907d70d 5 AnalogIn emg_biceps(PTB0); //Analog input
DanAuhust 0:49ab9907d70d 6 PwmOut red(LED_RED); // EMG meting
DanAuhust 0:49ab9907d70d 7 // PwmOut blue(LED_BLUE); // uitgangssignaal controle
DanAuhust 0:49ab9907d70d 8 // PwmOut green(LED_GREEN);
DanAuhust 0:49ab9907d70d 9
DanAuhust 0:49ab9907d70d 10 Ticker timer;
DanAuhust 0:49ab9907d70d 11 MODSERIAL pc(USBTX,USBRX,64,1024);
DanAuhust 0:49ab9907d70d 12
DanAuhust 1:04e5611501f6 13 #define gain_biceps 10 // nog niet gebruikt.
DanAuhust 1:04e5611501f6 14 #define threshold_biceps 0.04
DanAuhust 1:04e5611501f6 15 #define border_biceps 0.1125
DanAuhust 1:04e5611501f6 16 #define maxcount 40 // kies niet te groot, anders werkt rem niet snel genoeg. het kost maxcount*2/1000 seconde om van richting te veranderen.
DanAuhust 1:04e5611501f6 17 #define inertia 4
DanAuhust 0:49ab9907d70d 18
DanAuhust 0:49ab9907d70d 19 #define NUM0 0.8841 // constante
DanAuhust 0:49ab9907d70d 20 #define NUM1 -3.53647 // z^-1
DanAuhust 0:49ab9907d70d 21 #define NUM2 5.3046 // z^-2etc.
DanAuhust 0:49ab9907d70d 22 #define NUM3 -3.5364
DanAuhust 0:49ab9907d70d 23 #define NUM4 0.8841
DanAuhust 0:49ab9907d70d 24
DanAuhust 0:49ab9907d70d 25 #define DEN0 1 // constante
DanAuhust 0:49ab9907d70d 26 #define DEN1 -3.7538
DanAuhust 0:49ab9907d70d 27 #define DEN2 5.2912
DanAuhust 0:49ab9907d70d 28 #define DEN3 -3.3189
DanAuhust 0:49ab9907d70d 29 #define DEN4 0.7816
DanAuhust 0:49ab9907d70d 30
DanAuhust 0:49ab9907d70d 31 /* hou in de gaten welke waarden globaal gedefinieerd moeten worden*/
DanAuhust 1:04e5611501f6 32 float count = 0, square_biceps = 0, sum_biceps = 0, mean_biceps = 0;
DanAuhust 1:04e5611501f6 33 // mean_biceps wel of niet constant nemen?
DanAuhust 0:49ab9907d70d 34
DanAuhust 0:49ab9907d70d 35 void looper()
DanAuhust 0:49ab9907d70d 36 {
DanAuhust 0:49ab9907d70d 37 /*value0 is huidig, 1 is t-1, 2 is t-2 etc. Gebruik later aanduidingen ABCD. */
DanAuhust 0:49ab9907d70d 38 float mean;
DanAuhust 0:49ab9907d70d 39 static float in0 = 0, in1 = 0, in2 = 0, in3 = 0, in4 = 0;
DanAuhust 0:49ab9907d70d 40 static float out0 = 0, out1 = 0, out2 = 0, out3 = 0, out4 = 0;
DanAuhust 0:49ab9907d70d 41
DanAuhust 1:04e5611501f6 42 in4 = in3; in3 = in2; in2 = in1; in1 = in0;
DanAuhust 0:49ab9907d70d 43 in0 = emg_biceps.read();
DanAuhust 0:49ab9907d70d 44 red = in0;
DanAuhust 0:49ab9907d70d 45 /* rode led voor meting emg*/
DanAuhust 0:49ab9907d70d 46 out4 = out3; out3 = out2; out2 = out1; out1 = out0;
DanAuhust 0:49ab9907d70d 47 out0 = (NUM0*in0 + NUM1*in1 + NUM2*in2 + NUM3*in3 + NUM4*in4 - DEN1*out1 - DEN2*out2 - DEN3*out3 - DEN4*out4 ) / DEN0;
DanAuhust 0:49ab9907d70d 48
DanAuhust 0:49ab9907d70d 49 /*send value to PC. use 6 digits after decimal sign*/
DanAuhust 0:49ab9907d70d 50 //if(pc.rxBufferGetSize(0)-pc.rxBufferGetCount() > 30) // ! Testen filter: gebruik de if om de serial optimaal te gebruiken.
DanAuhust 0:49ab9907d70d 51 //pc.printf("%.6f\n",emg_out_biceps);
DanAuhust 0:49ab9907d70d 52 /**When not using the LED, the above could also have been done this way:
DanAuhust 0:49ab9907d70d 53 * pc.printf("%.6\n", emg0.read());
DanAuhust 0:49ab9907d70d 54 */
DanAuhust 1:04e5611501f6 55
DanAuhust 0:49ab9907d70d 56 mean = mean_biceps;
DanAuhust 0:49ab9907d70d 57 sum_biceps += out0;
DanAuhust 1:04e5611501f6 58 square_biceps += (out0 - mean)*(out0 - mean); // WAS EERST (out0 - mean)^2 neem absolute waarde, kwadrateer, voeg toe aan vorige square
DanAuhust 0:49ab9907d70d 59 // voeg rest EMG's toe, variabelen alleen _spier geven als het nodig is.
DanAuhust 0:49ab9907d70d 60 count += 1; // hou bij hoeveel squares er zijn opgeteld
DanAuhust 0:49ab9907d70d 61 }
DanAuhust 0:49ab9907d70d 62
DanAuhust 0:49ab9907d70d 63 int main()
DanAuhust 0:49ab9907d70d 64 {
DanAuhust 0:49ab9907d70d 65 /*setup baudrate. Choose the same in your program on PC side*/
DanAuhust 0:49ab9907d70d 66 pc.baud(115200);
DanAuhust 0:49ab9907d70d 67 /*set the period for the PWM to the red LED*/
DanAuhust 0:49ab9907d70d 68 red.period_ms(2); // periode pwm = 2*Fs , blijkbaar.
DanAuhust 0:49ab9907d70d 69 // blue.period_ms(2);
DanAuhust 0:49ab9907d70d 70 /**Here you attach the 'void looper(void)' function to the Ticker object0
DanAuhust 0:49ab9907d70d 71 * The looper() function will be called every 0.001 seconds.
DanAuhust 0:49ab9907d70d 72 * Please mind that the parentheses after looper are omitted when using attach.
DanAuhust 0:49ab9907d70d 73 */
DanAuhust 1:04e5611501f6 74 float sig_in_biceps; float sig_out_biceps;
DanAuhust 1:04e5611501f6 75 static float sig_prev_biceps = 0;
DanAuhust 1:04e5611501f6 76 float dV_biceps;
DanAuhust 1:04e5611501f6 77 /* rem: eerst threshold na STD, threshold van .03 lijkt prima in testmetingen (voor biceps).
DanAuhust 1:04e5611501f6 78 Als onder threshold, output is nul, maar waarde wel onthouden, negeer rem verder. -> else if structuur
DanAuhust 1:04e5611501f6 79 als terug in threshold terwijl stat1 = 1, dan inertia, om trilling bij verlaten threshold tegen te gaan.
DanAuhust 1:04e5611501f6 80 status is -1, 0 of 1.
DanAuhust 1:04e5611501f6 81 biceps: threshold 0.12 , max is 0.57 , speling = .57-.12 = 4.5 , 25% van 4,5 = 1.125
DanAuhust 1:04e5611501f6 82 sigin < threshold -> stat0 = 0 , stat1 == 1 ? JA dan remmen, anders trillend rond threshold, NEE dan sigout = 0
DanAuhust 1:04e5611501f6 83 dV > 1.125 -> stat0 = 1 , stat1 == -1 ? JA dan remmen, NEE dan sigout=sigin
DanAuhust 1:04e5611501f6 84 dV < -1.125 -> stat0 = -1 , stat1 == 1 ? Ja dan remen, NEE dan sigout=sigin
DanAuhust 1:04e5611501f6 85 binnen grenzen: status0 = 0, voorlopig geen verdere vragen. */
DanAuhust 1:04e5611501f6 86 int stat0_biceps; // huidige status
DanAuhust 1:04e5611501f6 87 int stat1_biceps = 0; // vorige status
DanAuhust 0:49ab9907d70d 88 timer.attach(looper, 0.001);
DanAuhust 0:49ab9907d70d 89 while(1) // Loop
DanAuhust 0:49ab9907d70d 90 { if (count >= maxcount)
DanAuhust 1:04e5611501f6 91 { sig_in_biceps = sqrt(square_biceps/count);
DanAuhust 1:04e5611501f6 92 dV_biceps = sig_in_biceps - sig_prev_biceps;
DanAuhust 1:04e5611501f6 93 mean_biceps = sum_biceps/count;
DanAuhust 1:04e5611501f6 94 count= 0; square_biceps = 0; sum_biceps = 0; // en neem de STD als er genoeg zijn geteld, stuur die door, en reset sqaure en count
DanAuhust 1:04e5611501f6 95 if (sig_in_biceps <= threshold_biceps) // threshold
DanAuhust 1:04e5611501f6 96 { stat0_biceps = 0;
DanAuhust 1:04e5611501f6 97 if (stat1_biceps == 1)
DanAuhust 1:04e5611501f6 98 sig_out_biceps = sig_prev_biceps + dV_biceps / inertia;
DanAuhust 1:04e5611501f6 99 else sig_out_biceps = 0;
DanAuhust 1:04e5611501f6 100 }
DanAuhust 1:04e5611501f6 101 else if ( dV_biceps >= border_biceps ) // stijging
DanAuhust 1:04e5611501f6 102 { stat0_biceps = 1;
DanAuhust 1:04e5611501f6 103 if (stat1_biceps == -1)
DanAuhust 1:04e5611501f6 104 sig_out_biceps = sig_prev_biceps + dV_biceps / inertia;
DanAuhust 1:04e5611501f6 105 else sig_out_biceps = sig_in_biceps;
DanAuhust 1:04e5611501f6 106 }
DanAuhust 1:04e5611501f6 107 else if ( dV_biceps <= -border_biceps ) // daling
DanAuhust 1:04e5611501f6 108 { stat0_biceps = -1;
DanAuhust 1:04e5611501f6 109 if (stat1_biceps == 1)
DanAuhust 1:04e5611501f6 110 sig_out_biceps = sig_prev_biceps + dV_biceps / inertia;
DanAuhust 1:04e5611501f6 111 else sig_out_biceps = sig_in_biceps;
DanAuhust 1:04e5611501f6 112 }
DanAuhust 1:04e5611501f6 113 else { stat0_biceps = 0;
DanAuhust 1:04e5611501f6 114 sig_out_biceps = sig_in_biceps;
DanAuhust 1:04e5611501f6 115 }
DanAuhust 1:04e5611501f6 116 sig_prev_biceps = sig_in_biceps;
DanAuhust 1:04e5611501f6 117 stat1_biceps = stat0_biceps;
DanAuhust 0:49ab9907d70d 118 if(pc.rxBufferGetSize(0)-pc.rxBufferGetCount() > 30)
DanAuhust 1:04e5611501f6 119 pc.printf("%.6f\n",sig_in_biceps); // verwissel in en out om verschillende delen te testen.
DanAuhust 0:49ab9907d70d 120 }
DanAuhust 0:49ab9907d70d 121
DanAuhust 0:49ab9907d70d 122 }
DanAuhust 0:49ab9907d70d 123 }