De hele robot in 1 keer bam
Dependencies: mbed QEI Servo HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 19:94dc52f8a59e
- Parent:
- 18:9f24792bb39a
- Child:
- 20:0e9218673aa8
diff -r 9f24792bb39a -r 94dc52f8a59e main.cpp --- a/main.cpp Fri Oct 25 07:52:41 2019 +0000 +++ b/main.cpp Fri Oct 25 08:29:50 2019 +0000 @@ -76,18 +76,34 @@ int trim_cal_i = trim_cal * Fs - 1; // Determine iterator of transient behaviour trim // Notch biquad filter coefficients (iirnotch Q factor 35 @50Hz) from MATLAB: -BiQuad bq_notch( 0.995636295063941, -1.89829218816065, 0.995636295063941, 1, -1.89829218816065, 0.991272590127882); // b01 b11 b21 a01 a11 a21 -BiQuadChain bqc_notch; +BiQuad bq1_notch( 0.995636295063941, -1.89829218816065, 0.995636295063941, 1, -1.89829218816065, 0.991272590127882); // b01 b11 b21 a01 a11 a21 +BiQuad bq2_notch = bq1_notch; +BiQuad bq3_notch = bq1_notch; +BiQuadChain bqc1_notch; +BiQuadChain bqc2_notch; +BiQuadChain bqc3_notch; // Highpass biquad filter coefficients (butter 4th order @10Hz cutoff) from MATLAB -BiQuad bq_H1(0.922946103200875, -1.84589220640175, 0.922946103200875, 1, -1.88920703055163, 0.892769008131025); // b01 b11 b21 a01 a11 a21 -BiQuad bq_H2(1, -2, 1, 1, -1.95046575793011, 0.954143234875078); // b02 b12 b22 a02 a12 a22 -BiQuadChain bqc_high; +BiQuad bq1_H1(0.922946103200875, -1.84589220640175, 0.922946103200875, 1, -1.88920703055163, 0.892769008131025); // b01 b11 b21 a01 a11 a21 +BiQuad bq1_H2(1, -2, 1, 1, -1.95046575793011, 0.954143234875078); // b02 b12 b22 a02 a12 a22 +BiQuad bq2_H1 = bq1_H1; +BiQuad bq2_H2 = bq1_H2; +BiQuad bq3_H1 = bq1_H1; +BiQuad bq3_H2 = bq1_H2; +BiQuadChain bqc1_high; +BiQuadChain bqc2_high; +BiQuadChain bqc3_high; // Lowpass biquad filter coefficients (butter 4th order @5Hz cutoff) from MATLAB: -BiQuad bq_L1(5.32116245737504e-08, 1.06423249147501e-07, 5.32116245737504e-08, 1, -1.94396715039462, 0.944882378004138); // b01 b11 b21 a01 a11 a21 -BiQuad bq_L2(1, 2, 1, 1, -1.97586467534468, 0.976794920438162); // b02 b12 b22 a02 a12 a22 -BiQuadChain bqc_low; +BiQuad bq1_L1(5.32116245737504e-08, 1.06423249147501e-07, 5.32116245737504e-08, 1, -1.94396715039462, 0.944882378004138); // b01 b11 b21 a01 a11 a21 +BiQuad bq1_L2(1, 2, 1, 1, -1.97586467534468, 0.976794920438162); // b02 b12 b22 a02 a12 a22 +BiQuad bq2_L1 = bq1_L1; +BiQuad bq2_L2 = bq1_L2; +BiQuad bq3_L1 = bq1_L1; +BiQuad bq3_L2 = bq1_L2; +BiQuadChain bqc1_low; +BiQuadChain bqc2_low; +BiQuadChain bqc3_low; /* ------ HELPER FUNCTIONS ------ @@ -121,9 +137,9 @@ // Check filter stability bool checkBQChainStable() { - bool n_stable = bqc_notch.stable(); - bool hp_stable = bqc_high.stable(); - bool l_stable = bqc_low.stable(); + bool n_stable = bqc1_notch.stable(); + bool hp_stable = bqc1_high.stable(); + bool l_stable = bqc1_low.stable(); if (n_stable && hp_stable && l_stable) { return true; @@ -148,13 +164,13 @@ scope.set(0, emg1 ); // Filter notch and highpass - double emg1_n_hp = bqc_notch_high.step( emg1 ); + double emg1_n_hp = bqc1_notch_high.step( emg1 ); // Rectify double emg1_rectify = fabs( emg1_n_hp ); // Filter lowpass (completes envelope) - double emg1_env = bqc_low.step( emg1_rectify ); + double emg1_env = bqc1_low.step( emg1_rectify ); // Output EMG after filters scope.set(1, emg1_env ); @@ -170,21 +186,21 @@ emg3 = emg3_in.read(); // Output raw EMG input - scope.set(0, emg1 ); - scope.set(1, emg2 ); + //scope.set(0, emg1 ); + // scope.set(1, emg2 ); - double emg1_n = bqc_notch.step( emg1 ); // Filter notch - double emg1_hp = bqc_high.step( emg1_n ); // Filter highpass + double emg1_n = bqc1_notch.step( emg1 ); // Filter notch + double emg1_hp = bqc1_high.step( emg1_n ); // Filter highpass double emg1_rectify = fabs( emg1_hp ); // Rectify - double emg1_env = bqc_low.step( emg1_rectify ); // Filter lowpass (completes envelope) + double emg1_env = bqc1_low.step( emg1_rectify ); // Filter lowpass (completes envelope) - double emg2_n = bqc_notch.step( emg2 ); // Filter notch - double emg2_hp = bqc_high.step( emg2_n ); // Filter highpass + double emg2_n = bqc2_notch.step( emg2 ); // Filter notch + double emg2_hp = bqc2_high.step( emg2_n ); // Filter highpass double emg2_rectify = fabs( emg2_hp ); // Rectify - double emg2_env = bqc_low.step( emg2_rectify ); // Filter lowpass (completes envelope) + double emg2_env = bqc2_low.step( emg2_rectify ); // Filter lowpass (completes envelope) - // scope.set(1, emg1_n); - // scope.set(1, emg1_n); + scope.set(0, emg1_n); + scope.set(1, emg2_n); // scope.set(3, emg1_rectify); // Output EMG after filters @@ -208,6 +224,9 @@ emg1_MVC = getMean(emg1_cal); // Store MVC globally emg1_MVC_stdev = getStdev(emg1_cal, emg1_MVC); // Store MVC stde globally emg1_cal.clear(); // Empty vector to prevent memory overflow + emg2_MVC = getMean(emg2_cal); // Store MVC globally + emg2_MVC_stdev = getStdev(emg2_cal, emg2_MVC); // Store MVC stde globally + emg2_cal.clear(); // Empty vector to prevent memory overflow led_b = 1; // Turn off calibration led } @@ -226,6 +245,9 @@ emg1_rest = getMean(emg1_cal); // Store rest globally emg1_rest_stdev = getStdev(emg1_cal, emg1_rest);// Store rest stdev globally emg1_cal.clear(); // Empty vector to prevent memory overflow + emg2_rest = getMean(emg2_cal); // Store rest globally + emg2_rest_stdev = getStdev(emg2_cal, emg2_rest);// Store rest stdev globally + emg2_cal.clear(); // Empty vector to prevent memory overflow led_b = 1; // Turn off calibration led } @@ -261,9 +283,17 @@ // tickSample.attach(&sample, Ts); // Initialize sample ticker // Create BQ chains to reduce computations - bqc_notch.add( &bq_notch ); - bqc_high.add( &bq_H1 ).add( &bq_H2 ); - bqc_low.add( &bq_L1 ).add( &bq_L2 ); + bqc1_notch.add( &bq1_notch ); + bqc1_high.add( &bq1_H1 ).add( &bq1_H2 ); + bqc1_low.add( &bq1_L1 ).add( &bq1_L2 ); + + bqc2_notch.add( &bq2_notch ); + bqc2_high.add( &bq2_H1 ).add( &bq2_H2 ); + bqc2_low.add( &bq2_L1 ).add( &bq2_L2 ); + + bqc3_notch.add( &bq3_notch ); + bqc3_high.add( &bq3_H1 ).add( &bq3_H2 ); + bqc3_low.add( &bq3_L1 ).add( &bq3_L2 ); led_b = 1; // Turn blue led off at startup led_g = 1; // Turn green led off at startup @@ -285,6 +315,6 @@ // Show that system is running // led_g = !led_g; pc.printf("EMG MVC: %f stdev: %f\r\nEMG Rest: %f stdev: %f\r\n", emg1_MVC, emg1_MVC_stdev, emg1_rest, emg1_rest_stdev); - wait(0.5); + wait(1.0f); } } \ No newline at end of file