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@1:059cca298369, 2019-10-20 (annotated)

Committer:
Jellehierck
Date:
Sun Oct 20 18:28:59 2019 +0000
Revision:
1:059cca298369
Parent:
0:6972d0e91af1
Child:
2:d3e9788ab1b3
Added filter values, rewrote variable names (not finished)

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
Jellehierck 1:059cca298369 38 // Notch (iirnotch Q factor 35 @50Hz) from MATLAB: 0.995636295063941 -1.89829218816065 0.995636295063941 1 -1.89829218816065 0.991272590127882
Jellehierck 1:059cca298369 39 const double n_b0 = 0.995636295063941;
Jellehierck 1:059cca298369 40 const double n_b1 = -1.89829218816065;
Jellehierck 1:059cca298369 41 const double n_b2 = 0.995636295063941;
Jellehierck 1:059cca298369 42 const double n_a0 = 1;
Jellehierck 1:059cca298369 43 const double n_a1 = -1.89829218816065;
Jellehierck 1:059cca298369 44 const double n_a2 = 0.991272590127882;
Jellehierck 1:059cca298369 45
Jellehierck 1:059cca298369 46 // Highpass (butter 4th order @10Hz cutoff) from MATLAB: 0.922946103200875 -1.84589220640175 0.922946103200875 1 -1.88920703055163 0.892769008131025
Jellehierck 1:059cca298369 47 const double h_b0 = 0.922946103200875;
Jellehierck 1:059cca298369 48 const double h_b1 = -1.84589220640175;
Jellehierck 1:059cca298369 49 const double h_b2 = 0.922946103200875;
Jellehierck 1:059cca298369 50 const double h_a0 = 1;
Jellehierck 1:059cca298369 51 const double h_a1 = -1.88920703055163;
Jellehierck 1:059cca298369 52 const double h_02 = 0.892769008131025;
Jellehierck 1:059cca298369 53
Jellehierck 1:059cca298369 54 // Lowpass (butter 4th order @5Hz cutoff) from MATLAB: 5.32116245737504e-08 1.06423249147501e-07 5.32116245737504e-08 1 -1.94396715039462 0.944882378004138
Jellehierck 1:059cca298369 55 const double l_b0 = 5.32116245737504e-08;
Jellehierck 1:059cca298369 56 const double l_b1 = 1.06423249147501e-07;
Jellehierck 1:059cca298369 57 const double l_b2 = 5.32116245737504e-08;
Jellehierck 1:059cca298369 58 const double l_a0 = 1;
Jellehierck 1:059cca298369 59 const double l_a1 = -1.94396715039462;
Jellehierck 1:059cca298369 60 const double l_a2 = 0.944882378004138;
Jellehierck 1:059cca298369 61
IsaRobin 0:6972d0e91af1 62
IsaRobin 0:6972d0e91af1 63 //BiQuad values, dit moet nog aangepast worden aan de hand van de in matlab verkregen coefficienten
IsaRobin 0:6972d0e91af1 64 BiQuadChain notch;
Jellehierck 1:059cca298369 65 BiQuad bq_n1( n1, n2, n3, n4, n5, n6);
Jellehierck 1:059cca298369 66 BiQuad bq_n2( n1, n2, n3, n4, n5, n6);
Jellehierck 1:059cca298369 67 BiQuad bq_n3( n1, n2, n3, n4, n5, n6);
IsaRobin 0:6972d0e91af1 68 BiQuadChain highpass;
Jellehierck 1:059cca298369 69 BiQuad bq_h1( h1, h2, h3, h4, h5, h6);
Jellehierck 1:059cca298369 70 BiQuad bq_h2( h1, h2, h3, h4, h5, h6);
Jellehierck 1:059cca298369 71 BiQuad bq_h3( h1, h2, h3, h4, h5, h6);
IsaRobin 0:6972d0e91af1 72 BiQuadChain lowpass;
Jellehierck 1:059cca298369 73 BiQuad bq_l1( l1, l2, l3, l4, l5, l6);
Jellehierck 1:059cca298369 74 BiQuad bq_l2( l1, l2, l3, l4, l5, l6);
Jellehierck 1:059cca298369 75 BiQuad bq_l3( l1, l2, l3, l4, l5, l6);
IsaRobin 0:6972d0e91af1 76
IsaRobin 0:6972d0e91af1 77 //filteren van het EMG signaal
IsaRobin 0:6972d0e91af1 78 //eerst dus EMG signaal uitlezen
IsaRobin 0:6972d0e91af1 79 emg1 = emg1_in.read();
IsaRobin 0:6972d0e91af1 80 emg2 = emg2_in.read();
IsaRobin 0:6972d0e91af1 81 emg3 = emg3_in.read();
IsaRobin 0:6972d0e91af1 82
IsaRobin 0:6972d0e91af1 83 //notch filter toepassen
IsaRobin 0:6972d0e91af1 84 notch1 = N1.step(emg1);
IsaRobin 0:6972d0e91af1 85 notch2 = N2.step(emg2);
IsaRobin 0:6972d0e91af1 86 notch3 = N3.step(emg3);
IsaRobin 0:6972d0e91af1 87
IsaRobin 0:6972d0e91af1 88 //high pass filter
IsaRobin 0:6972d0e91af1 89 high1 = H1.step(notch1);
IsaRobin 0:6972d0e91af1 90 high2 = H2.step(notch2);
IsaRobin 0:6972d0e91af1 91 high3 = H3.step(notch3);
IsaRobin 0:6972d0e91af1 92
IsaRobin 0:6972d0e91af1 93 //rectify toepassen, oftewel absolute waarde van EMG signaal nemen
IsaRobin 0:6972d0e91af1 94 absolute1 = fabs(high1);
IsaRobin 0:6972d0e91af1 95 absolute2 = fabs(high2);
IsaRobin 0:6972d0e91af1 96 absolute3 = fabs(high3);
IsaRobin 0:6972d0e91af1 97
IsaRobin 0:6972d0e91af1 98 //low pass filter
IsaRobin 0:6972d0e91af1 99 low1 = L1.step(absolute1);
IsaRobin 0:6972d0e91af1 100 low2 = L2.step(absolute2);
IsaRobin 0:6972d0e91af1 101 low3 = L3.step(absolute3);