Joost Herijgers / Mbed 2 deprecated Calibration_EMG_15

kapotte printf

Dependencies:   HIDScope biquadFilter mbed

main.cpp@2:dac640cad05e, 2017-10-23 (annotated)

Committer:
Joost38H
Date:
Mon Oct 23 09:06:57 2017 +0000
Revision:
2:dac640cad05e
Parent:
1:4d7097e583e0 
kapotte printf

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Joost38H 0:33f422404c9f 1 #include "mbed.h"
Joost38H 0:33f422404c9f 2 #include "HIDScope.h"
Joost38H 0:33f422404c9f 3 #include "BiQuad.h"
Joost38H 0:33f422404c9f 4 #include "math.h"
Joost38H 0:33f422404c9f 5
Joost38H 2:dac640cad05e 6
Joost38H 2:dac640cad05e 7 Serial pc(USBTX, USBRX);
Joost38H 2:dac640cad05e 8
Joost38H 0:33f422404c9f 9
Joost38H 0:33f422404c9f 10 //Defining all in- and outputs
Joost38H 0:33f422404c9f 11 //EMG input
Joost38H 0:33f422404c9f 12 AnalogIn emgBR( A0 ); //Right Biceps
Joost38H 0:33f422404c9f 13 AnalogIn emgBL( A1 ); //Left Biceps
Joost38H 0:33f422404c9f 14
Joost38H 0:33f422404c9f 15 //LED output
Joost38H 0:33f422404c9f 16 DigitalOut led_B( LED_BLUE );
Joost38H 0:33f422404c9f 17 DigitalOut led_R( LED_RED );
Joost38H 0:33f422404c9f 18 DigitalOut led_G( LED_GREEN );
Joost38H 0:33f422404c9f 19
Joost38H 1:4d7097e583e0 20 //Setting Tickers for sampling EMG and determing if the threshold is met
Joost38H 0:33f422404c9f 21 Ticker sample_timer;
Joost38H 0:33f422404c9f 22 Ticker threshold_timer;
Joost38H 0:33f422404c9f 23
Joost38H 0:33f422404c9f 24 // Defining HIDScope ports needed
Joost38H 1:4d7097e583e0 25 HIDScope scope( 2 );
Joost38H 0:33f422404c9f 26
Joost38H 0:33f422404c9f 27
Joost38H 0:33f422404c9f 28 /* Defining all the different BiQuad filters, which contain a Notch filter,
Joost38H 0:33f422404c9f 29 High-pass filter and Low-pass filter. The Notch filter cancels all frequencies
Joost38H 0:33f422404c9f 30 between 49 and 51 Hz, the High-pass filter cancels all frequencies below 20 Hz
Joost38H 0:33f422404c9f 31 and the Low-pass filter cancels out all frequencies below 4 Hz */
Joost38H 0:33f422404c9f 32
Joost38H 0:33f422404c9f 33 /* Defining all the normalized values of b and a in the Notch filter for the
Joost38H 0:33f422404c9f 34 creation of the Notch BiQuad */
Joost38H 0:33f422404c9f 35
Joost38H 0:33f422404c9f 36 BiQuad bqNotch1( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
Joost38H 0:33f422404c9f 37 BiQuad bqNotch2( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
Joost38H 0:33f422404c9f 38
Joost38H 0:33f422404c9f 39 /* Defining all the normalized values of b and a in the High-pass filter for the
Joost38H 0:33f422404c9f 40 creation of the High-pass BiQuad */
Joost38H 0:33f422404c9f 41
Joost38H 0:33f422404c9f 42 BiQuad bqHigh1( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
Joost38H 0:33f422404c9f 43 BiQuad bqHigh2( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
Joost38H 0:33f422404c9f 44
Joost38H 0:33f422404c9f 45 /* Defining all the normalized values of b and a in the Low-pass filter for the
Joost38H 0:33f422404c9f 46 creation of the Low-pass BiQuad */
Joost38H 0:33f422404c9f 47
Joost38H 0:33f422404c9f 48 BiQuad bqLow1( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );
Joost38H 0:33f422404c9f 49 BiQuad bqLow2( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );
Joost38H 0:33f422404c9f 50
Joost38H 0:33f422404c9f 51 // Creating a variable needed for the creation of the BiQuadChain
Joost38H 0:33f422404c9f 52 BiQuadChain bqChain1;
Joost38H 0:33f422404c9f 53 BiQuadChain bqChain2;
Joost38H 0:33f422404c9f 54
Joost38H 0:33f422404c9f 55
Joost38H 0:33f422404c9f 56 //Defining all doubles needed in the filtering process
Joost38H 0:33f422404c9f 57 double emgBRfiltered; //Right biceps Notch+High pass filter
Joost38H 0:33f422404c9f 58 double emgBRrectified; //Right biceps rectified
Joost38H 0:33f422404c9f 59 double emgBRcomplete; //Right biceps low-pass filter, filtering complete
Joost38H 0:33f422404c9f 60
Joost38H 0:33f422404c9f 61 double emgBLfiltered; //Left biceps Notch+High pass filter
Joost38H 0:33f422404c9f 62 double emgBLrectified; //Left biceps rectified
Joost38H 0:33f422404c9f 63 double emgBLcomplete; //Left biceps low-pass filter, filtering complete
Joost38H 0:33f422404c9f 64
Joost38H 0:33f422404c9f 65
Joost38H 0:33f422404c9f 66 double threshold = 0.03;
Joost38H 0:33f422404c9f 67
Joost38H 0:33f422404c9f 68
Joost38H 0:33f422404c9f 69 /** Sample function
Joost38H 0:33f422404c9f 70 * this function samples the BR EMG, filters the EMG and sends it to HIDScope
Joost38H 0:33f422404c9f 71 **/
Joost38H 0:33f422404c9f 72 void EMG_sample()
Joost38H 0:33f422404c9f 73 {
Joost38H 0:33f422404c9f 74 emgBRfiltered = bqChain1.step( emgBR.read() ); //Notch+High-pass
Joost38H 0:33f422404c9f 75 emgBRrectified = fabs(emgBRfiltered); //Rectification
Joost38H 0:33f422404c9f 76 emgBRcomplete = bqLow1.step(emgBRrectified); //Low-pass
Joost38H 0:33f422404c9f 77
Joost38H 2:dac640cad05e 78
Joost38H 0:33f422404c9f 79
Joost38H 0:33f422404c9f 80 emgBLfiltered = bqChain2.step( emgBL.read() ); //Notch+High-pass
Joost38H 0:33f422404c9f 81 emgBLrectified = fabs( emgBLfiltered ); //Rectification
Joost38H 0:33f422404c9f 82 emgBLcomplete = bqLow2.step( emgBLrectified ); //Low-pass
Joost38H 0:33f422404c9f 83
Joost38H 0:33f422404c9f 84 /* Set the sampled emg values in channel 0 (the first channel) and 1
Joost38H 0:33f422404c9f 85 (the second channel) in the 'HIDScope' instance named 'scope' */
Joost38H 0:33f422404c9f 86
Joost38H 0:33f422404c9f 87 scope.set(1, emgBLcomplete );
Joost38H 0:33f422404c9f 88 scope.set(0, emgBRcomplete );
Joost38H 0:33f422404c9f 89
Joost38H 0:33f422404c9f 90
Joost38H 1:4d7097e583e0 91 /* Finally, send all channels to the PC at once */
Joost38H 0:33f422404c9f 92 scope.send();
Joost38H 0:33f422404c9f 93 /* To indicate that the function is working, the LED is toggled */
Joost38H 0:33f422404c9f 94 led_B = !led_B;
Joost38H 2:dac640cad05e 95
Joost38H 2:dac640cad05e 96
Joost38H 0:33f422404c9f 97 }
Joost38H 1:4d7097e583e0 98
Joost38H 2:dac640cad05e 99 double numsamples = 500;
Joost38H 2:dac640cad05e 100 double emgBRarray[500];
Joost38H 2:dac640cad05e 101 double emgBRsum;
Joost38H 2:dac640cad05e 102 double emgBRmeanMVC;
Joost38H 2:dac640cad05e 103 double thresholdBR;
Joost38H 2:dac640cad05e 104
Joost38H 2:dac640cad05e 105 double getThresholdBR()
Joost38H 2:dac640cad05e 106 {
Joost38H 2:dac640cad05e 107 for (int i=0; i<numsamples; i++) {
Joost38H 2:dac640cad05e 108 emgBRarray[i] = emgBRcomplete;
Joost38H 2:dac640cad05e 109 emgBRsum = emgBRsum + emgBRarray[i];
Joost38H 2:dac640cad05e 110 }
Joost38H 2:dac640cad05e 111
Joost38H 2:dac640cad05e 112 emgBRmeanMVC = emgBRsum / numsamples;
Joost38H 2:dac640cad05e 113
Joost38H 2:dac640cad05e 114 thresholdBR = emgBRmeanMVC * 0.3;
Joost38H 2:dac640cad05e 115 return thresholdBR;
Joost38H 2:dac640cad05e 116 }
Joost38H 2:dac640cad05e 117
Joost38H 2:dac640cad05e 118 double getThresholdBL()
Joost38H 2:dac640cad05e 119 {
Joost38H 2:dac640cad05e 120 double numsamples = 500;
Joost38H 2:dac640cad05e 121 double emgBLarray[500];
Joost38H 2:dac640cad05e 122 double emgBLsum;
Joost38H 2:dac640cad05e 123 for (int i=0; i<numsamples; i++) {
Joost38H 2:dac640cad05e 124 emgBLarray[i] = emgBLcomplete;
Joost38H 2:dac640cad05e 125 emgBLsum = emgBLsum + emgBLarray[i];
Joost38H 2:dac640cad05e 126 }
Joost38H 2:dac640cad05e 127
Joost38H 2:dac640cad05e 128 double emgBLmeanMVC = emgBLsum / numsamples;
Joost38H 2:dac640cad05e 129
Joost38H 2:dac640cad05e 130 double thresholdBL = emgBLmeanMVC * 0.3;
Joost38H 2:dac640cad05e 131 return thresholdBL;
Joost38H 2:dac640cad05e 132 }
Joost38H 2:dac640cad05e 133
Joost38H 1:4d7097e583e0 134
Joost38H 1:4d7097e583e0 135 // Function to make the BiQuadChain for the Notch and High pass filter for all three filters
Joost38H 0:33f422404c9f 136 void getbqChain()
Joost38H 0:33f422404c9f 137 {
Joost38H 0:33f422404c9f 138 bqChain1.add(&bqNotch1).add(&bqHigh1); //Making the BiQuadChain
Joost38H 0:33f422404c9f 139 bqChain2.add(&bqNotch2).add(&bqHigh2);
Joost38H 0:33f422404c9f 140 }
Joost38H 0:33f422404c9f 141
Joost38H 1:4d7097e583e0 142
Joost38H 1:4d7097e583e0 143 // Function to check if the threshold is met, with LED feedback for the user
Joost38H 0:33f422404c9f 144 void ThresholdReached()
Joost38H 0:33f422404c9f 145 {
Joost38H 2:dac640cad05e 146 if (emgBRcomplete > thresholdBR)
Joost38H 0:33f422404c9f 147 {
Joost38H 0:33f422404c9f 148 led_G = 0;
Joost38H 0:33f422404c9f 149 led_R = 1;
Joost38H 0:33f422404c9f 150 led_B = 1;
Joost38H 0:33f422404c9f 151 }
Joost38H 1:4d7097e583e0 152
Joost38H 1:4d7097e583e0 153 else if (emgBLcomplete > threshold)
Joost38H 1:4d7097e583e0 154 {
Joost38H 1:4d7097e583e0 155 led_G = 1;
Joost38H 1:4d7097e583e0 156 led_R = 0;
Joost38H 1:4d7097e583e0 157 led_B = 1;
Joost38H 1:4d7097e583e0 158 }
Joost38H 1:4d7097e583e0 159
Joost38H 0:33f422404c9f 160
Joost38H 0:33f422404c9f 161 else {
Joost38H 0:33f422404c9f 162 led_G = 1;
Joost38H 0:33f422404c9f 163 led_R = 1;
Joost38H 0:33f422404c9f 164 led_B = 0;
Joost38H 0:33f422404c9f 165 }
Joost38H 0:33f422404c9f 166
Joost38H 0:33f422404c9f 167 }
Joost38H 2:dac640cad05e 168
Joost38H 0:33f422404c9f 169
Joost38H 0:33f422404c9f 170 int main()
Joost38H 0:33f422404c9f 171 {
Joost38H 0:33f422404c9f 172 /*Attach the 'sample' function to the timer 'sample_timer'.
Joost38H 0:33f422404c9f 173 this ensures that 'sample' is executed every 0.002 seconds, to get a
Joost38H 0:33f422404c9f 174 sample frequency of 500 Hz
Joost38H 0:33f422404c9f 175 */
Joost38H 2:dac640cad05e 176 pc.baud(115200);
Joost38H 0:33f422404c9f 177 getbqChain();
Joost38H 0:33f422404c9f 178 sample_timer.attach(&EMG_sample, 0.002);
Joost38H 2:dac640cad05e 179 thresholdBR = getThresholdBR();
Joost38H 2:dac640cad05e 180 pc.printf("Threshold is %f.2\n", threshold);
Joost38H 0:33f422404c9f 181 threshold_timer.attach(&ThresholdReached, 0.002);
Joost38H 0:33f422404c9f 182
Joost38H 1:4d7097e583e0 183
Joost38H 0:33f422404c9f 184 while(1) {}
Joost38H 0:33f422404c9f 185 }