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@0:6972d0e91af1, 2019-10-20 (annotated)

Committer:
IsaRobin
Date:
Sun Oct 20 18:06:57 2019 +0000
Revision:
0:6972d0e91af1
Child:
1:059cca298369
filteren van EMG signaal

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
IsaRobin 0:6972d0e91af1 4 #include "QEI.h"// is needed for the encoder
IsaRobin 0:6972d0e91af1 5 #include "MODSERIAL.h"// in order for connection with the pc
IsaRobin 0:6972d0e91af1 6 #include "BiQuad.h"
IsaRobin 0:6972d0e91af1 7 #include "FastPWM.h"
IsaRobin 0:6972d0e91af1 8 #include "Arduino.h" //misschien handig omdat we het EMG arduino board gebruiken (?)
IsaRobin 0:6972d0e91af1 9 #include "EMGFilters.h"
IsaRobin 0:6972d0e91af1 10 #include <vector> // For easy array management
IsaRobin 0:6972d0e91af1 11
IsaRobin 0:6972d0e91af1 12 // PC connection
IsaRobin 0:6972d0e91af1 13 MODSERIAL pc(USBTX, USBRX);
IsaRobin 0:6972d0e91af1 14
IsaRobin 0:6972d0e91af1 15 //EMG inputs definieren
IsaRobin 0:6972d0e91af1 16 AnalogIn emg1_in (A1); //emg van rechterbicep, voor de x-richting
IsaRobin 0:6972d0e91af1 17 AnalogIn emg2_in (A2); //emg van linkerbicep, voor de y-richting
IsaRobin 0:6972d0e91af1 18 AnalogIn emg3_in (A3); //emg van een derde (nog te bepalen) spier, voor het vernaderen van de richting
IsaRobin 0:6972d0e91af1 19
IsaRobin 0:6972d0e91af1 20 //variablen voor EMG
IsaRobin 0:6972d0e91af1 21 double emg1;
IsaRobin 0:6972d0e91af1 22 double emg2;
IsaRobin 0:6972d0e91af1 23 double emg3;
IsaRobin 0:6972d0e91af1 24 double notch1;
IsaRobin 0:6972d0e91af1 25 double notch2;
IsaRobin 0:6972d0e91af1 26 double notch3;
IsaRobin 0:6972d0e91af1 27 double highpass1;
IsaRobin 0:6972d0e91af1 28 double highpass2;
IsaRobin 0:6972d0e91af1 29 double highpass3;
IsaRobin 0:6972d0e91af1 30 double lowpass1;
IsaRobin 0:6972d0e91af1 31 double lowpass2;
IsaRobin 0:6972d0e91af1 32 double lowpass3;
IsaRobin 0:6972d0e91af1 33 double rectify1;
IsaRobin 0:6972d0e91af1 34 double rectify2;
IsaRobin 0:6972d0e91af1 35 double rectify3;
IsaRobin 0:6972d0e91af1 36
IsaRobin 0:6972d0e91af1 37 //values for EMG, dit moet nog aangepast worden aan de hand van de in matlab verkregen coefficienten
IsaRobin 0:6972d0e91af1 38 const double n1 = 0,9956 //op internet staat overal a0, a1, b0, b1, b2 en dan voor iedere filter de volgende twee letters van het alfabet
IsaRobin 0:6972d0e91af1 39 const double n2 = -1,8983
IsaRobin 0:6972d0e91af1 40 const double n3 = 0,9956
IsaRobin 0:6972d0e91af1 41 const double n4 = 1
IsaRobin 0:6972d0e91af1 42 const double n5 = -1,8983
IsaRobin 0:6972d0e91af1 43 const double n6 = 0.9913
IsaRobin 0:6972d0e91af1 44 const double h1 = 0,9229
IsaRobin 0:6972d0e91af1 45 const double h2 = -1,8459
IsaRobin 0:6972d0e91af1 46 const double h3 = 0,9229
IsaRobin 0:6972d0e91af1 47 const double h4 = 1
IsaRobin 0:6972d0e91af1 48 const double h5 = -1,8892
IsaRobin 0:6972d0e91af1 49 const double n6 = 0,8928
IsaRobin 0:6972d0e91af1 50 const double l1 = 1
IsaRobin 0:6972d0e91af1 51 const double l2 = 2
IsaRobin 0:6972d0e91af1 52
IsaRobin 0:6972d0e91af1 53 //BiQuad values, dit moet nog aangepast worden aan de hand van de in matlab verkregen coefficienten
IsaRobin 0:6972d0e91af1 54 BiQuadChain notch;
IsaRobin 0:6972d0e91af1 55 BiQuad N1( n1, n2, n3, n4, n5, n6);
IsaRobin 0:6972d0e91af1 56 BiQuad N2( n1, n2, n3, n4, n5, n6);
IsaRobin 0:6972d0e91af1 57 BiQuad N3( n1, n2, n3, n4, n5, n6);
IsaRobin 0:6972d0e91af1 58 BiQuadChain highpass;
IsaRobin 0:6972d0e91af1 59 BiQuad H1( h1, h2, h3, h4, h5, h6);
IsaRobin 0:6972d0e91af1 60 BiQuad H2( h1, h2, h3, h4, h5, h6);
IsaRobin 0:6972d0e91af1 61 BiQuad H3( h1, h2, h3, h4, h5, h6);
IsaRobin 0:6972d0e91af1 62 BiQuadChain lowpass;
IsaRobin 0:6972d0e91af1 63 BiQuad L1( l1, l2, l3, l4, l5, l6);
IsaRobin 0:6972d0e91af1 64 BiQuad L2( l1, l2, l3, l4, l5, l6);
IsaRobin 0:6972d0e91af1 65 BiQuad L3( l1, l2, l3, l4, l5, l6);
IsaRobin 0:6972d0e91af1 66
IsaRobin 0:6972d0e91af1 67 //filteren van het EMG signaal
IsaRobin 0:6972d0e91af1 68 //eerst dus EMG signaal uitlezen
IsaRobin 0:6972d0e91af1 69 emg1 = emg1_in.read();
IsaRobin 0:6972d0e91af1 70 emg2 = emg2_in.read();
IsaRobin 0:6972d0e91af1 71 emg3 = emg3_in.read();
IsaRobin 0:6972d0e91af1 72
IsaRobin 0:6972d0e91af1 73 //notch filter toepassen
IsaRobin 0:6972d0e91af1 74 notch1 = N1.step(emg1);
IsaRobin 0:6972d0e91af1 75 notch2 = N2.step(emg2);
IsaRobin 0:6972d0e91af1 76 notch3 = N3.step(emg3);
IsaRobin 0:6972d0e91af1 77
IsaRobin 0:6972d0e91af1 78 //high pass filter
IsaRobin 0:6972d0e91af1 79 high1 = H1.step(notch1);
IsaRobin 0:6972d0e91af1 80 high2 = H2.step(notch2);
IsaRobin 0:6972d0e91af1 81 high3 = H3.step(notch3);
IsaRobin 0:6972d0e91af1 82
IsaRobin 0:6972d0e91af1 83 //rectify toepassen, oftewel absolute waarde van EMG signaal nemen
IsaRobin 0:6972d0e91af1 84 absolute1 = fabs(high1);
IsaRobin 0:6972d0e91af1 85 absolute2 = fabs(high2);
IsaRobin 0:6972d0e91af1 86 absolute3 = fabs(high3);
IsaRobin 0:6972d0e91af1 87
IsaRobin 0:6972d0e91af1 88 //low pass filter
IsaRobin 0:6972d0e91af1 89 low1 = L1.step(absolute1);
IsaRobin 0:6972d0e91af1 90 low2 = L2.step(absolute2);
IsaRobin 0:6972d0e91af1 91 low3 = L3.step(absolute3);