Test code to calibrate EMG - MVC measurement
Dependencies: HIDScope MODSERIAL mbed
Fork of emgCalibration by
main.cpp
- Committer:
- Vigilance88
- Date:
- 2015-10-15
- Revision:
- 1:93d79d6e96bc
- Parent:
- 0:d8312eea7c50
- Child:
- 2:174b10062c61
File content as of revision 1:93d79d6e96bc:
#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 double tijd; double y5; Ticker sample_timer; // naam van de emg-ticker 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; //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; volatile bool klaar; // 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 printf("started sampling \r\n"); } //stop sampling void stop_sampling(void) { sample_timer.detach(); printf("stopped sampling \r\n"); } void calibrate_emg(int muscle) { //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); Ticker timer; printf("Robot Started \r\n"); printf("Testcode calibration \r\n"); wait(1); printf("+ means current sample is higher than stored MVC\r\n"); printf("- means current sample is lower than stored MVC\r\n"); wait(3); printf(" \r\n Starting in 5... \r\n"); wait(1); printf(" \r\n Starting in 4... \r\n"); wait(1); printf(" \r\n Starting in 3... \r\n"); wait(1); printf(" \r\n Starting in 2... \r\n"); wait(1); printf(" \r\n Starting in 1... \r\n"); wait(1); start_sampling(); timer.attach(&calibrate_emg,0.002); wait(5); timer.detach(); printf("\r\n MVC = %f \r\n",bicepsMVC); printf("Calibrate_emg() exited \r\n"); printf("Measured time: %f seconds \r\n",tijd); while (true){ wait(1); } }