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@6:5437cc97e1e6, 2019-10-20 (annotated)

Committer:
Jellehierck
Date:
Sun Oct 20 19:53:00 2019 +0000
Revision:
6:5437cc97e1e6
Parent:
5:3d65f89e3755
Child:
7:7a088536f1c9
Added filter stability check, cleaned up code a bit

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 6:5437cc97e1e6 22 DigitalOut led_g(LED_GREEN);
Jellehierck 6:5437cc97e1e6 23 DigitalOut led_r(LED_RED);
Jellehierck 4:09a01d2db8f7 24
IsaRobin 0:6972d0e91af1 25 //variablen voor EMG
IsaRobin 0:6972d0e91af1 26 double emg1;
IsaRobin 0:6972d0e91af1 27 double emg2;
IsaRobin 0:6972d0e91af1 28 double emg3;
IsaRobin 0:6972d0e91af1 29 double notch1;
IsaRobin 0:6972d0e91af1 30 double notch2;
IsaRobin 0:6972d0e91af1 31 double notch3;
IsaRobin 0:6972d0e91af1 32 double highpass1;
IsaRobin 0:6972d0e91af1 33 double highpass2;
IsaRobin 0:6972d0e91af1 34 double highpass3;
IsaRobin 0:6972d0e91af1 35 double lowpass1;
IsaRobin 0:6972d0e91af1 36 double lowpass2;
IsaRobin 0:6972d0e91af1 37 double lowpass3;
IsaRobin 0:6972d0e91af1 38 double rectify1;
IsaRobin 0:6972d0e91af1 39 double rectify2;
IsaRobin 0:6972d0e91af1 40 double rectify3;
IsaRobin 0:6972d0e91af1 41
Jellehierck 4:09a01d2db8f7 42 // Initialize tickers
Jellehierck 4:09a01d2db8f7 43 Ticker tickSample;
Jellehierck 4:09a01d2db8f7 44
Jellehierck 4:09a01d2db8f7 45 // Sample rate
Jellehierck 4:09a01d2db8f7 46 const double Fs = 500; //Hz
Jellehierck 4:09a01d2db8f7 47
Jellehierck 3:c0ece64850db 48 // Notch filter coefficients (iirnotch Q factor 35 @50Hz) from MATLAB in the following form:
Jellehierck 3:c0ece64850db 49 // b01 b11 b21 a01 a11 a21
Jellehierck 3:c0ece64850db 50 BiQuad bq_notch(0.995636295063941, -1.89829218816065, 0.995636295063941, 1, -1.89829218816065, 0.991272590127882);
Jellehierck 1:059cca298369 51
Jellehierck 3:c0ece64850db 52 // Highpass filter coefficients (butter 4th order @10Hz cutoff) from MATLAB in the following form:
Jellehierck 3:c0ece64850db 53 // b01 b11 b21 a01 a11 a21
Jellehierck 3:c0ece64850db 54 // b02 b12 b22 a02 a12 a22
Jellehierck 3:c0ece64850db 55 BiQuad bq_H1(0.922946103200875, -1.84589220640175, 0.922946103200875, 1, -1.88920703055163, 0.892769008131025);
Jellehierck 3:c0ece64850db 56 BiQuad bq_H2(1, -2, 1, 1, -1.95046575793011, 0.954143234875078);
Jellehierck 4:09a01d2db8f7 57 BiQuadChain bqc_notch_high; // Used to chain two 2nd other filters into a 4th order filter
IsaRobin 0:6972d0e91af1 58
Jellehierck 3:c0ece64850db 59 // Lowpass filter coefficients (butter 4th order @5Hz cutoff) from MATLAB in the following form:
Jellehierck 3:c0ece64850db 60 // b01 b11 b21 a01 a11 a21
Jellehierck 3:c0ece64850db 61 // b02 b12 b22 a02 a12 a22
Jellehierck 3:c0ece64850db 62 BiQuad bq_L1(5.32116245737504e-08, 1.06423249147501e-07, 5.32116245737504e-08, 1, -1.94396715039462, 0.944882378004138);
Jellehierck 3:c0ece64850db 63 BiQuad bq_L2(1, 2, 1, 1, -1.97586467534468, 0.976794920438162);
Jellehierck 3:c0ece64850db 64 BiQuadChain bqc_low; // Used to chain two 2nd other filters into a 4th order filter
Jellehierck 2:d3e9788ab1b3 65
Jellehierck 6:5437cc97e1e6 66 // Check if filters are stable
Jellehierck 6:5437cc97e1e6 67 bool checkBQChainStable()
Jellehierck 6:5437cc97e1e6 68 {
Jellehierck 6:5437cc97e1e6 69 bool n_hp_stable = bqc_notch_high.stable();
Jellehierck 6:5437cc97e1e6 70 bool l_stable = bqc_low.stable();
Jellehierck 6:5437cc97e1e6 71
Jellehierck 6:5437cc97e1e6 72 if (n_hp_stable && l_stable) {
Jellehierck 6:5437cc97e1e6 73 return true;
Jellehierck 6:5437cc97e1e6 74 } else {
Jellehierck 6:5437cc97e1e6 75 return false;
Jellehierck 6:5437cc97e1e6 76 }
Jellehierck 6:5437cc97e1e6 77 }
Jellehierck 6:5437cc97e1e6 78
Jellehierck 6:5437cc97e1e6 79
Jellehierck 6:5437cc97e1e6 80 // Read samples, filter samples and output to HIDScope
Jellehierck 2:d3e9788ab1b3 81 void sample()
Jellehierck 2:d3e9788ab1b3 82 {
Jellehierck 4:09a01d2db8f7 83 // Read EMG inputs
Jellehierck 2:d3e9788ab1b3 84 emg1 = emg1_in.read();
Jellehierck 2:d3e9788ab1b3 85 emg2 = emg2_in.read();
Jellehierck 2:d3e9788ab1b3 86 emg3 = emg3_in.read();
Jellehierck 4:09a01d2db8f7 87
Jellehierck 4:09a01d2db8f7 88 // Output raw EMG input
Jellehierck 4:09a01d2db8f7 89 scope.set(0, emg1 );
Jellehierck 6:5437cc97e1e6 90
Jellehierck 5:3d65f89e3755 91 // Filter notch and highpass
Jellehierck 5:3d65f89e3755 92 double emg1_n_hp = bqc_notch_high.step( emg1 );
Jellehierck 6:5437cc97e1e6 93
Jellehierck 5:3d65f89e3755 94 // Rectify
Jellehierck 5:3d65f89e3755 95 double emg1_rectify = fabs( emg1_n_hp );
Jellehierck 6:5437cc97e1e6 96
Jellehierck 5:3d65f89e3755 97 // Filter lowpass (completes envelope)
Jellehierck 5:3d65f89e3755 98 double emg1_env = bqc_low.step( emg1_rectify );
Jellehierck 4:09a01d2db8f7 99
Jellehierck 4:09a01d2db8f7 100 // Output EMG after filters
Jellehierck 5:3d65f89e3755 101 scope.set(1, emg1_env );
Jellehierck 4:09a01d2db8f7 102 scope.send();
Jellehierck 2:d3e9788ab1b3 103 }
IsaRobin 0:6972d0e91af1 104
Jellehierck 5:3d65f89e3755 105 void main()
Jellehierck 4:09a01d2db8f7 106 {
Jellehierck 6:5437cc97e1e6 107 // Initialize sample ticker
Jellehierck 4:09a01d2db8f7 108 const double Ts = 1/Fs;
Jellehierck 4:09a01d2db8f7 109 tickSample.attach(&sample, Ts);
Jellehierck 6:5437cc97e1e6 110
Jellehierck 6:5437cc97e1e6 111 // Create BQ chains to reduce computations
Jellehierck 5:3d65f89e3755 112 bqc_notch_high.add( &bq_notch ).add( &bq_H1 ).add( &bq_H2 );
Jellehierck 5:3d65f89e3755 113 bqc_low.add( &bq_L1 ).add( &bq_L2 );
Jellehierck 4:09a01d2db8f7 114
Jellehierck 6:5437cc97e1e6 115 // If any filter chain is unstable, red led will light up
Jellehierck 6:5437cc97e1e6 116 if (checkBQChainStable) {
Jellehierck 6:5437cc97e1e6 117 led_r = 1; // LED off
Jellehierck 6:5437cc97e1e6 118 } else {
Jellehierck 6:5437cc97e1e6 119 led_r = 0; // LED on
Jellehierck 6:5437cc97e1e6 120 }
Jellehierck 6:5437cc97e1e6 121
Jellehierck 4:09a01d2db8f7 122 while(true) {
Jellehierck 6:5437cc97e1e6 123
Jellehierck 6:5437cc97e1e6 124 // Show that system is running
Jellehierck 6:5437cc97e1e6 125 led_g = !led_g;
Jellehierck 4:09a01d2db8f7 126 wait(0.5);
Jellehierck 4:09a01d2db8f7 127 }
Jellehierck 4:09a01d2db8f7 128 }