emg zooi er op
Dependencies: HIDScope MODSERIAL QEI mbed
Fork of Wearealltogheter_goed by
main.cpp
- Committer:
- Frostworks
- Date:
- 2016-10-24
- Revision:
- 22:ad85b8acf8b5
- Parent:
- 21:2b55d53e11f6
- Child:
- 23:fdde3e4b9e69
File content as of revision 22:ad85b8acf8b5:
#include "mbed.h" #include "HIDScope.h" #include "MODSERIAL.h" //Define objects AnalogIn emg0( A0 ); AnalogIn emg1( A1 ); DigitalIn buttonCalibrate(SW2); MODSERIAL pc(USBTX, USBRX); volatile float x; volatile float x_prev =0; volatile float b; // filtered 'output' of ReadAnalogInAndFilter bool calibrate = false; double threshold_Left = 0; double threshold_Right= 0; Ticker sample_timer; HIDScope scope( 2 ); DigitalOut led(LED1); const double a1 = -1.6475; const double a2 = 0.7009; const double b0 = 0.8371; const double b1 = -1.6742; const double b2 = 0.8371; const double c1 = -1.9645; const double c2 = 0.9651; const double d0 = 0.0001551; const double d1 = 0.0003103; const double d2 = 0.0001551; double v1_high = 0; double v2_high = 0; double v1_low = 0; double v2_low = 0; double highpassFilterLeft = 0; double lowpassFilterLeft = 0; double highpassFilterRight = 0; double lowpassFilterRight = 0; double biquad1(double u, double&v1, double&v2, const double a1, const double a2, const double b0, const double b1, const double b2) { double v = u - a1*v1 - a2*v2; double y = b0*v + b1*v1 + b2*v2; v2 = v1; v1 = v; return y; } /*double biquad2(double u, double&v1, double&v2, const double c1, const double c2, const double d0, const double d1, const double d2) { double v = u - c1*v1 - c2*v2; double y = d0*v + d1*v1 + d2*v2; v2 = v1; v1 = v; return y; } */ /** Sample function * this function samples the emg and sends it to HIDScope **/ void filterSampleLeft() { highpassFilterLeft = fabs(biquad1(emg0.read(), v1_high, v2_high, a1, a2, b0, b1, b2)); lowpassFilterLeft = biquad1(highpassFilterLeft, v1_low, v2_low, c1, c2, d0, d1, d2); scope.set(0, lowpassFilterLeft ); scope.send(); //pc.printf("%f \n \r ", lowpassFilter); } void filterSampleRight() { highpassFilterRight = fabs(biquad1(emg1.read(), v1_high, v2_high, a1, a2, b0, b1, b2)); lowpassFilterRight = biquad1(highpassFilterRight, v1_low, v2_low, c1, c2, d0, d1, d2); scope.set(1, lowpassFilterRight ); scope.send(); //pc.printf("%f \n \r ", lowpassFilter); } void sample() { /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */ scope.set(0, emg0.read() ); scope.set(1, emg1.read() ); /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) * Ensure that enough channels are available (HIDScope scope( 2 )) * Finally, send all channels to the PC at once */ x = emg0; // Capture data scope.set(0, x); // store data in first element of scope memory b = (x_prev + x)/2.0; // averaging filter x_prev = x; // Prepare for next round scope.send(); /* To indicate that the function is working, the LED is toggled */ led = !led; pc.printf("%f, %f \n \r ", x, b); } int main() { /**Attach the 'sample' function to the timer 'sample_timer'. * this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz */ //sample_timer.attach(&sample, 0.001953125); sample_timer.attach(&filterSampleLeft, 0.001953125); //512 Hz sample_timer.attach(&filterSampleRight, 0.001953125); pc.baud(115200); pc.printf("please push the button to calibrate \n \r"); while (1) { if (buttonCalibrate == 0) { calibrate = true; threshold_Left = lowpassFilterLeft*0.7; threshold_Right = lowpassFilterRight*0.7; } if (calibrate == true) { pc.printf("calibration complete, left = %f, right = %f \n \r", threshold_Left, threshold_Right); /*empty loop, sample() is executed periodically*/ } } }