main.cpp

00001 #include "mbed.h"
00002 #include "HIDScope.h"
00003 #include "MODSERIAL.h"
00004 
00005 //Define objects
00006 AnalogIn    emg0( A0 );
00007 AnalogIn    emg1( A1 );
00008 DigitalIn   buttonCalibrate(SW2);
00009 MODSERIAL pc(USBTX, USBRX);
00010 
00011 volatile float x;
00012 volatile float x_prev =0;
00013 volatile float b; // filtered 'output' of ReadAnalogInAndFilter
00014 
00015 bool calibrate = false;
00016 double threshold_Left = 0;
00017 double threshold_Right= 0;
00018 Ticker      sample_timer;
00019 HIDScope    scope( 2 );
00020 DigitalOut  led(LED1);
00021 const double a1 = -1.6475;
00022 const double a2 = 0.7009;
00023 const double b0 = 0.8371;
00024 const double b1 = -1.6742;
00025 const double b2 = 0.8371;
00026 const double c1 = -1.9645;
00027 const double c2 = 0.9651;
00028 const double d0 = 0.0001551;
00029 const double d1 = 0.0003103;
00030 const double d2 = 0.0001551;
00031 double v1_high = 0;
00032 double v2_high = 0;
00033 double v1_low = 0;
00034 double v2_low = 0;
00035 double highpassFilterLeft = 0;
00036 double lowpassFilterLeft = 0;
00037 double highpassFilterRight = 0;
00038 double lowpassFilterRight = 0;
00039 
00040 double biquad1(double u, double&v1, double&v2, const double a1, const double a2, const double b0,
00041                const double b1, const double b2)
00042 {
00043     double v = u - a1*v1 - a2*v2;
00044     double y = b0*v + b1*v1 + b2*v2;
00045     v2 = v1;
00046     v1 = v;
00047     return y;
00048 }
00049 /*double biquad2(double u, double&v1, double&v2, const double c1, const double c2, const double d0,
00050                const double d1, const double d2)
00051 {
00052     double v = u - c1*v1 - c2*v2;
00053     double y = d0*v + d1*v1 + d2*v2;
00054     v2 = v1;
00055     v1 = v;
00056     return y;
00057 }
00058 */
00059 /** Sample function
00060  * this function samples the emg and sends it to HIDScope
00061  **/
00062 
00063 void filterSampleLeft()
00064 {
00065     highpassFilterLeft = fabs(biquad1(emg0.read(), v1_high, v2_high, a1, a2, b0, b1, b2));
00066     lowpassFilterLeft = biquad1(highpassFilterLeft, v1_low, v2_low, c1, c2, d0, d1, d2);
00067     scope.set(0, lowpassFilterLeft );
00068     scope.send();
00069     //pc.printf("%f \n \r ", lowpassFilter);
00070 }
00071 void filterSampleRight()
00072 {
00073     highpassFilterRight = fabs(biquad1(emg1.read(), v1_high, v2_high, a1, a2, b0, b1, b2));
00074     lowpassFilterRight = biquad1(highpassFilterRight, v1_low, v2_low, c1, c2, d0, d1, d2);
00075     scope.set(1, lowpassFilterRight );
00076     scope.send();
00077     //pc.printf("%f \n \r ", lowpassFilter);
00078 }
00079 void sample()
00080 {
00081     /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
00082     scope.set(0, emg0.read() );
00083     scope.set(1, emg1.read() );
00084     /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
00085     *  Ensure that enough channels are available (HIDScope scope( 2 ))
00086     *  Finally, send all channels to the PC at once */
00087 
00088     x = emg0;   // Capture data        scope.set(0, x);   // store data in first element of scope memory
00089     b = (x_prev + x)/2.0;   // averaging filter
00090     x_prev = x; // Prepare for next round
00091 
00092     scope.send();
00093     /* To indicate that the function is working, the LED is toggled */
00094     led = !led;
00095     pc.printf("%f, %f \n \r ", x, b);
00096 }
00097 
00098 int main()
00099 {
00100     /**Attach the 'sample' function to the timer 'sample_timer'.
00101     * this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz
00102     */
00103     //sample_timer.attach(&sample, 0.001953125);
00104     sample_timer.attach(&filterSampleLeft, 0.001953125);        //512 Hz
00105     sample_timer.attach(&filterSampleRight, 0.001953125);
00106     pc.baud(115200);
00107     pc.printf("please push the button to calibrate \n \r");
00108     while (1) {
00109         if (buttonCalibrate == 0) {
00110             calibrate = true;
00111             threshold_Left = lowpassFilterLeft*0.7;
00112             threshold_Right = lowpassFilterRight*0.7;
00113         }
00114         if (calibrate == true) {
00115             pc.printf("calibration complete, left = %f, right = %f \n \r", threshold_Left, threshold_Right);
00116             /*empty loop, sample() is executed periodically*/
00117         }
00118     }
00119 }