main.cpp

00001 #include "mbed.h"
00002 #include "HIDScope.h"
00003 
00004 #define MOV_AVG_NUM     100
00005 
00006 //Define objects
00007 AnalogIn    b_emg(A0); //Analog input bicep
00008 AnalogIn    t_emg(A1); //Analog input tricep
00009 AnalogIn    p_emg(A2); //Analog input bicep for push
00010 Ticker      sample_timer;
00011 HIDScope    scope(6);
00012 
00013 double b_highV[4];
00014 double b_lowV[4];
00015 double t_highV[4];
00016 double t_lowV[4];
00017 double p_highV[4];
00018 double p_lowV[4];
00019 
00020 //double lastOutputs[MOV_AVG_NUM-1];
00021 
00022 double filter(double input, double coeff_input[], double coeff_output[], double prev_outputs[])
00023 {
00024     double new_input = input;
00025     for(int i=1; i<5; i++) 
00026         new_input -= coeff_input[i] * prev_outputs[i-1];
00027 
00028     double new_output = coeff_output[0] * new_input;
00029     for(int i=1; i<5; i++) 
00030         new_output += coeff_output[i] * prev_outputs[i-1];
00031 
00032     // Set the new output as the first value of the 'recent outputs'
00033     for(int i = 3; i > 0; i--)
00034         prev_outputs[i] = prev_outputs[i-1];
00035     prev_outputs[0] = new_input;
00036     return new_output;
00037 }
00038 double fh_b[]= {0.7602, -3.0406, 4.5609, -3.0406, 0.7602};
00039 double fh_a[]= {1, -3.4532, 4.5041, -2.6273, 0.5778};
00040 double b_highpass_filter(double u)
00041 {
00042     return filter(u, fh_a, fh_b, t_highV);
00043     }
00044 double t_highpass_filter(double u)
00045 {
00046     return filter(u, fh_a, fh_b, b_highV);
00047 }
00048 double p_highpass_filter(double u)
00049 {
00050     return filter(u, fh_a, fh_b, p_highV);
00051 }
00052 
00053 double fl_b[]= {0.00001329,    0.00005317,    0.00007976,   0.00005317,    0.00001329};
00054 double fl_a[]= {1.0000,   -3.6717,    5.0680,   -3.1160,    0.7199};
00055 double b_lowpass_filter(double u)
00056 {
00057         return filter(u, fl_a, fl_b, t_lowV);
00058 }
00059 double t_lowpass_filter(double u)
00060 {
00061 
00062     return filter(u, fl_a, fl_b, b_lowV);
00063 }
00064 double p_lowpass_filter(double u)
00065 {
00066 
00067     return filter(u, fl_a, fl_b, p_lowV);
00068 }
00069 
00070 double last_biceps = 0;
00071 double last_triceps = 0;
00072 double last_push = 0;
00073 /** Sample function
00074  * this function samples the emg and sends it to HIDScope
00075  **/
00076 void sample()
00077 {
00078     double b_input = b_emg.read();
00079     double output1 = b_highpass_filter(b_input);
00080     double output2 = fabs(output1);
00081     double output3 = b_lowpass_filter(output2);
00082       
00083     double t_input = t_emg.read();
00084     double output4 = t_highpass_filter(t_input);
00085     double output5 = fabs(output4);
00086     double output6 = t_lowpass_filter(output5);
00087     
00088     double p_input = p_emg.read();
00089     double output7 = p_highpass_filter(p_input);
00090     double output8 = fabs(output7);
00091     double output9 = p_lowpass_filter(output8);
00092     
00093     /*double tot = output3;
00094     for(int i=0; i<MOV_AVG_NUM-1; i++) {
00095         tot += lastOutputs[i];
00096         if(i != 0) lastOutputs[i] = lastOutputs[i-1];
00097     }
00098     lastOutputs[0] = output3;
00099     output3 = tot/MOV_AVG_NUM;*/
00100     
00101     /* Second, set the sampled emg value in channel zero (the first channel) in the 'HIDScope' variable named 'scope' */
00102        
00103     double bicout;
00104     double tricout;
00105     double pushout;
00106     if (last_biceps == 0 and output3 > 0.06) {
00107         bicout = 1;
00108     } else if (last_biceps == 1 and output3 < 0.045) {
00109         bicout = 0;
00110     } else {
00111         bicout = last_biceps;   
00112     }
00113     last_biceps = bicout;
00114     
00115     /*if (output3>0.06) {//This is the output for the right bicep
00116         bicout=1;
00117     } else if(output3>0.04) {
00118         bicout=0.5;
00119     } else {
00120         bicout=0;
00121     }*/
00122     
00123     /*if (output6>0.0561) {//this is the output for the right tricep
00124         tricout=1;
00125     } else if(output6>0.0380) {
00126         tricout=0.5;
00127     } else {
00128         tricout=0;
00129     }*/
00130     
00131     if (last_triceps == 0 and output6 > 0.045) {
00132         tricout = 1;
00133     } else if(last_triceps == 1 and output6 < 0.04) {
00134         tricout = 0;    
00135     } else {
00136         tricout = last_triceps;
00137     }
00138     last_triceps = tricout;
00139     
00140     if (output9>0.05) {//this is the output for the left bicep (the push motion)
00141         pushout=1;
00142     } else {
00143         pushout=0;
00144     }
00145     
00146     scope.set(0,output3);
00147     scope.set(1,output6);
00148     scope.set(2,output9);
00149     scope.set(3,bicout);
00150     scope.set(4,tricout);
00151     scope.set(5,pushout);
00152     /* Repeat the step above if required for more channels (channel 0 up to 5 = 6 channels) */
00153     /* Finally, send all channels to the PC at once */
00154     scope.send();
00155 }
00156 
00157 int main()
00158 {
00159     /**Attach the 'sample' function to the timer 'sample_timer'.
00160     * this ensures that 'sample' is executed every... 0.002 seconds
00161     */
00162     sample_timer.attach(&sample, 0.002);
00163 
00164     /*empty loop, sample() is executed periodically*/
00165     while(1) {}
00166 }