emg.cpp

00001 #include "mbed.h"
00002 #include "HIDScope.h"
00003 #include "emg.h"
00004 
00005 //Define objects
00006 AnalogIn    emg(A0); //Analog input
00007 Ticker      sample_timer;
00008 HIDScope    scope(2);
00009 
00010 double highV[4];
00011 double lowV[4];
00012 // An array which stores the last MOV_AVG_NUM outputs
00013 double lastOutputs[MOV_AVG_NUM-1];
00014 // The sum of the last MOV_AVG_NUM outputs
00015 double outputSum;
00016 
00017 double filter(double input, double coeff_input[], double coeff_output[], double prev_outputs[])
00018 {
00019     double new_input = input;
00020     for(int i=1; i<5; i++) 
00021         new_input -= coeff_input[i] * prev_outputs[i-1];
00022 
00023     double new_output = coeff_output[0] * new_input;
00024     for(int i=1; i<5; i++) 
00025         new_output += coeff_output[i] * prev_outputs[i-1];
00026 
00027     // Set the new output as the first value of the 'recent outputs'
00028     for(int i = 3; i > 0; i--)
00029         prev_outputs[i] = prev_outputs[i-1];
00030     prev_outputs[0] = new_input;
00031     return new_output;
00032 }
00033 
00034 double fh_b[]= {0.7602, -3.0406, 4.5609, -3.0406, 0.7602};
00035 double fh_a[]= {1, -3.4532, 4.5041, -2.6273, 0.5778};
00036 double highpass_filter(double u)
00037 {
00038     return filter(u, fh_a, fh_b, highV);
00039 }
00040 
00041 double fl_b[]= {0.00000658, 0.00002631, 0.00003947, 0.00002631, 0.00000658};
00042 double fl_a[]= {1.0000, -3.7264, 5.2160, -3.2500, 0.7605};
00043 double lowpass_filter(double u)
00044 {
00045     return filter(u, fl_a, fl_b, lowV);
00046 }
00047 
00048 void sample()
00049 {
00050     double input = emg.read();
00051     double output1 = highpass_filter(input);
00052     double output2 = fabs(output1);
00053     double output3 = lowpass_filter(output2);
00054     
00055     /* Calculate the average of the last MOV_AVG_NUM outputs and the new input */
00056     outputSum = outputSum - lastOutputs[MOV_AVG_NUM-2] + output3;
00057     for(int i=0; i<MOV_AVG_NUM-1; i++) {
00058         if(i != 0) lastOutputs[i] = lastOutputs[i-1];
00059     }
00060     lastOutputs[0] = output3;
00061     output3 = outputSum/MOV_AVG_NUM;
00062     
00063     /* Second, set the sampled emg value in channel zero (the first channel) in the 'HIDScope' variable named 'scope' */
00064     scope.set(0,input);
00065     scope.set(1,output3);
00066     /* Repeat the step above if required for more channels (channel 0 up to 5 = 6 channels) */
00067     /* Finally, send all channels to the PC at once */
00068     scope.send();
00069 }
00070 
00071 /*
00072 int main()
00073 {
00074     //Attach the 'sample' function to the timer 'sample_timer'. this ensures that 'sample' is executed every... 0.002 seconds
00075 
00076     sample_timer.attach(&sample, 0.002);
00077 
00078     //empty loop, sample() is executed periodically
00079     while(1) {}
00080 }*/