EMG and motor script together, Not fully working yet,
Dependencies: Encoder QEI biquadFilter mbed
Diff: main.cpp
- Revision:
- 1:6aac013b0ba3
- Parent:
- 0:eb16ed402ffa
- Child:
- 2:2c4ee76dc830
diff -r eb16ed402ffa -r 6aac013b0ba3 main.cpp --- a/main.cpp Wed Oct 25 10:40:18 2017 +0000 +++ b/main.cpp Wed Oct 25 11:00:20 2017 +0000 @@ -27,6 +27,10 @@ DigitalOut led_G(LED_GREEN); DigitalOut led_B(LED_BLUE); +//Setting Tickers for sampling EMG and determing if the threshold is met +Ticker sample_timer; +Ticker threshold_timer; + InterruptIn button(SW2); // Wordt uiteindelijk vervangen door EMG Timer t; @@ -40,6 +44,102 @@ int delta1; int delta2; +/* Defining all the different BiQuad filters, which contain a Notch filter, +High-pass filter and Low-pass filter. The Notch filter cancels all frequencies +between 49 and 51 Hz, the High-pass filter cancels all frequencies below 20 Hz +and the Low-pass filter cancels out all frequencies below 4 Hz */ + +/* Defining all the normalized values of b and a in the Notch filter for the +creation of the Notch BiQuad */ + +BiQuad bqNotch1( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 ); +BiQuad bqNotch2( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 ); + +/* Defining all the normalized values of b and a in the High-pass filter for the +creation of the High-pass BiQuad */ + +BiQuad bqHigh1( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 ); +BiQuad bqHigh2( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 ); + +/* Defining all the normalized values of b and a in the Low-pass filter for the +creation of the Low-pass BiQuad */ + +BiQuad bqLow1( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 ); +BiQuad bqLow2( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 ); + +// Creating a variable needed for the creation of the BiQuadChain +BiQuadChain bqChain1; +BiQuadChain bqChain2; + + +//Declaring all doubles needed in the filtering process +double emgBRfiltered; //Right biceps Notch+High pass filter +double emgBRrectified; //Right biceps rectified +double emgBRcomplete; //Right biceps low-pass filter, filtering complete + +double emgBLfiltered; //Left biceps Notch+High pass filter +double emgBLrectified; //Left biceps rectified +double emgBLcomplete; //Left biceps low-pass filter, filtering complete + +/* Declaring counters and variables needed to get threshold of both muscles. +This is neeeded for the calibration process*/ +int countBR = 0; +int countBL = 0; + +double numsamples = 500; +double emgBRsum = 0; +double emgBRmeanMVC; +double thresholdBR; + +double emgBLsum = 0; +double emgBLmeanMVC; +double thresholdBL; + +// EMG functions +void EMG_sample() +{ + //Filtering steps for the Right Biceps EMG + emgBRfiltered = bqChain1.step( emgBR.read() ); //Notch+High-pass + emgBRrectified = fabs(emgBRfiltered); //Rectification + emgBRcomplete = bqLow1.step(emgBRrectified); //Low-pass + + + /*Getting threshold value for Right Biceps, a value of 30% of + Maximum Voluntary Contraction is chosen as threshold value */ + if (countBR < numsamples) { + emgBRsum = emgBRsum + emgBRcomplete; + countBR++; + } + + emgBRmeanMVC = emgBRsum / numsamples; + + thresholdBR = emgBRmeanMVC * 0.3; + + + //Filtering steps for the Left Biceps EMG + emgBLfiltered = bqChain2.step( emgBL.read() ); //Notch+High-pass + emgBLrectified = fabs( emgBLfiltered ); //Rectification + emgBLcomplete = bqLow2.step( emgBLrectified ); //Low-pass + + /*Getting threshold value for Left Biceps, a value of 30% of + Maximum Voluntary Contraction is chosen as threshold value */ + if (countBL < numsamples) { + emgBLsum = emgBLsum + emgBLcomplete; + countBL++; + } + + emgBLmeanMVC = emgBLsum / numsamples; + + thresholdBL = emgBLmeanMVC * 0.3; +} + +// Function to make the BiQuadChain for the Notch and High pass filter for both filters +void getbqChain() +{ + bqChain1.add(&bqNotch1).add(&bqHigh1); + bqChain2.add(&bqNotch2).add(&bqHigh2); +} + // Initial input value for X int Xin=0; //<- Hoe zit het met deze als we de robot daadwerkelijk gebruiken double huidigetijdX; @@ -62,10 +162,10 @@ led_G=1; led_B=1; while(true){ - button.fall(ledtX); //This has to be replaced by EMG - /*if (EMG>threshold){ + //button.fall(ledtX); //This has to be replaced by EMG + if (emgBRcomplete > thresholdBR){ ledtX(); // dit is wat je uiteindelijk wil dat er staat - }*/ + } t.start(); huidigetijdX=t.read(); if (huidigetijdX>2){ @@ -97,15 +197,15 @@ led_G=1; led_B=0; while(true){ - button.fall(ledtY); //See comments at X - /*if (EMG>threshold){ - Piek(); // dit is wat je uiteindelijk wil dat er staat - }*/ + //button.fall(ledtY); //See comments at X + if (emgBRcomplete > thresholdBR){ + ledtY(); // dit is wat je uiteindelijk wil dat er staat + } t.start(); huidigetijdY=t.read(); if (huidigetijdY>2){ led_B=1; - button.fall(0); + button.fall(0); // Wat is deze? return 0; // ga door naar het volgende programma } } @@ -393,6 +493,8 @@ int main() { pc.baud(115200); + getbqChain(); + sample_timer.attach(&EMG_sample, 0.002); tellerX(); tellerY(); calculator();