Test code to calibrate EMG - MVC measurement
Dependencies: HIDScope MODSERIAL mbed
Fork of emgCalibration by
main.cpp
- Committer:
- Vigilance88
- Date:
- 2015-10-16
- Revision:
- 5:d7ee4a5612af
- Parent:
- 4:329e1022cbd3
File content as of revision 5:d7ee4a5612af:
#include "mbed.h" #include "HIDScope.h" #include "MODSERIAL.h" #define SAMPLING_RATE 0.002 MODSERIAL pc(USBTX,USBRX); AnalogIn emg1(A0); // onderste board AnalogIn emg2(A1); // 2e board AnalogIn emg3(A2); // 3e board AnalogIn emg4(A3); // bovenste board Ticker sample_timer; // naam van de emg-ticker DigitalOut red(LED_RED); DigitalOut green(LED_GREEN); DigitalOut blue(LED_BLUE); int const window=150; //30 samples int i=0; //buffer index double bicepsbuffer [window]; //HIDScope scope(4); // aantal kanalen voor je HIDScope double avg1=0; int muscle; double tijd; double y5; //highpass filter 20 Hz const double numhigh_1 = 0.956543225556877; const double numhigh_2 = -1.91308645113754; const double numhigh_3 = 0.956543225556877; const double denhigh_2 = -1.91197067426073; const double denhigh_3 = 0.9149758348014341; //biquad 1 const double notch1gain = 1.000000; const double notch1_b0 = 1; const double notch1_b1 = -1.61816176147 * notch1gain; const double notch1_b2 = 1.00000000000 * notch1gain; const double notch1_a1 = -1.58071559235 * notch1gain; const double notch1_a2 = 0.97319685401 * notch1gain; //biquad 2 const double notch2gain = 0.973674; const double notch2_b0 = 1 * notch2gain; const double notch2_b1 = -1.61816176147 * notch2gain; const double notch2_b2 = 1.00000000000 * notch2gain; const double notch2_a1 = -1.61244708381 * notch2gain; const double notch2_a2 = 0.97415116257 * notch2gain; //lowpass filter 7 Hz - envelop const double numlow_1 = 0.000119046743110057; const double numlow_2 = 0.000238093486220118; const double numlow_3 = 0.000119046743110057; const double denlow_2 = -1.968902268531908; const double denlow_3 = 0.9693784555043481; //EMG variables double emg_biceps; double biceps_power; double bicepsMVC = 0; double emg_triceps; double triceps_power; double tricepsMVC = 0; double emg_flexor; double flexor_power; double flexorMVC = 0; double emg_extens; double extens_power; double extensMVC = 0; // storage variables definieren double f1_v1 = 0, f1_v2 = 0, f2_v1 = 0, f2_v2 = 0, f3_v1 = 0, f3_v2 = 0, f4_v1 = 0, f4_v2 = 0; double biquad( double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2) { double v = u - a1*v1 - a2*v2; double y = b0*v + b1*v1 + b2*v2; v2=v1; v1=v; return y; } void sample_filter() { // double u1=..., u2...; double u1 = emg1.read(); double y1 = biquad(u1, f1_v1, f1_v2, denhigh_2, denhigh_3, numhigh_1, numhigh_2, numhigh_3); double y2 = biquad(y1, f2_v1, f2_v2, notch1_a1, notch1_a2, notch1_b0, notch1_b1, notch1_b2); double y3 = biquad(y2, f3_v1, f3_v2, notch2_a1, notch2_a2, notch2_b0, notch2_b1, notch2_b2); double y4 = abs(y3); double y5 = biquad(y4, f4_v1, f4_v2, denlow_2, denlow_3, numlow_1, numlow_2, numlow_3); biceps_power=y5; //scope.set(0,u1); //scope.set(1,y4); //scope.set(2,biceps_power); //scope.send(); } //Start sampling void start_sampling(void) { sample_timer.attach(&sample_filter,0.002); //500 Hz EMG blue=0; green=0; pc.printf("||- started sampling -|| \r\n"); } //stop sampling void stop_sampling(void) { sample_timer.detach(); blue=1; green=1; pc.printf("||- stopped sampling -|| \r\n"); } void calibrate_emg() { //double sampletime=0; //sampletime=+SAMPLE_RATE; // // if(sampletime<5) //int muscle=1; //for(int index=0; index<2500;index++){ //measure 5 seconds@500hz = 2500 samples if(muscle==1){ if(biceps_power>bicepsMVC){ printf("+ "); bicepsMVC=biceps_power; } else printf("- "); } if(muscle==2){ if(triceps_power>tricepsMVC){ tricepsMVC=triceps_power; } } if(muscle==3){ if(flexor_power>flexorMVC){ flexorMVC=flexor_power; } } if(muscle==4){ if(extens_power>extensMVC){ extensMVC=extens_power; } } //} tijd = tijd + 0.002; } int main() { pc.baud(115200); red=1; green=1; blue=1; Ticker timer; pc.printf("|-- Robot Started --| \r\n"); pc.printf("Testcode calibration \r\n"); wait(1); pc.printf("+ means current sample is higher than stored MVC\r\n"); pc.printf("- means current sample is lower than stored MVC\r\n"); wait(3); pc.printf(" Biceps is first. "); wait(1); pc.printf(" Press any key to begin... "); wait(1); char input; input=pc.getc(); pc.putc(input); pc.printf(" \r\n Starting in 3... \r\n"); wait(1); pc.printf(" \r\n Starting in 2... \r\n"); wait(1); pc.printf(" \r\n Starting in 1... \r\n"); wait(1); start_sampling(); muscle=1; timer.attach(&calibrate_emg,0.002); wait(5); timer.detach(); pc.printf("\r\n MVC = %f \r\n",bicepsMVC); pc.printf("Calibrate_emg() exited \r\n"); pc.printf("Measured time: %f seconds \r\n",tijd); tijd=0; // Triceps: pc.printf(" Triceps is next "); wait(1); pc.printf(" Press any key to begin... "); wait(1); input=pc.getc(); pc.putc(input); pc.printf(" \r\n Starting in 3... \r\n"); wait(1); pc.printf(" \r\n Starting in 2... \r\n"); wait(1); pc.printf(" \r\n Starting in 1... \r\n"); wait(1); start_sampling(); muscle=1; timer.attach(&calibrate_emg,0.002); wait(5); timer.detach(); pc.printf("\r\n Triceps MVC = %f \r\n",tricepsMVC); pc.printf("Calibrate_emg() exited \r\n"); pc.printf("Measured time: %f seconds \r\n",tijd); tijd=0; //Flexor: //Extensor: stop_sampling(); while (true){ wait(1); } }