De hele robot in 1 keer bam
Dependencies: mbed QEI Servo HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 4:09a01d2db8f7
- Parent:
- 3:c0ece64850db
- Child:
- 5:3d65f89e3755
--- a/main.cpp Sun Oct 20 19:01:21 2019 +0000 +++ b/main.cpp Sun Oct 20 19:25:11 2019 +0000 @@ -10,6 +10,7 @@ #include <vector> // For easy array management // PC serial connection +HIDScope scope( 2 ); // MODSERIAL pc(USBTX, USBRX); //EMG inputs definieren @@ -17,6 +18,9 @@ AnalogIn emg2_in (A2); //emg van linkerbicep, voor de y-richting AnalogIn emg3_in (A3); //emg van een derde (nog te bepalen) spier, voor het vernaderen van de richting +// LED +DigitalOut led(LED_GREEN); + //variablen voor EMG double emg1; double emg2; @@ -34,6 +38,12 @@ double rectify2; double rectify3; +// Initialize tickers +Ticker tickSample; + +// Sample rate +const double Fs = 500; //Hz + // 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); @@ -43,7 +53,7 @@ // 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_high; // Used to chain two 2nd other filters into a 4th order filter +BiQuadChain bqc_notch_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 @@ -54,11 +64,20 @@ void sample() { + // Read EMG inputs emg1 = emg1_in.read(); emg2 = emg2_in.read(); emg3 = emg3_in.read(); + + // Output raw EMG input + scope.set(0, emg1 ); + + // Output EMG after filters + scope.set(1, emg1 ); + scope.send(); } +/* //notch filter toepassen notch1 = N1.step(emg1); notch2 = N2.step(emg2); @@ -78,3 +97,16 @@ low1 = L1.step(absolute1); low2 = L2.step(absolute2); low3 = L3.step(absolute3); +*/ + +int main() +{ + const double Ts = 1/Fs; + tickSample.attach(&sample, Ts); + + while(true) { + led = !led; + wait(0.5); + } + return 0; +} \ No newline at end of file