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@2:d3e9788ab1b3, 2019-10-20 (annotated)

Committer:
Jellehierck
Date:
Sun Oct 20 18:45:13 2019 +0000
Revision:
2:d3e9788ab1b3
Parent:
1:059cca298369
Child:
3:c0ece64850db
Adjusted filters (removed redundant filters). Added libraries. Commented out irrelevant libraries. (in progress) adding functions for different filters.

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 2:d3e9788ab1b3 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
Jellehierck 2:d3e9788ab1b3 37 // Notch filter coefficients (iirnotch Q factor 35 @50Hz) from MATLAB: 0.995636295063941 -1.89829218816065 0.995636295063941 1 -1.89829218816065 0.991272590127882
Jellehierck 1:059cca298369 38 const double n_b0 = 0.995636295063941;
Jellehierck 1:059cca298369 39 const double n_b1 = -1.89829218816065;
Jellehierck 1:059cca298369 40 const double n_b2 = 0.995636295063941;
Jellehierck 1:059cca298369 41 const double n_a0 = 1;
Jellehierck 1:059cca298369 42 const double n_a1 = -1.89829218816065;
Jellehierck 1:059cca298369 43 const double n_a2 = 0.991272590127882;
Jellehierck 1:059cca298369 44
Jellehierck 2:d3e9788ab1b3 45 // Highpass filter coefficients (butter 4th order @10Hz cutoff) from MATLAB: 0.922946103200875 -1.84589220640175 0.922946103200875 1 -1.88920703055163 0.892769008131025
Jellehierck 1:059cca298369 46 const double h_b0 = 0.922946103200875;
Jellehierck 1:059cca298369 47 const double h_b1 = -1.84589220640175;
Jellehierck 1:059cca298369 48 const double h_b2 = 0.922946103200875;
Jellehierck 1:059cca298369 49 const double h_a0 = 1;
Jellehierck 1:059cca298369 50 const double h_a1 = -1.88920703055163;
Jellehierck 1:059cca298369 51 const double h_02 = 0.892769008131025;
Jellehierck 1:059cca298369 52
Jellehierck 2:d3e9788ab1b3 53 // Lowpass filter coefficients (butter 4th order @5Hz cutoff) from MATLAB: 5.32116245737504e-08 1.06423249147501e-07 5.32116245737504e-08 1 -1.94396715039462 0.944882378004138
Jellehierck 1:059cca298369 54 const double l_b0 = 5.32116245737504e-08;
Jellehierck 1:059cca298369 55 const double l_b1 = 1.06423249147501e-07;
Jellehierck 1:059cca298369 56 const double l_b2 = 5.32116245737504e-08;
Jellehierck 1:059cca298369 57 const double l_a0 = 1;
Jellehierck 1:059cca298369 58 const double l_a1 = -1.94396715039462;
Jellehierck 1:059cca298369 59 const double l_a2 = 0.944882378004138;
Jellehierck 1:059cca298369 60
IsaRobin 0:6972d0e91af1 61
Jellehierck 2:d3e9788ab1b3 62 //BiQuad filters intialization
Jellehierck 2:d3e9788ab1b3 63 BiQuadChain notch;
Jellehierck 2:d3e9788ab1b3 64 BiQuad bq_notch( n_b0, n_b1, n_b2, n_a0, n_a1, n_a2);
Jellehierck 2:d3e9788ab1b3 65
Jellehierck 2:d3e9788ab1b3 66 BiQuadChain highpass;
Jellehierck 2:d3e9788ab1b3 67 BiQuad bq_high( h_b0, h_b1, h_b2, h_a0, h_a1, h_a2);
IsaRobin 0:6972d0e91af1 68
Jellehierck 2:d3e9788ab1b3 69 BiQuadChain lowpass;
Jellehierck 2:d3e9788ab1b3 70 BiQuad bq_notch( l_b0, l_b1, l_b2, l_a0, l_a1, l_a2);
Jellehierck 2:d3e9788ab1b3 71
Jellehierck 2:d3e9788ab1b3 72 void sample()
Jellehierck 2:d3e9788ab1b3 73 {
Jellehierck 2:d3e9788ab1b3 74 emg1 = emg1_in.read();
Jellehierck 2:d3e9788ab1b3 75 emg2 = emg2_in.read();
Jellehierck 2:d3e9788ab1b3 76 emg3 = emg3_in.read();
Jellehierck 2:d3e9788ab1b3 77 }
IsaRobin 0:6972d0e91af1 78
IsaRobin 0:6972d0e91af1 79 //notch filter toepassen
IsaRobin 0:6972d0e91af1 80 notch1 = N1.step(emg1);
IsaRobin 0:6972d0e91af1 81 notch2 = N2.step(emg2);
IsaRobin 0:6972d0e91af1 82 notch3 = N3.step(emg3);
IsaRobin 0:6972d0e91af1 83
Jellehierck 2:d3e9788ab1b3 84 //high pass filter
IsaRobin 0:6972d0e91af1 85 high1 = H1.step(notch1);
IsaRobin 0:6972d0e91af1 86 high2 = H2.step(notch2);
IsaRobin 0:6972d0e91af1 87 high3 = H3.step(notch3);
IsaRobin 0:6972d0e91af1 88
IsaRobin 0:6972d0e91af1 89 //rectify toepassen, oftewel absolute waarde van EMG signaal nemen
IsaRobin 0:6972d0e91af1 90 absolute1 = fabs(high1);
IsaRobin 0:6972d0e91af1 91 absolute2 = fabs(high2);
IsaRobin 0:6972d0e91af1 92 absolute3 = fabs(high3);
IsaRobin 0:6972d0e91af1 93
IsaRobin 0:6972d0e91af1 94 //low pass filter
IsaRobin 0:6972d0e91af1 95 low1 = L1.step(absolute1);
IsaRobin 0:6972d0e91af1 96 low2 = L2.step(absolute2);
IsaRobin 0:6972d0e91af1 97 low3 = L3.step(absolute3);