De hele robot in 1 keer bam
Dependencies: mbed QEI Servo HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 7:7a088536f1c9
- Parent:
- 6:5437cc97e1e6
- Child:
- 8:ea3de43c9e8b
diff -r 5437cc97e1e6 -r 7a088536f1c9 main.cpp --- a/main.cpp Sun Oct 20 19:53:00 2019 +0000 +++ b/main.cpp Mon Oct 21 09:21:17 2019 +0000 @@ -8,6 +8,7 @@ // #include "Arduino.h" //misschien handig omdat we het EMG arduino board gebruiken (?) // #include "EMGFilters.h" #include <vector> // For easy array management +#include <numeric> // For manipulating array data // PC serial connection HIDScope scope( 2 ); @@ -26,24 +27,19 @@ double emg1; double emg2; double emg3; -double notch1; -double notch2; -double notch3; -double highpass1; -double highpass2; -double highpass3; -double lowpass1; -double lowpass2; -double lowpass3; -double rectify1; -double rectify2; -double rectify3; + +vector<double> emg1_cal; + +int i_cal; // Initialize tickers Ticker tickSample; +Timeout timeoutCalibrationMVC; +Ticker tickSampleCalibration; // Sample rate const double Fs = 500; //Hz +const double Ts = 1/Fs; //sec // Notch filter coefficients (iirnotch Q factor 35 @50Hz) from MATLAB in the following form: // b01 b11 b21 a01 a11 a21 @@ -63,6 +59,11 @@ BiQuad bq_L2(1, 2, 1, 1, -1.97586467534468, 0.976794920438162); BiQuadChain bqc_low; // Used to chain two 2nd other filters into a 4th order filter +double stdev(const vector<double> &vect) +{ + +} + // Check if filters are stable bool checkBQChainStable() { @@ -100,12 +101,48 @@ // Output EMG after filters scope.set(1, emg1_env ); scope.send(); + + } +void sampleCalibration() +{ + // Read EMG inputs + emg1 = emg1_in.read(); + emg2 = emg2_in.read(); + emg3 = emg3_in.read(); + + // 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_env = bqc_low.step( emg1_rectify ); // Filter lowpass (completes envelope) + + // Output EMG after filters + scope.set(1, emg1_env ); + scope.send(); + + emg1_cal.push_back(emg1_env); +} + +void calibrationMVCFinished() +{ + tickSampleCalibration.detach(); + +} + +void calibrationMVC() +{ + timeoutCalibrationMVC.attach( &calibrationMVCFinished, 10.0f); + tickSampleCalibration.attach( &sampleCalibration, Ts ); +} + + + void main() { // Initialize sample ticker - const double Ts = 1/Fs; tickSample.attach(&sample, Ts); // Create BQ chains to reduce computations @@ -120,7 +157,7 @@ } while(true) { - + // Show that system is running led_g = !led_g; wait(0.5);