De hele robot in 1 keer bam
Dependencies: mbed QEI Servo HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 11:042170a9b93a
- Parent:
- 10:97a79aa10a56
- Child:
- 12:70f0710400c2
--- a/main.cpp Mon Oct 21 12:52:47 2019 +0000 +++ b/main.cpp Mon Oct 21 14:07:30 2019 +0000 @@ -11,7 +11,7 @@ #include <numeric> // For manipulating array data // PC serial connection -HIDScope scope( 2 ); +HIDScope scope( 3 ); MODSERIAL pc(USBTX, USBRX); //EMG inputs definieren @@ -37,7 +37,6 @@ vector<double> emg1_cal; -int i_cal; // Initialize tickers Ticker tickSample; @@ -45,20 +44,25 @@ Ticker tickSampleCalibration; // Sample rate -const double Fs = 500; //Hz -const double Ts = 1/Fs; //sec +const double Fs = 500; // Sampling frequency (s) +const double Ts = 1/Fs; // Sampling time (s) + +const double Tcal = 10.0f; // Calibration duration (s) + +int trim_cal = 1; // Trim the beginning of the calibration vector to reduce transient behaviour by X seconds +int trim_cal_i = trim_cal * Fs - 1; // 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 ( -1.1978e-16, 0.9561, 0.9780, -1.1978e-16, 0.9780 ); +BiQuad bq_notch( 0.995636295063941, -1.89829218816065, 0.995636295063941, 1, -1.89829218816065, 0.991272590127882); +BiQuadChain bqc_notch; // Highpass filter coefficients (butter 4th order @10Hz cutoff) from MATLAB in the following form: // b01 b11 b21 a01 a11 a21 // b02 b12 b22 a02 a12 a22 BiQuad bq_H1(0.922946103200875, -1.84589220640175, 0.922946103200875, 1, -1.88920703055163, 0.892769008131025); BiQuad bq_H2(1, -2, 1, 1, -1.95046575793011, 0.954143234875078); -BiQuadChain bqc_notch_high; // Used to chain two 2nd other filters into a 4th order filter +BiQuadChain bqc_high; // Used to chain two 2nd other filters into a 4th order filter // Lowpass filter coefficients (butter 4th order @5Hz cutoff) from MATLAB in the following form: // b01 b11 b21 a01 a11 a21 @@ -93,10 +97,11 @@ // Check if filters are stable bool checkBQChainStable() { - bool n_hp_stable = bqc_notch_high.stable(); + bool n_stable = bqc_notch.stable(); + bool hp_stable = bqc_high.stable(); bool l_stable = bqc_low.stable(); - if (n_hp_stable && l_stable) { + if (n_stable && hp_stable && l_stable) { return true; } else { return false; @@ -104,6 +109,7 @@ } +/* // Read samples, filter samples and output to HIDScope void sample() { @@ -127,9 +133,8 @@ // Output EMG after filters scope.set(1, emg1_env ); scope.send(); - - } +*/ void sampleCalibration() { @@ -142,12 +147,13 @@ scope.set(0, emg1 ); double emg1_n = bqc_notch.step( emg1 ); // Filter notch + scope.set(1, emg1_n); double emg1_hp = bqc_high.step( emg1_n ); // Filter highpass - double emg1_rectify = fabs( emg1_n_hp ); // Rectify + double emg1_rectify = fabs( emg1_hp ); // Rectify double emg1_env = bqc_low.step( emg1_rectify ); // Filter lowpass (completes envelope) // Output EMG after filters - scope.set(1, emg1_env ); + scope.set(2, emg1_env ); scope.send(); emg1_cal.push_back(emg1_env); @@ -156,6 +162,7 @@ void calibrationMVCFinished() { tickSampleCalibration.detach(); + emg1_mean = getMean(emg1_cal); emg1_stdev = getStdev(emg1_cal, emg1_mean); @@ -166,7 +173,7 @@ void calibrationMVC() { - timeoutCalibrationMVC.attach( &calibrationMVCFinished, 10.0f); + timeoutCalibrationMVC.attach( &calibrationMVCFinished, Tcal); tickSampleCalibration.attach( &sampleCalibration, Ts ); led_b = 0; }