Diff: main.cpp

Revision:

21:30a33a4aa868

Parent:

20:97059009a491

--- a/main.cpp	Thu Sep 22 08:53:50 2016 +0000
+++ b/main.cpp	Fri Oct 26 11:03:48 2018 +0000
@@ -1,37 +1,91 @@
 #include "mbed.h"
 #include "HIDScope.h"
+#include "BiQuad.h"
+#include "MovingAverage.h"
+#include "MODSERIAL.h"
+#include "math.h"
+#define NSAMPLE 200
+
 
 //Define objects
-AnalogIn    emg0( A0 );
-AnalogIn    emg1( A1 );
-
+AnalogIn    emg0( A0 ); //Biceps Left
+AnalogIn    emg1( A1 ); //Biceps Right
+InterruptIn button(PTC6);
+InterruptIn button2(PTA4);
 Ticker      sample_timer;
-HIDScope    scope( 2 );
-DigitalOut  led(LED1);
+
+//Ticker      driving_timer;
+HIDScope    scope( 4 ); //Number of Channels
+DigitalOut  led1(LED_GREEN);
+DigitalOut  led2(LED_BLUE);
+DigitalOut  led3(LED_RED);
+MovingAverage <double>Movag_LB(NSAMPLE,0.0); //Make Moving Average, Define NSAMPLE above 
+MovingAverage <double>Movag_RB(NSAMPLE,0.0);
+
+MODSERIAL pc(USBTX, USBRX);
+
+volatile bool BicepLeft = false;
+volatile bool BicepRight = false;
 
-/** Sample function
- * this function samples the emg and sends it to HIDScope
- **/
+
+//Make Biquad filters Left(a0, a1, a2, b1, b2) 
+BiQuadChain bqc1; //chain voor High Pass en Notch
+BiQuad bq1(0.39131200825291007, -0.7826240165058201, 0.39131200825291007, -0.36950493743858204, 0.1957430955730582); //High Pass Filter
+BiQuad bq2(9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01); //Notch Filter
+BiQuad bq3(0.00044924972004654657, 0.0008984994400930931, 0.00044924972004654657, -1.6169385656964415, 0.6187355645766276); //Low Pass Filter
+
+//Make Biquad filters Right
+BiQuadChain bqc2; //chain voor High Pass en Notch
+BiQuad bq4(0.39131200825291007, -0.7826240165058201, 0.39131200825291007, -0.36950493743858204, 0.1957430955730582); //High Pass Filter
+BiQuad bq5(9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01); //Notch Filter
+
+//Function to read samples and filter and send to HIDScope
 void sample()
 {
-    /* 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, emg0.read() );
-    scope.set(1, emg1.read() );
-    /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) 
-    *  Ensure that enough channels are available (HIDScope scope( 2 ))
-    *  Finally, send all channels to the PC at once */
+    //Filter Left Bicep
+    double LB_Filter_1 = bqc1.step(emg0.read()); //High Pass + Notch
+    double LB_Rectify = fabs(LB_Filter_1); //Rectify Signal
+    //double Filtered_emg_1 = bq3.step(LB_Rectify); //Low Pass 
+    Movag_LB.Insert(LB_Rectify);
+    double LB_Out = Movag_LB.GetAverage();
+    
+    //Filter Right Bicep
+    double RB_Filter_1 = bqc2.step(emg1.read()); //High Pass + Notch
+    double RB_Rectify = fabs(RB_Filter_1); //Rectify Signal
+    //double Filtered_emg_1 = bq3.step(RB_Rectify); //Low Pass 
+    Movag_RB.Insert(RB_Rectify);
+    double RB_Out = Movag_RB.GetAverage();
+    
+    scope.set(0, emg0.read() ); //Raw EMG signal left bicep
+    scope.set(1, LB_Out); // Filtered EMG signal left bicep
+    scope.set(2, emg1.read()); //Raw EMG signal  right bicep
+    scope.set(3, RB_Out); //Filtered EMG signal right bicep
+
     scope.send();
-    /* To indicate that the function is working, the LED is toggled */
-    led = !led;
+
+    if (LB_Out > ThresholdLeft) {
+        led1 = 0;
+        BicepLeft = true;
+        }
+    else {
+        led1 = 1;
+        BicepLeft = false;
+        }
+    if (RB_Out > ThresholdRight) {
+        led2 = 0;
+        BicepRight = true;
+        }
+    else {
+        led2 = 1;
+        BicepRight = false;
+        }
 }
 
+
 int main()
 {   
-    /**Attach the 'sample' function to the timer 'sample_timer'.
-    * this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz
-    */
-    sample_timer.attach(&sample, 0.002);
-
-    /*empty loop, sample() is executed periodically*/
+    bqc1.add( &bq1 ).add( &bq2 ); //make BiQuadChain
+    bqc2.add( &bq4 ).add( &bq5 ); //make BiQuadChain
+    sample_timer.attach(&sample, 0.002); //read samples at 500 Hz
     while(1) {}
-}
\ No newline at end of file
+}