Gerhard Berman / Mbed 2 deprecated frdm_calibratie_maximum2

Working with all filters, all filter results are low amplitude.

Dependencies:   HIDScope biquadFilter mbed

Fork of frdm_calibratie_maximum by Marieke M

main.cpp@2:27081b83a58e, 2016-10-21 (annotated)

Committer:
Marieke
Date:
Fri Oct 21 11:58:32 2016 +0000
Revision:
2:27081b83a58e
Parent:
1:278677bb6b99
Child:
3:339b19905505
working with putty!!!;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Marieke 0:4d69864f1002 1 #include "mbed.h"
Marieke 2:27081b83a58e 2 #include "BiQuad.h"
Marieke 0:4d69864f1002 3 #define SERIAL_BAUD 115200
Marieke 0:4d69864f1002 4
Marieke 0:4d69864f1002 5 AnalogIn emg0( A0 );
Marieke 0:4d69864f1002 6 AnalogIn emg1( A1 );
Marieke 0:4d69864f1002 7 Serial pc(USBTX,USBRX);
Marieke 0:4d69864f1002 8
Marieke 0:4d69864f1002 9 Ticker sample_timer, average_timer, filter_timer, t;
Marieke 0:4d69864f1002 10 //HIDScope scope( 6 );
Marieke 0:4d69864f1002 11 DigitalOut led1(LED_RED);
Marieke 0:4d69864f1002 12 DigitalOut led2(LED_BLUE);
Marieke 0:4d69864f1002 13
Marieke 0:4d69864f1002 14 volatile int time_passed = 0;
Marieke 0:4d69864f1002 15 volatile bool filter_timer_go=false;
Marieke 1:278677bb6b99 16
Marieke 2:27081b83a58e 17 double EMGright, EMGleft, inR;
Marieke 2:27081b83a58e 18 double averageEMGr =0;
Marieke 2:27081b83a58e 19 double averageEMGl =0;
Marieke 0:4d69864f1002 20 //average_timer_go=false,
Marieke 0:4d69864f1002 21 // Activates go-flags
Marieke 0:4d69864f1002 22 //void average_timer_act(){average_timer_go=true;};
Marieke 2:27081b83a58e 23 void filter_timer_act(){filter_timer_go=true;};
Marieke 0:4d69864f1002 24
Marieke 1:278677bb6b99 25
Marieke 1:278677bb6b99 26
Marieke 0:4d69864f1002 27 BiQuadChain bcq;
Marieke 0:4d69864f1002 28 // Notch filter wo=50; bw=wo/35
Marieke 0:4d69864f1002 29 BiQuad bq1(9.7805e-01,-1.1978e-16,9.7805e-01,1.0000e+00,-1.1978e-16,9.5610e-01);
Marieke 0:4d69864f1002 30 // High pass Butterworth filter 2nd order, Fc=10;
Marieke 0:4d69864f1002 31 BiQuad bq2(9.8239e-01,-1.9648e+00,9.8239e-01,1.0000e+00,-1.9645e+00,9.6508e-01);
Marieke 0:4d69864f1002 32 // Low pass Butterworth filter 2nd order, Fc = 8;
Marieke 0:4d69864f1002 33 BiQuad bq3(5.6248e-05,1.1250e-04,5.6248e-05,1.0000e+00,-1.9787e+00,9.7890e-01);
Marieke 0:4d69864f1002 34 // Low pass Butterworth filter 4th order, Fc = 450; plited up in 2x2nd order
Marieke 0:4d69864f1002 35 BiQuad bq4(1.0000e+00,2.0000e+00,1.0000e+00,1.0000e+00,-4.6382e-01,8.9354e-02);
Marieke 2:27081b83a58e 36 BiQuad bq5(1.0000e+00,2.0000e+00,1.0000e+00,1.0000e+00,-6.3254e-01,4.8559e-01);
Marieke 0:4d69864f1002 37
Marieke 0:4d69864f1002 38 void KeepTrackOfTime()
Marieke 0:4d69864f1002 39 {
Marieke 0:4d69864f1002 40 time_passed++;
Marieke 0:4d69864f1002 41 }
Marieke 0:4d69864f1002 42
Marieke 0:4d69864f1002 43 // In the following: R is used for right arm, L is used for left arm!
Marieke 2:27081b83a58e 44 void FilteredSample(double averageEMGr, double averageEMGl)
Marieke 0:4d69864f1002 45 {
Marieke 2:27081b83a58e 46 double inR = emg0.read()-averageEMGr;
Marieke 2:27081b83a58e 47 double inL = emg1.read()-averageEMGl;
Marieke 0:4d69864f1002 48 //double inLcal = inL-averageL(sumL,size);
Marieke 0:4d69864f1002 49 double outRnotch = bq1.step(inR);
Marieke 0:4d69864f1002 50 double outRhigh = bq2.step(outRnotch);
Marieke 0:4d69864f1002 51 double outRrect = fabs(outRhigh);
Marieke 0:4d69864f1002 52 double outRlow = bcq.step(outRrect);
Marieke 0:4d69864f1002 53 double outLnotch = bq1.step(inL);
Marieke 0:4d69864f1002 54 double outLhigh = bq2.step(outLnotch);
Marieke 0:4d69864f1002 55 double outLrect = fabs(outLhigh);
Marieke 0:4d69864f1002 56 double outLlow = bcq.step(outLrect);
Marieke 0:4d69864f1002 57
Marieke 2:27081b83a58e 58 pc.printf("Detrend EMG = %f\n\r",inR);
Marieke 2:27081b83a58e 59 pc.printf("EMG signal= %f\n\r",emg0.read());
Marieke 2:27081b83a58e 60 pc.printf("average EMG right = %f\n\r",averageEMGr);
Marieke 2:27081b83a58e 61
Marieke 2:27081b83a58e 62 led2 = !led2;
Marieke 2:27081b83a58e 63
Marieke 2:27081b83a58e 64 /*scope.set(0, inR);
Marieke 0:4d69864f1002 65 scope.set(1, inL);
Marieke 0:4d69864f1002 66 scope.set(2, outRlow);
Marieke 0:4d69864f1002 67 scope.set(3, outLlow);
Marieke 0:4d69864f1002 68
Marieke 0:4d69864f1002 69
Marieke 0:4d69864f1002 70 scope.send();
Marieke 2:27081b83a58e 71 // To indicate that the function is working, the LED is toggled*/
Marieke 0:4d69864f1002 72 }
Marieke 0:4d69864f1002 73
Marieke 0:4d69864f1002 74 int main()
Marieke 0:4d69864f1002 75 {
Marieke 0:4d69864f1002 76 pc.baud(115200);
Marieke 0:4d69864f1002 77 led1=1;
Marieke 0:4d69864f1002 78 led2=1;
Marieke 0:4d69864f1002 79 t.attach(&KeepTrackOfTime,1.0); //taking the time in seconds
Marieke 0:4d69864f1002 80
Marieke 2:27081b83a58e 81 for (;;) {
Marieke 2:27081b83a58e 82 if (time_passed<=5)
Marieke 2:27081b83a58e 83 {
Marieke 0:4d69864f1002 84
Marieke 2:27081b83a58e 85 led1 =!led1;
Marieke 2:27081b83a58e 86 //sample_timer.attach(&sample, 0.002); //500Hz
Marieke 2:27081b83a58e 87 pc.printf("\rStart IF-loop\r\n");
Marieke 0:4d69864f1002 88
Marieke 2:27081b83a58e 89 int size = 12500;
Marieke 2:27081b83a58e 90 int i;
Marieke 2:27081b83a58e 91 double EMGsumR;
Marieke 2:27081b83a58e 92 double EMGsumL;
Marieke 2:27081b83a58e 93
Marieke 2:27081b83a58e 94 while(i<size)
Marieke 0:4d69864f1002 95 {
Marieke 2:27081b83a58e 96 EMGsumR = emg0.read()+EMGsumR;
Marieke 2:27081b83a58e 97 EMGsumL = emg1.read()+EMGsumL;
Marieke 2:27081b83a58e 98 i++;
Marieke 2:27081b83a58e 99 wait(0.0004);
Marieke 1:278677bb6b99 100 }
Marieke 1:278677bb6b99 101
Marieke 2:27081b83a58e 102 averageEMGr = (double) EMGsumR/size;
Marieke 2:27081b83a58e 103 averageEMGl = (double) EMGsumL/size;
Marieke 1:278677bb6b99 104 double InRcal = emg0.read()-averageEMGr;
Marieke 1:278677bb6b99 105 double InLcal = emg1.read()-averageEMGl;
Marieke 1:278677bb6b99 106
Marieke 2:27081b83a58e 107 scope.set(0, emg0.read() );
Marieke 0:4d69864f1002 108 scope.set(1, 1 );
Marieke 0:4d69864f1002 109 scope.set(2, averageEMGr);
Marieke 0:4d69864f1002 110 scope.set(3, averageL(sumL,size));
Marieke 0:4d69864f1002 111 scope.set(5, InRcal );
Marieke 0:4d69864f1002 112 scope.set(6, InLcal );
Marieke 0:4d69864f1002 113
Marieke 0:4d69864f1002 114 scope.send();*/
Marieke 1:278677bb6b99 115 //pc.printf("EMG-signal = %f\n\r",emg0.read());
Marieke 0:4d69864f1002 116
Marieke 2:27081b83a58e 117 /*pc.printf("\r\nIF-loop end:");
Marieke 2:27081b83a58e 118 pc.printf("time is %i sec\n",time_passed);
Marieke 2:27081b83a58e 119 pc.printf("averageR = %f\n\r", averageEMGr);*/
Marieke 2:27081b83a58e 120 }
Marieke 2:27081b83a58e 121 else
Marieke 2:27081b83a58e 122 {
Marieke 0:4d69864f1002 123 led1 = 1;
Marieke 0:4d69864f1002 124 led2=!led2;
Marieke 0:4d69864f1002 125
Marieke 1:278677bb6b99 126 bcq.add(&bq3).add(&bq4).add(&bq5);
Marieke 0:4d69864f1002 127
Marieke 0:4d69864f1002 128 filter_timer.attach(&filter_timer_act, 0.0004); //2500Hz (same as with filter coefficients on matlab!!! Thus adjust!)
Marieke 2:27081b83a58e 129 pc.printf("\rMain-loop\n\r");
Marieke 2:27081b83a58e 130 //pc.printf("Detrend EMG = %f\n\r",inR);
Marieke 0:4d69864f1002 131
Marieke 2:27081b83a58e 132 while(1)
Marieke 0:4d69864f1002 133 {
Marieke 0:4d69864f1002 134 if (filter_timer_go){
Marieke 0:4d69864f1002 135 filter_timer_go=false;
Marieke 2:27081b83a58e 136 FilteredSample(averageEMGr, averageEMGl);}
Marieke 1:278677bb6b99 137 }
Marieke 0:4d69864f1002 138
Marieke 2:27081b83a58e 139 }
Marieke 2:27081b83a58e 140 }
Marieke 0:4d69864f1002 141 }