Roos Wijngaarden / Mbed 2 deprecated EMG_Calibration

Sets threshold at 15% of maximum contraction

Dependencies:   HIDScope MODSERIAL Math MovingAverage biquadFilter mbed

main.cpp@0:91d5c871dbf9, 2018-10-26 (annotated)

Committer:
Roooos
Date:
Fri Oct 26 11:07:02 2018 +0000
Revision:
0:91d5c871dbf9
Sets threshold at 15% of maximum contraction

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Roooos 0:91d5c871dbf9 1 #include "mbed.h"
Roooos 0:91d5c871dbf9 2 #include "HIDScope.h"
Roooos 0:91d5c871dbf9 3 #include "BiQuad.h"
Roooos 0:91d5c871dbf9 4 #include "MovingAverage.h"
Roooos 0:91d5c871dbf9 5 #include "MODSERIAL.h"
Roooos 0:91d5c871dbf9 6 #include "math.h"
Roooos 0:91d5c871dbf9 7 #define NSAMPLE 200
Roooos 0:91d5c871dbf9 8
Roooos 0:91d5c871dbf9 9 //Define objects
Roooos 0:91d5c871dbf9 10 AnalogIn emg0( A0 ); //Biceps Left
Roooos 0:91d5c871dbf9 11 AnalogIn emg1( A1 ); //Biceps Right
Roooos 0:91d5c871dbf9 12 Ticker sample_timer;
Roooos 0:91d5c871dbf9 13 InterruptIn button(PTC6);
Roooos 0:91d5c871dbf9 14
Roooos 0:91d5c871dbf9 15 //Ticker driving_timer;
Roooos 0:91d5c871dbf9 16 HIDScope scope( 4 ); //Number of Channels
Roooos 0:91d5c871dbf9 17 DigitalOut led1(LED_RED);
Roooos 0:91d5c871dbf9 18 DigitalOut led2(LED_GREEN);
Roooos 0:91d5c871dbf9 19 DigitalOut led3(LED_BLUE);
Roooos 0:91d5c871dbf9 20 MovingAverage <double>Movag_LB(NSAMPLE,0.0); //Make Moving Average, Define NSAMPLE above
Roooos 0:91d5c871dbf9 21 MovingAverage <double>Movag_RB(NSAMPLE,0.0);
Roooos 0:91d5c871dbf9 22 volatile double LB_Out;
Roooos 0:91d5c871dbf9 23 volatile double RB_Out;
Roooos 0:91d5c871dbf9 24
Roooos 0:91d5c871dbf9 25 MODSERIAL pc(USBTX, USBRX);
Roooos 0:91d5c871dbf9 26
Roooos 0:91d5c871dbf9 27 //Make Biquad filters Left(a0, a1, a2, b1, b2)
Roooos 0:91d5c871dbf9 28 BiQuadChain bqc1; //chain voor High Pass en Notch
Roooos 0:91d5c871dbf9 29 BiQuad bq1(0.39131200825291007, -0.7826240165058201, 0.39131200825291007, -0.36950493743858204, 0.1957430955730582); //High Pass Filter
Roooos 0:91d5c871dbf9 30 BiQuad bq2(9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01); //Notch Filter
Roooos 0:91d5c871dbf9 31 //BiQuad bq3(0.00044924972004654657, 0.0008984994400930931, 0.00044924972004654657, -1.6169385656964415, 0.6187355645766276); //Low Pass Filter
Roooos 0:91d5c871dbf9 32
Roooos 0:91d5c871dbf9 33 //Make Biquad filters Right
Roooos 0:91d5c871dbf9 34 BiQuadChain bqc2; //chain voor High Pass en Notch
Roooos 0:91d5c871dbf9 35 BiQuad bq4(0.39131200825291007, -0.7826240165058201, 0.39131200825291007, -0.36950493743858204, 0.1957430955730582); //High Pass Filter
Roooos 0:91d5c871dbf9 36 BiQuad bq5(9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01); //Notch Filter
Roooos 0:91d5c871dbf9 37
Roooos 0:91d5c871dbf9 38 volatile double MaxLeft = 0.0;
Roooos 0:91d5c871dbf9 39 volatile double MaxRight = 0.0;
Roooos 0:91d5c871dbf9 40 volatile double ThresholdLeft = 0.0;
Roooos 0:91d5c871dbf9 41 volatile double ThresholdRight = 0.0;
Roooos 0:91d5c871dbf9 42
Roooos 0:91d5c871dbf9 43 //Function to read samples and filter and send to HIDScope
Roooos 0:91d5c871dbf9 44 void sample()
Roooos 0:91d5c871dbf9 45 {
Roooos 0:91d5c871dbf9 46 //Filter Left Bicep
Roooos 0:91d5c871dbf9 47 double LB_Filter_1 = bqc1.step(emg0.read()); //High Pass + Notch
Roooos 0:91d5c871dbf9 48 double LB_Rectify = fabs(LB_Filter_1); //Rectify Signal
Roooos 0:91d5c871dbf9 49 //double Filtered_emg_1 = bq3.step(LB_Rectify); //Low Pass
Roooos 0:91d5c871dbf9 50 Movag_LB.Insert(LB_Rectify);
Roooos 0:91d5c871dbf9 51 LB_Out = Movag_LB.GetAverage();
Roooos 0:91d5c871dbf9 52
Roooos 0:91d5c871dbf9 53 //Filter Right Bicep
Roooos 0:91d5c871dbf9 54 double RB_Filter_1 = bqc2.step(emg1.read()); //High Pass + Notch
Roooos 0:91d5c871dbf9 55 double RB_Rectify = fabs(RB_Filter_1); //Rectify Signal
Roooos 0:91d5c871dbf9 56 //double Filtered_emg_1 = bq3.step(RB_Rectify); //Low Pass
Roooos 0:91d5c871dbf9 57 Movag_RB.Insert(RB_Rectify);
Roooos 0:91d5c871dbf9 58 RB_Out = Movag_RB.GetAverage();
Roooos 0:91d5c871dbf9 59
Roooos 0:91d5c871dbf9 60 scope.set(0, emg0.read() ); //Raw EMG signal left bicep
Roooos 0:91d5c871dbf9 61 scope.set(1, LB_Out); // Filtered EMG signal left bicep
Roooos 0:91d5c871dbf9 62 scope.set(2, emg1.read()); //Raw EMG signal right bicep
Roooos 0:91d5c871dbf9 63 scope.set(3, RB_Out); //Filtered EMG signal right bicep
Roooos 0:91d5c871dbf9 64
Roooos 0:91d5c871dbf9 65 scope.send();
Roooos 0:91d5c871dbf9 66
Roooos 0:91d5c871dbf9 67 if(LB_Out > MaxLeft) {
Roooos 0:91d5c871dbf9 68 MaxLeft = LB_Out;
Roooos 0:91d5c871dbf9 69 led2 = 0;
Roooos 0:91d5c871dbf9 70 }
Roooos 0:91d5c871dbf9 71 else {
Roooos 0:91d5c871dbf9 72 led2 = 1;
Roooos 0:91d5c871dbf9 73 }
Roooos 0:91d5c871dbf9 74 if(RB_Out > MaxRight) {
Roooos 0:91d5c871dbf9 75 MaxRight = RB_Out;
Roooos 0:91d5c871dbf9 76 led3 = 0;
Roooos 0:91d5c871dbf9 77 }
Roooos 0:91d5c871dbf9 78 else {
Roooos 0:91d5c871dbf9 79 led3 = 1;
Roooos 0:91d5c871dbf9 80 }
Roooos 0:91d5c871dbf9 81 ThresholdLeft = abs(0.15000*MaxLeft);
Roooos 0:91d5c871dbf9 82 ThresholdRight = abs(0.15000*MaxRight);
Roooos 0:91d5c871dbf9 83
Roooos 0:91d5c871dbf9 84 }
Roooos 0:91d5c871dbf9 85
Roooos 0:91d5c871dbf9 86 void PrintThreshold()
Roooos 0:91d5c871dbf9 87 {
Roooos 0:91d5c871dbf9 88 pc.baud(115200);
Roooos 0:91d5c871dbf9 89 pc.printf("left: %.3f threshold : %.3f\r\n right: %.3f threshold : %.3f\r\n ",MaxLeft, ThresholdLeft,MaxRight,ThresholdRight);
Roooos 0:91d5c871dbf9 90 }
Roooos 0:91d5c871dbf9 91
Roooos 0:91d5c871dbf9 92
Roooos 0:91d5c871dbf9 93 int main()
Roooos 0:91d5c871dbf9 94 {
Roooos 0:91d5c871dbf9 95 bqc1.add( &bq1 ).add( &bq2 ); //make BiQuadChain
Roooos 0:91d5c871dbf9 96 bqc2.add( &bq4 ).add( &bq5 ); //make BiQuadChain
Roooos 0:91d5c871dbf9 97 sample_timer.attach(&sample, 0.002); //read samples at 500 Hz
Roooos 0:91d5c871dbf9 98 button.fall(&PrintThreshold);
Roooos 0:91d5c871dbf9 99 while(1) {}
Roooos 0:91d5c871dbf9 100 }
Roooos 0:91d5c871dbf9 101