Roy van Zijl / Mbed 2 deprecated TestSubjectA

Mbed bordje 1 -af

Dependencies:   Encoder HIDScope MODSERIAL Matrix MatrixMath biquad-master mbed

Fork of dsjklafjaslkjdfalkjfdaslkasdjklajadsflkjdasflkjdasflkadsflkasd by Dion de Greef

main.cpp@2:293665548183, 2017-10-24 (annotated)

Committer:
RoyvZ
Date:
Tue Oct 24 11:58:29 2017 +0000
Revision:
2:293665548183
Parent:
0:b6c8d56842ce
Child:
3:2ffbee47fb38
EMG filtering;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RoyvZ 0:b6c8d56842ce 1 /**
RoyvZ 0:b6c8d56842ce 2 * Demo program for BiQuad and BiQuadChain classes
RoyvZ 0:b6c8d56842ce 3 * author: T.J.W. Lankhorst <t.j.w.lankhorst@student.utwente.nl> and Matthijs and Roy and Dion
RoyvZ 0:b6c8d56842ce 4 */
RoyvZ 0:b6c8d56842ce 5 #include "mbed.h"
RoyvZ 0:b6c8d56842ce 6 #include "HIDScope.h"
RoyvZ 0:b6c8d56842ce 7 #include <stdlib.h>
RoyvZ 0:b6c8d56842ce 8 #include <iostream>
RoyvZ 0:b6c8d56842ce 9 #include <iomanip>
RoyvZ 0:b6c8d56842ce 10 #include <complex>
RoyvZ 0:b6c8d56842ce 11 #include "BiQuad.h"
RoyvZ 0:b6c8d56842ce 12
RoyvZ 2:293665548183 13
RoyvZ 0:b6c8d56842ce 14 AnalogIn emg0( A0 );
RoyvZ 0:b6c8d56842ce 15 AnalogIn emg1( A1 );
RoyvZ 2:293665548183 16 DigitalIn button1(PTA4);
RoyvZ 0:b6c8d56842ce 17
RoyvZ 0:b6c8d56842ce 18 Ticker sample_timer;
RoyvZ 2:293665548183 19 Ticker cal_timer;
RoyvZ 0:b6c8d56842ce 20 HIDScope scope( 2 );
RoyvZ 0:b6c8d56842ce 21 DigitalOut led(LED1);
RoyvZ 2:293665548183 22 //DigitalOut led2(LED_GREEN);
RoyvZ 2:293665548183 23
RoyvZ 2:293665548183 24 double S[50];
RoyvZ 2:293665548183 25 double meanSum = 0;
RoyvZ 2:293665548183 26
RoyvZ 2:293665548183 27 //Filter toevoegen, bestaande uit notch, high- en lowpass filter
RoyvZ 2:293665548183 28 BiQuadChain Notch;
RoyvZ 2:293665548183 29 //50 Hz Notch filter, for radiation from surroundings
RoyvZ 0:b6c8d56842ce 30
RoyvZ 2:293665548183 31 //Notch filter chain for 100 Hz
RoyvZ 2:293665548183 32 BiQuad bqNotch1( 9.75180e-01, -1.85510e+00, 9.75218e-01, -1.87865e+00, 9.75178e-01 );
RoyvZ 2:293665548183 33 BiQuad bqNotch2( 1.00000e+00, -1.90228e+00, 1.00004e+00, -1.88308e+00, 9.87097e-01 );
RoyvZ 2:293665548183 34 BiQuad bqNotch3( 1.00000e+00, -1.90219e+00, 9.99925e-01, -1.89724e+00, 9.87929e-01 );
RoyvZ 2:293665548183 35
RoyvZ 2:293665548183 36 //Notch filter chain for 50 Hz
RoyvZ 2:293665548183 37 BiQuad bq1( 9.87512e-01, -1.95079e+00, 9.87563e-01, -1.96308e+00, 9.87512e-01 );
RoyvZ 2:293665548183 38 BiQuad bq2( 1.00000e+00, -1.97533e+00, 9.99919e-01, -1.96726e+00, 9.93522e-01 );
RoyvZ 2:293665548183 39 BiQuad bq3( 1.00000e+00, -1.97546e+00, 1.00003e+00, -1.97108e+00, 9.93951e-01 );
RoyvZ 2:293665548183 40
RoyvZ 2:293665548183 41 //15 Hz Highpass filter, as recommended by multiple studies regarding EMG signals
RoyvZ 2:293665548183 42 //BiQuad Highpass15(9.35527e-01, -1.87105e+00, 9.35527e-01, -1.86689e+00, 8.75215e-01);
RoyvZ 2:293665548183 43
RoyvZ 2:293665548183 44 //2Hz Highpass
RoyvZ 2:293665548183 45 BiQuad Highpass1(9.91154e-01, -1.98231e+00, 9.91154e-01, -1.98223e+00, 9.82385e-01);
RoyvZ 0:b6c8d56842ce 46
RoyvZ 0:b6c8d56842ce 47
RoyvZ 2:293665548183 48 //20 Hz Lowpass
RoyvZ 2:293665548183 49 BiQuad Lowpass100( 3.62168e-03, 7.24336e-03, 3.62168e-03, -1.82269e+00, 8.37182e-01 );
RoyvZ 0:b6c8d56842ce 50
RoyvZ 2:293665548183 51 //450 Hz Lowpass filter, to remove any noise (sample frequency is only 500 Hz, but still...)
RoyvZ 2:293665548183 52 //BiQuad Lowpass450(8.00592e-01, 1.60118e+00, 8.00592e-01, 1.56102e+00, 6.41352e-01);
RoyvZ 0:b6c8d56842ce 53
RoyvZ 0:b6c8d56842ce 54 void sample()
RoyvZ 0:b6c8d56842ce 55 {
RoyvZ 0:b6c8d56842ce 56 /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
RoyvZ 2:293665548183 57 //scope.set(0, emg0.read() );
RoyvZ 2:293665548183 58 //scope.set(1, fabs(Notch.step(emg0.read())) );
RoyvZ 2:293665548183 59 scope.set(0, fabs(Highpass1.step(Lowpass100.step(Notch.step(emg0.read())))) );
RoyvZ 2:293665548183 60
RoyvZ 2:293665548183 61 for (int i=1;i < 50; i++) {
RoyvZ 2:293665548183 62 S[i] = S[i-1];
RoyvZ 2:293665548183 63 meanSum = meanSum + S[i];
RoyvZ 2:293665548183 64 }
RoyvZ 2:293665548183 65 S[0] = fabs(Highpass1.step(Lowpass100.step(Notch.step(emg0.read()))));
RoyvZ 2:293665548183 66 double mean = meanSum / 50;
RoyvZ 2:293665548183 67 scope.set(1, fabs(4*Highpass1.step(Lowpass100.step(Notch.step(emg0.read())))));
RoyvZ 2:293665548183 68 meanSum = 0;
RoyvZ 0:b6c8d56842ce 69 /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
RoyvZ 0:b6c8d56842ce 70 * Ensure that enough channels are available (HIDScope scope( 2 ))
RoyvZ 0:b6c8d56842ce 71 * Finally, send all channels to the PC at once */
RoyvZ 0:b6c8d56842ce 72 scope.send();
RoyvZ 0:b6c8d56842ce 73 /* To indicate that the function is working, the LED is toggled */
RoyvZ 0:b6c8d56842ce 74 led = !led;
RoyvZ 0:b6c8d56842ce 75 }
RoyvZ 0:b6c8d56842ce 76
RoyvZ 2:293665548183 77 void calibration()
RoyvZ 2:293665548183 78 {
RoyvZ 2:293665548183 79 //function to determine maximal EMG input in order to allow motorcontrol to be activated
RoyvZ 2:293665548183 80 double maxsignal = 0;
RoyvZ 2:293665548183 81 if (button1.read() == false){
RoyvZ 2:293665548183 82 for (int n = 1; n < 5000; n++){ //calibrate for 5000 samples or 10 seconds
RoyvZ 2:293665548183 83 double signalfinal = fabs(Highpass1.step(Lowpass100.step(Notch.step(emg0.read()))));
RoyvZ 2:293665548183 84 if (signalfinal >= maxsignal){
RoyvZ 2:293665548183 85 maxsignal = signalfinal; //keep changing the maximal signal
RoyvZ 2:293665548183 86 }
RoyvZ 2:293665548183 87 }
RoyvZ 2:293665548183 88 }
RoyvZ 2:293665548183 89 }
RoyvZ 2:293665548183 90
RoyvZ 0:b6c8d56842ce 91 int main()
RoyvZ 0:b6c8d56842ce 92 {
RoyvZ 2:293665548183 93 //Constructing the notch filter chain
RoyvZ 2:293665548183 94 Notch.add( &bqNotch1 ).add( &bqNotch2 ).add( &bqNotch3 ).add( &bq1 ).add( &bq2 ).add( &bq3 );
RoyvZ 2:293665548183 95
RoyvZ 0:b6c8d56842ce 96 /**Attach the 'sample' function to the timer 'sample_timer'.
RoyvZ 2:293665548183 97 * this ensures that 'sample' is executed every... 0.001 seconds = 1000 Hz
RoyvZ 0:b6c8d56842ce 98 */
RoyvZ 2:293665548183 99 sample_timer.attach(&sample, 0.001);
RoyvZ 2:293665548183 100 cal_timer.attach(&calibration, 0.002);//ticker to check if motor is being calibrated
RoyvZ 0:b6c8d56842ce 101
RoyvZ 0:b6c8d56842ce 102 /*empty loop, sample() is executed periodically*/
RoyvZ 0:b6c8d56842ce 103 while(true) {
RoyvZ 0:b6c8d56842ce 104
RoyvZ 2:293665548183 105
RoyvZ 0:b6c8d56842ce 106 }
RoyvZ 0:b6c8d56842ce 107
RoyvZ 0:b6c8d56842ce 108 }