met printstatements 1
Dependencies: HIDScope MODSERIAL PID QEI biquadFilter mbed
Fork of EMG_5 by
main.cpp
- Committer:
- relvorelvo
- Date:
- 2016-11-03
- Revision:
- 24:26659f1de039
- Parent:
- 23:e5db011bd410
- Child:
- 25:07187cf76863
File content as of revision 24:26659f1de039:
#include "mbed.h" #include "HIDScope.h" #include "BiQuad.h" #include "MODSERIAL.h" //Define objects AnalogIn emg1_in( A0 ); AnalogIn emg2_in( A1 ); AnalogIn emg3_in( A2 ); DigitalIn max_reader1( SW2 ); DigitalIn max_reader3( SW3 ); Ticker main_timer; Ticker max_read1; Ticker max_read3; HIDScope scope( 5 ); DigitalOut red(LED_RED); DigitalOut blue(LED_BLUE); DigitalOut green(LED_GREEN); MODSERIAL pc(USBTX, USBRX); // EMG variables //Right Biceps double emg1; double emg1highfilter; double emg1notchfilter; double emg1abs; double emg1lowfilter; double emg1peak; double maxpart1; // Left Biceps double emg2; double emg2highfilter; double emg2notchfilter; double emg2abs; double emg2lowfilter; double emg2peak; double max1; double maxpart2; // Left Lower Arm OR Triceps double emg3; double emg3highfilter; double emg3notchfilter; double emg3abs; double emg3lowfilter; double emg3peak; double max3; double maxpart3; // Filter settings // Right Biceps BiQuad filterhigh1(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01); /* Filter at 10 Hz */ BiQuad filternotch1(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01); /* Filter at 50 Hz */ BiQuad filterpeak1(1.0033,-1.984,9.852e-01,-1.9838,9.8855e-01); /* Gain peak at 11 Hz */ BiQuad filterlow1(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01); /* Filter at 15 Hz */ // Left Biceps BiQuad filterhigh2(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01); BiQuad filternotch2(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01); BiQuad filterpeak2(1.0033,-1.984,9.852e-01,-1.9838,9.8855e-01); BiQuad filterlow2(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01); // Left Lower Arm OR Triceps BiQuad filterhigh3(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01); BiQuad filternotch3(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01); BiQuad filterpeak3(1.0033,-1.984,9.852e-01,-1.9838,9.8855e-01); BiQuad filterlow3(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01); // // Finding max values for correct motor switch void get_max1(){ if (max_reader1==0){ green = !green; red = 1; blue = 1; for(int n=0;n<2000;n++){ emg1 = emg1_in.read(); emg1highfilter = filterhigh1.step(emg1); emg1notchfilter = filternotch1.step(emg1highfilter); emg1abs = fabs(emg1notchfilter); emg1lowfilter = filterlow1.step(emg1abs); emg1peak = filterpeak1.step(emg1lowfilter); if (max1<emg1peak){ max1 = emg1peak; } wait(0.001f); } wait(0.2f); green = 1; } maxpart1 = 0.28*max1; maxpart2 = 0.18*max1; } void get_max3(){ if (max_reader3==0){ green = 1; blue = 1; red = !red; for(int n=0;n<2000;n++){ emg3 = emg3_in.read(); emg3highfilter = filterhigh3.step(emg3); emg3notchfilter = filternotch3.step(emg3highfilter); emg3abs = fabs(emg3notchfilter); emg3lowfilter = filterlow3.step(emg3abs); emg3peak = filterpeak3.step(emg3lowfilter); if (max3<emg3peak){ max3 = emg3peak; } wait(0.001f); } wait(0.2f); red = 1; } maxpart3 = 0.28*max3; } // Filtering & Scope void filter() { // Right Biceps emg1 = emg1_in.read(); emg1highfilter = filterhigh1.step(emg1); emg1notchfilter = filternotch1.step(emg1highfilter); emg1abs = fabs(emg1notchfilter); emg1lowfilter = filterlow1.step(emg1abs); emg1peak = filterpeak1.step(emg1lowfilter); // Left Biceps emg2 = emg2_in.read(); emg2highfilter = filterhigh2.step(emg2); emg2notchfilter = filternotch2.step(emg2highfilter); emg2abs = fabs(emg2notchfilter); emg2lowfilter = filterlow2.step(emg2abs); emg2peak = filterpeak2.step(emg2lowfilter); // Left Lower Arm OR Triceps emg3 = emg3_in.read(); emg3highfilter = filterhigh3.step(emg3); emg3notchfilter = filternotch3.step(emg3highfilter); emg3abs = fabs(emg3notchfilter); emg3lowfilter = filterlow3.step(emg3abs); emg3peak = filterpeak3.step(emg3lowfilter); /* Compare measurement to the calibrated value to decide actions */ if (maxpart1<emg1peak){ red = 0; blue = 1; green = 1; } else { if (maxpart2<emg2peak){ red = 1; blue = 0; green = 1; } else { if (maxpart3<emg3peak){ red = 1; blue = 1; green = 0; } else { red = 1; blue = 1; green = 1; } } } /* 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, emg1peak ); scope.set(1, emg2peak ); scope.set(2, maxpart1 ); scope.set(3, emg3peak ); scope.set(4, maxpart3 ); scope.send(); } int main(){ main_timer.attach(&filter, 0.001); max_read1.attach(&get_max1, 2); max_read3.attach(&get_max3, 2); while(1) {} }