M9 Biorobotics Group 8 / Mbed 2 deprecated CARD_program

De hele robot in 1 keer bam

Dependencies:   mbed QEI Servo HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@4:09a01d2db8f7, 2019-10-20 (annotated)

Committer:
Jellehierck
Date:
Sun Oct 20 19:25:11 2019 +0000
Revision:
4:09a01d2db8f7
Parent:
3:c0ece64850db
Child:
5:3d65f89e3755
Filters rewrite complete. Raw EMG output can be measured now.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IsaRobin 0:6972d0e91af1 1 //c++ script for filtering of measured EMG signals
IsaRobin 0:6972d0e91af1 2 #include "mbed.h" //Base library
IsaRobin 0:6972d0e91af1 3 #include "HIDScope.h" // to see if program is working and EMG is filtered properly
Jellehierck 2:d3e9788ab1b3 4 // #include "QEI.h"// is needed for the encoder
Jellehierck 2:d3e9788ab1b3 5 // #include "MODSERIAL.h"// in order for connection with the pc
Jellehierck 2:d3e9788ab1b3 6 #include "BiQuad.h"
Jellehierck 2:d3e9788ab1b3 7 // #include "FastPWM.h"
Jellehierck 2:d3e9788ab1b3 8 // #include "Arduino.h" //misschien handig omdat we het EMG arduino board gebruiken (?)
Jellehierck 2:d3e9788ab1b3 9 // #include "EMGFilters.h"
IsaRobin 0:6972d0e91af1 10 #include <vector> // For easy array management
IsaRobin 0:6972d0e91af1 11
Jellehierck 2:d3e9788ab1b3 12 // PC serial connection
Jellehierck 4:09a01d2db8f7 13 HIDScope scope( 2 );
Jellehierck 2:d3e9788ab1b3 14 // MODSERIAL pc(USBTX, USBRX);
IsaRobin 0:6972d0e91af1 15
IsaRobin 0:6972d0e91af1 16 //EMG inputs definieren
IsaRobin 0:6972d0e91af1 17 AnalogIn emg1_in (A1); //emg van rechterbicep, voor de x-richting
IsaRobin 0:6972d0e91af1 18 AnalogIn emg2_in (A2); //emg van linkerbicep, voor de y-richting
IsaRobin 0:6972d0e91af1 19 AnalogIn emg3_in (A3); //emg van een derde (nog te bepalen) spier, voor het vernaderen van de richting
IsaRobin 0:6972d0e91af1 20
Jellehierck 4:09a01d2db8f7 21 // LED
Jellehierck 4:09a01d2db8f7 22 DigitalOut led(LED_GREEN);
Jellehierck 4:09a01d2db8f7 23
IsaRobin 0:6972d0e91af1 24 //variablen voor EMG
IsaRobin 0:6972d0e91af1 25 double emg1;
IsaRobin 0:6972d0e91af1 26 double emg2;
IsaRobin 0:6972d0e91af1 27 double emg3;
IsaRobin 0:6972d0e91af1 28 double notch1;
IsaRobin 0:6972d0e91af1 29 double notch2;
IsaRobin 0:6972d0e91af1 30 double notch3;
IsaRobin 0:6972d0e91af1 31 double highpass1;
IsaRobin 0:6972d0e91af1 32 double highpass2;
IsaRobin 0:6972d0e91af1 33 double highpass3;
IsaRobin 0:6972d0e91af1 34 double lowpass1;
IsaRobin 0:6972d0e91af1 35 double lowpass2;
IsaRobin 0:6972d0e91af1 36 double lowpass3;
IsaRobin 0:6972d0e91af1 37 double rectify1;
IsaRobin 0:6972d0e91af1 38 double rectify2;
IsaRobin 0:6972d0e91af1 39 double rectify3;
IsaRobin 0:6972d0e91af1 40
Jellehierck 4:09a01d2db8f7 41 // Initialize tickers
Jellehierck 4:09a01d2db8f7 42 Ticker tickSample;
Jellehierck 4:09a01d2db8f7 43
Jellehierck 4:09a01d2db8f7 44 // Sample rate
Jellehierck 4:09a01d2db8f7 45 const double Fs = 500; //Hz
Jellehierck 4:09a01d2db8f7 46
Jellehierck 3:c0ece64850db 47 // Notch filter coefficients (iirnotch Q factor 35 @50Hz) from MATLAB in the following form:
Jellehierck 3:c0ece64850db 48 // b01 b11 b21 a01 a11 a21
Jellehierck 3:c0ece64850db 49 BiQuad bq_notch(0.995636295063941, -1.89829218816065, 0.995636295063941, 1, -1.89829218816065, 0.991272590127882);
Jellehierck 1:059cca298369 50
Jellehierck 3:c0ece64850db 51 // Highpass filter coefficients (butter 4th order @10Hz cutoff) from MATLAB in the following form:
Jellehierck 3:c0ece64850db 52 // b01 b11 b21 a01 a11 a21
Jellehierck 3:c0ece64850db 53 // b02 b12 b22 a02 a12 a22
Jellehierck 3:c0ece64850db 54 BiQuad bq_H1(0.922946103200875, -1.84589220640175, 0.922946103200875, 1, -1.88920703055163, 0.892769008131025);
Jellehierck 3:c0ece64850db 55 BiQuad bq_H2(1, -2, 1, 1, -1.95046575793011, 0.954143234875078);
Jellehierck 4:09a01d2db8f7 56 BiQuadChain bqc_notch_high; // Used to chain two 2nd other filters into a 4th order filter
IsaRobin 0:6972d0e91af1 57
Jellehierck 3:c0ece64850db 58 // Lowpass filter coefficients (butter 4th order @5Hz cutoff) from MATLAB in the following form:
Jellehierck 3:c0ece64850db 59 // b01 b11 b21 a01 a11 a21
Jellehierck 3:c0ece64850db 60 // b02 b12 b22 a02 a12 a22
Jellehierck 3:c0ece64850db 61 BiQuad bq_L1(5.32116245737504e-08, 1.06423249147501e-07, 5.32116245737504e-08, 1, -1.94396715039462, 0.944882378004138);
Jellehierck 3:c0ece64850db 62 BiQuad bq_L2(1, 2, 1, 1, -1.97586467534468, 0.976794920438162);
Jellehierck 3:c0ece64850db 63 BiQuadChain bqc_low; // Used to chain two 2nd other filters into a 4th order filter
Jellehierck 2:d3e9788ab1b3 64
Jellehierck 2:d3e9788ab1b3 65 void sample()
Jellehierck 2:d3e9788ab1b3 66 {
Jellehierck 4:09a01d2db8f7 67 // Read EMG inputs
Jellehierck 2:d3e9788ab1b3 68 emg1 = emg1_in.read();
Jellehierck 2:d3e9788ab1b3 69 emg2 = emg2_in.read();
Jellehierck 2:d3e9788ab1b3 70 emg3 = emg3_in.read();
Jellehierck 4:09a01d2db8f7 71
Jellehierck 4:09a01d2db8f7 72 // Output raw EMG input
Jellehierck 4:09a01d2db8f7 73 scope.set(0, emg1 );
Jellehierck 4:09a01d2db8f7 74
Jellehierck 4:09a01d2db8f7 75 // Output EMG after filters
Jellehierck 4:09a01d2db8f7 76 scope.set(1, emg1 );
Jellehierck 4:09a01d2db8f7 77 scope.send();
Jellehierck 2:d3e9788ab1b3 78 }
IsaRobin 0:6972d0e91af1 79
Jellehierck 4:09a01d2db8f7 80 /*
IsaRobin 0:6972d0e91af1 81 //notch filter toepassen
IsaRobin 0:6972d0e91af1 82 notch1 = N1.step(emg1);
IsaRobin 0:6972d0e91af1 83 notch2 = N2.step(emg2);
IsaRobin 0:6972d0e91af1 84 notch3 = N3.step(emg3);
IsaRobin 0:6972d0e91af1 85
Jellehierck 2:d3e9788ab1b3 86 //high pass filter
IsaRobin 0:6972d0e91af1 87 high1 = H1.step(notch1);
IsaRobin 0:6972d0e91af1 88 high2 = H2.step(notch2);
IsaRobin 0:6972d0e91af1 89 high3 = H3.step(notch3);
IsaRobin 0:6972d0e91af1 90
IsaRobin 0:6972d0e91af1 91 //rectify toepassen, oftewel absolute waarde van EMG signaal nemen
IsaRobin 0:6972d0e91af1 92 absolute1 = fabs(high1);
IsaRobin 0:6972d0e91af1 93 absolute2 = fabs(high2);
IsaRobin 0:6972d0e91af1 94 absolute3 = fabs(high3);
IsaRobin 0:6972d0e91af1 95
IsaRobin 0:6972d0e91af1 96 //low pass filter
IsaRobin 0:6972d0e91af1 97 low1 = L1.step(absolute1);
IsaRobin 0:6972d0e91af1 98 low2 = L2.step(absolute2);
IsaRobin 0:6972d0e91af1 99 low3 = L3.step(absolute3);
Jellehierck 4:09a01d2db8f7 100 */
Jellehierck 4:09a01d2db8f7 101
Jellehierck 4:09a01d2db8f7 102 int main()
Jellehierck 4:09a01d2db8f7 103 {
Jellehierck 4:09a01d2db8f7 104 const double Ts = 1/Fs;
Jellehierck 4:09a01d2db8f7 105 tickSample.attach(&sample, Ts);
Jellehierck 4:09a01d2db8f7 106
Jellehierck 4:09a01d2db8f7 107 while(true) {
Jellehierck 4:09a01d2db8f7 108 led = !led;
Jellehierck 4:09a01d2db8f7 109 wait(0.5);
Jellehierck 4:09a01d2db8f7 110 }
Jellehierck 4:09a01d2db8f7 111 return 0;
Jellehierck 4:09a01d2db8f7 112 }