main.cpp

00001 #include "mbed.h"
00002 #include "HIDScope.h"
00003 #include "BiQuad.h"
00004 #include "MovingAverage.h"
00005 #include "MODSERIAL.h"
00006 #include "math.h"
00007 #define NSAMPLE 200
00008 
00009 
00010 //Define objects
00011 AnalogIn    emg0( A0 ); //Biceps Left
00012 AnalogIn    emg1( A1 ); //Biceps Right
00013 InterruptIn button(PTC6);
00014 InterruptIn button2(PTA4);
00015 Ticker      sample_timer;
00016 
00017 //Ticker      driving_timer;
00018 HIDScope    scope( 4 ); //Number of Channels
00019 DigitalOut  led1(LED_GREEN);
00020 DigitalOut  led2(LED_BLUE);
00021 DigitalOut  led3(LED_RED);
00022 MovingAverage <double>Movag_LB(NSAMPLE,0.0); //Make Moving Average, Define NSAMPLE above 
00023 MovingAverage <double>Movag_RB(NSAMPLE,0.0);
00024 
00025 MODSERIAL pc(USBTX, USBRX);
00026 
00027 volatile bool BicepLeft = false;
00028 volatile bool BicepRight = false;
00029 
00030 
00031 //Make Biquad filters Left(a0, a1, a2, b1, b2) 
00032 BiQuadChain bqc1; //chain voor High Pass en Notch
00033 BiQuad bq1(0.39131200825291007, -0.7826240165058201, 0.39131200825291007, -0.36950493743858204, 0.1957430955730582); //High Pass Filter
00034 BiQuad bq2(9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01); //Notch Filter
00035 BiQuad bq3(0.00044924972004654657, 0.0008984994400930931, 0.00044924972004654657, -1.6169385656964415, 0.6187355645766276); //Low Pass Filter
00036 
00037 //Make Biquad filters Right
00038 BiQuadChain bqc2; //chain voor High Pass en Notch
00039 BiQuad bq4(0.39131200825291007, -0.7826240165058201, 0.39131200825291007, -0.36950493743858204, 0.1957430955730582); //High Pass Filter
00040 BiQuad bq5(9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01); //Notch Filter
00041 
00042 //Function to read samples and filter and send to HIDScope
00043 void sample()
00044 {
00045     //Filter Left Bicep
00046     double LB_Filter_1 = bqc1.step(emg0.read()); //High Pass + Notch
00047     double LB_Rectify = fabs(LB_Filter_1); //Rectify Signal
00048     //double Filtered_emg_1 = bq3.step(LB_Rectify); //Low Pass 
00049     Movag_LB.Insert(LB_Rectify);
00050     double LB_Out = Movag_LB.GetAverage();
00051     
00052     //Filter Right Bicep
00053     double RB_Filter_1 = bqc2.step(emg1.read()); //High Pass + Notch
00054     double RB_Rectify = fabs(RB_Filter_1); //Rectify Signal
00055     //double Filtered_emg_1 = bq3.step(RB_Rectify); //Low Pass 
00056     Movag_RB.Insert(RB_Rectify);
00057     double RB_Out = Movag_RB.GetAverage();
00058     
00059     scope.set(0, emg0.read() ); //Raw EMG signal left bicep
00060     scope.set(1, LB_Out); // Filtered EMG signal left bicep
00061     scope.set(2, emg1.read()); //Raw EMG signal  right bicep
00062     scope.set(3, RB_Out); //Filtered EMG signal right bicep
00063 
00064     scope.send();
00065 
00066     if (LB_Out > ThresholdLeft) {
00067         led1 = 0;
00068         BicepLeft = true;
00069         }
00070     else {
00071         led1 = 1;
00072         BicepLeft = false;
00073         }
00074     if (RB_Out > ThresholdRight) {
00075         led2 = 0;
00076         BicepRight = true;
00077         }
00078     else {
00079         led2 = 1;
00080         BicepRight = false;
00081         }
00082 }
00083 
00084 
00085 int main()
00086 {   
00087     bqc1.add( &bq1 ).add( &bq2 ); //make BiQuadChain
00088     bqc2.add( &bq4 ).add( &bq5 ); //make BiQuadChain
00089     sample_timer.attach(&sample, 0.002); //read samples at 500 Hz
00090     while(1) {}
00091 }