De hele robot in 1 keer bam
Dependencies: mbed QEI Servo HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 20:0e9218673aa8
- Parent:
- 19:94dc52f8a59e
- Child:
- 21:e4569b47945e
--- a/main.cpp Fri Oct 25 08:29:50 2019 +0000 +++ b/main.cpp Fri Oct 25 09:15:22 2019 +0000 @@ -90,7 +90,7 @@ BiQuad bq2_H2 = bq1_H2; BiQuad bq3_H1 = bq1_H1; BiQuad bq3_H2 = bq1_H2; -BiQuadChain bqc1_high; +BiQuadChain bqc1_high; BiQuadChain bqc2_high; BiQuadChain bqc3_high; @@ -188,22 +188,20 @@ // Output raw EMG input //scope.set(0, emg1 ); // scope.set(1, emg2 ); - + 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_rectify = fabs( emg1_hp ); // Rectify double emg1_env = bqc1_low.step( emg1_rectify ); // Filter lowpass (completes envelope) - + 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_rectify = fabs( emg2_hp ); // Rectify double emg2_env = bqc2_low.step( emg2_rectify ); // Filter lowpass (completes envelope) - + scope.set(0, emg1_n); scope.set(1, emg2_n); - // scope.set(3, emg1_rectify); - // Output EMG after filters scope.set(2, emg1_env ); scope.set(3, emg2_env ); scope.send(); @@ -224,9 +222,14 @@ 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 + emg1_cal.shrink_to_fit(); // 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 + emg2_cal.shrink_to_fit(); // Empty vector to prevent memory overflow + led_b = 1; // Turn off calibration led } @@ -245,9 +248,13 @@ 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 + emg1_cal.shrink_to_fit(); // 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 + emg2_cal.shrink_to_fit(); // Empty vector to prevent memory overflow led_b = 1; // Turn off calibration led } @@ -265,8 +272,8 @@ double margin_percentage = 10; // Set up % margin for rest double factor1 = 1 / emg1_MVC; // Factor to normalize MVC double emg1_th = emg1_rest * factor1 + margin_percentage/100; // Set normalized rest threshold - - pc.printf("Factor: %f TH: %f\r\n", factor1, emg1_th); + + pc.printf("Factor: %f TH: %f\r\n", factor1, emg1_th); } /* @@ -279,18 +286,18 @@ { pc.baud(115200); // MODSERIAL rate pc.printf("Starting\r\n"); - + // tickSample.attach(&sample, Ts); // Initialize sample ticker // Create BQ chains to reduce computations 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 ); @@ -314,7 +321,7 @@ // 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); + pc.printf("Vector emg1_cal: %f vector emg2_cal: %f\r\n", emg1_cal[0], emg2_cal[0]); wait(1.0f); } } \ No newline at end of file