De hele robot in 1 keer bam
Dependencies: mbed QEI Servo HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 10:97a79aa10a56
- Parent:
- 8:ea3de43c9e8b
- Child:
- 11:042170a9b93a
--- a/main.cpp Mon Oct 21 09:57:42 2019 +0000 +++ b/main.cpp Mon Oct 21 12:52:47 2019 +0000 @@ -32,6 +32,8 @@ double emg1; double emg2; double emg3; +double emg1_mean; +double emg1_stdev; vector<double> emg1_cal; @@ -48,7 +50,8 @@ // Notch filter coefficients (iirnotch Q factor 35 @50Hz) from MATLAB in the following form: // b01 b11 b21 a01 a11 a21 -BiQuad bq_notch(0.995636295063941, -1.89829218816065, 0.995636295063941, 1, -1.89829218816065, 0.991272590127882); +// BiQuad bq_notch( 0.995636295063941, -1.89829218816065, 0.995636295063941, 1, -1.89829218816065, 0.991272590127882); +BiQuad bq_notch ( -1.1978e-16, 0.9561, 0.9780, -1.1978e-16, 0.9780 ); // Highpass filter coefficients (butter 4th order @10Hz cutoff) from MATLAB in the following form: // b01 b11 b21 a01 a11 a21 @@ -137,9 +140,10 @@ // Output raw EMG input scope.set(0, emg1 ); - - double emg1_n_hp = bqc_notch_high.step( emg1 ); // Filter notch and highpass - double emg1_rectify = fabs( emg1_n_hp ); // Filter lowpass (completes envelope) + + double emg1_n = bqc_notch.step( emg1 ); // Filter notch + double emg1_hp = bqc_high.step( emg1_n ); // Filter highpass + double emg1_rectify = fabs( emg1_n_hp ); // Rectify double emg1_env = bqc_low.step( emg1_rectify ); // Filter lowpass (completes envelope) // Output EMG after filters @@ -152,17 +156,17 @@ void calibrationMVCFinished() { tickSampleCalibration.detach(); - double emg1_mean = getMean(emg1_cal); - double emg1_stdev = getStdev(emg1_cal, emg1_mean); + emg1_mean = getMean(emg1_cal); + emg1_stdev = getStdev(emg1_cal, emg1_mean); + + emg1_cal.clear(); led_b = 1; - - pc.printf("EMG Mean: %f stdev: %f\r\n", emg1_mean, emg1_stdev); } void calibrationMVC() { - timeoutCalibrationMVC.attach( &calibrationMVCFinished, 5.0f); + timeoutCalibrationMVC.attach( &calibrationMVCFinished, 10.0f); tickSampleCalibration.attach( &sampleCalibration, Ts ); led_b = 0; } @@ -177,7 +181,8 @@ // tickSample.attach(&sample, Ts); // Create BQ chains to reduce computations - bqc_notch_high.add( &bq_notch ).add( &bq_H1 ).add( &bq_H2 ); + bqc_notch.add( &bq_notch ); + bqc_high.add( &bq_H1 ).add( &bq_H2 ); bqc_low.add( &bq_L1 ).add( &bq_L2 ); led_b = 1; // Turn led off at startup @@ -196,6 +201,7 @@ // Show that system is running // led_g = !led_g; + pc.printf("EMG Mean: %f stdev: %f\r\n", emg1_mean, emg1_stdev); wait(0.5); } } \ No newline at end of file