Diff: main.cpp

Revision:

4:fcada70891c5

Parent:

3:faed8b7f6542

Child:

5:51a28834cd5b

--- a/main.cpp	Thu Oct 27 08:14:12 2016 +0000
+++ b/main.cpp	Thu Oct 27 14:55:11 2016 +0000
@@ -5,44 +5,93 @@
 #include <math.h>
 
 MODSERIAL   pc(USBTX, USBRX);
-Ticker      sampleTicker;
-Ticker      goTicker;
-AnalogIn    emg0(A0);
-AnalogIn    emg1(A1);
+Ticker      sampleTicker;//Ticker that measures the EMG signal every 0,002 seconds and filters it
+Ticker      goTicker; //Ticker that checks if the signal threshold is reached each time a new filtered EMG signal comes in and determines if the motors are going to rotate
+AnalogIn    emgl(A1);//Labels are attached to the olimex shields the left tricep should obviously be connected to the should with an L label on it
+AnalogIn    emgr(A0);
 DigitalOut  ledG(LED_GREEN);
 DigitalOut  ledB(LED_BLUE);
 DigitalOut  ledR(LED_RED);
-HIDScope    scope(2);
+HIDScope    scope(2); //scope has two ports for the two EMG signals
+
+/*coefficients of each filter
+    lno = left tricep notch filter
+    lhf = left tricep high pass filter
+    llf = left tricep lowpass filter
+    same goes for rno etc.
+    */
+double lno_b0 = 0.9911;     
+double lno_b1 = -1.6036;   
+double lno_b2 = 0.9911;    
+double lno_a1 = -1.603;    
+double lno_a2 = 0.9822;    
+
+double rno_b0 = 0.9911;     
+double rno_b1 = -1.6036;   
+double rno_b2 = 0.9911;    
+double rno_a1 = -1.603;    
+double rno_a2 = 0.9822;    
 
-double no_b0 = 0.9911;
-double no_b1 = -1.6036;
-double no_b2 = 0.9911;
-double no_a1 = -1.6036;
-double no_a2 = 0.9822;
+
+double lhf_b0 = 0.9355;    
+double lhf_b1 = -1.8711;   
+double lhf_b2 = 0.9355;    
+double lhf_a1 = -1.8669;   
+double lhf_a2 = 0.8752;   
 
-double hf_b0 = 0.9355;
-double hf_b1 = -1.8711;
-double hf_b2 = 0.9355;
-double hf_a1 = -1.8669;
-double hf_a2 = 0.8752;
+double rhf_b0 = 0.9355;    
+double rhf_b1 = -1.8711;   
+double rhf_b2 = 0.9355;    
+double rhf_a1 = -1.8669;   
+double rhf_a2 = 0.8752;    
+
+
+double llf_b0 = 8.7656e-5;
+double llf_b1 = 1.17531e-4;
+double llf_b2 = 8.7656e-5; 
+double llf_a1 = -1.9733;  
+double llf_a2 = 0.9737;    
 
-double lf_b0 = 8.7656e-5;
-double lf_b1 = 1.17531e-4;
-double lf_b2 = 8.7656e-5;
-double lf_a1 = -1.9733;
-double lf_a2 = 0.9737;
+double rlf_b0 = 8.7656e-5;
+double rlf_b1 = 1.17531e-4;
+double rlf_b2 = 8.7656e-5; 
+double rlf_a1 = -1.9733;  
+double rlf_a2 = 0.9737;    
+
+
+//starting values of the biquads of the corresponding filters
+double lno_v1 = 0, lno_v2 = 0;
+double lhf_v1 = 0, lhf_v2 = 0;
+double llf_v1 = 0, llf_v2 = 0;
+
+double rno_v1 = 0, rno_v2 = 0;
+double rhf_v1 = 0, rhf_v2 = 0;
+double rlf_v1 = 0, rlf_v2 = 0;
 
-double no_v1 = 0, no_v2 = 0;
-double hf_v1 = 0, hf_v2 = 0;
-double lf_v1 = 0, lf_v2 = 0;
-double no_y;
-double lf_y;
-double hf_y;
-double rect_y;
-int go = 0;
-const double threshold_value = 0.13;
+/* declaration of the outputs of each biquad.
+the output of the previous biquad is the input for the next biquad.
+so lno_y goes into lhf_y etc.
+*/ 
+double lno_y;
+double lhf_y;
+double llf_y;
+double lrect_y;
+double rno_y;
+double rhf_y;
+double rlf_y;
+double rrect_y;
 
-double biquad_no(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
+// set the threshold value for the filtered signal
+//if the signal exceeds this value the motors will start to rotate 
+const double threshold_value = 0.05;
+
+/* declaration of each biquad
+The coefficients will be filled in later on in void scopeSend
+As said before the input of each biquad is the output of the previous one 
+The input of the first biquad is the raw EMG signal and the output of the last biquad is the filtered signal.
+This is done for both left and right so this makes two chains of 3 biquads */
+
+double biquad_lno(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
     const double b1 , const double b2 )
 {
     double v = u - a1*v1 - a2*v2;
@@ -52,7 +101,7 @@
     return y;
 }
 
-double biquad_hf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
+double biquad_lhf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
     const double b1 , const double b2 )
 {
     double v = u - a1*v1 - a2*v2;
@@ -62,7 +111,36 @@
     return y;
 }
 
-double biquad_lf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
+double biquad_llf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
+    const double b1 , const double b2 )
+{
+    double v = u - a1*v1 - a2*v2;
+    double y = b0*v + b1*v1 + b2*v2;
+    v2 = v1;
+    v1 = v;
+    return y;
+}
+double biquad_rno(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
+    const double b1 , const double b2 )
+{
+    double v = u - a1*v1 - a2*v2;
+    double y = b0*v + b1*v1 + b2*v2;
+    v2 = v1;
+    v1 = v;
+    return y;
+}
+
+double biquad_rhf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
+    const double b1 , const double b2 )
+{
+    double v = u - a1*v1 - a2*v2;
+    double y = b0*v + b1*v1 + b2*v2;
+    v2 = v1;
+    v1 = v;
+    return y;
+}
+
+double biquad_rlf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
     const double b1 , const double b2 )
 {
     double v = u - a1*v1 - a2*v2;
@@ -72,46 +150,68 @@
     return y;
 }
 
+/* function that calculates the filtered EMG signal from the raw EMG signal. 
+So 2 chains of 3 biquads each are calculating the left and the right filtered EMG signal.
+After this is calculated, the signals are sent to HIDscope (scope.send) to see what they look like.
+The filtered left signal (llf_y) is shown in channel 1, the filtered right signal (rlf_y)is shown in channel 0 (scope.set)*/
 void scopeSend(void){
-    no_y = biquad_no(emg0.read(), no_v1, no_v2, no_a1, no_a2, no_b0, no_b1, no_b2);
-    hf_y = biquad_hf(no_y, hf_v1, hf_v2, hf_a1, hf_a2, hf_b0, hf_b1, hf_b2);
-    rect_y = fabs(hf_y);
-    lf_y = biquad_lf(rect_y, lf_v1, lf_v2, lf_a1, lf_a2, lf_b0, lf_b1, lf_b2)/0.2;  
-    scope.set(0, emg0.read());
-    scope.set(1, lf_y);
+    lno_y = biquad_lno(emgl.read(), lno_v1, lno_v2, lno_a1, lno_a2, lno_b0, lno_b1, lno_b2);
+    lhf_y = biquad_lhf(lno_y, lhf_v1, lhf_v2, lhf_a1, lhf_a2, lhf_b0, lhf_b1, lhf_b2);
+    lrect_y = fabs(lhf_y);
+    llf_y = biquad_llf(lrect_y, llf_v1, llf_v2, llf_a1, llf_a2, llf_b0, llf_b1, llf_b2)/0.2;
+    rno_y = biquad_rno(emgr.read(), rno_v1, rno_v2, rno_a1, rno_a2, rno_b0, rno_b1, rno_b2);
+    rhf_y = biquad_rhf(rno_y, rhf_v1, rhf_v2, rhf_a1, rhf_a2, rhf_b0, rhf_b1, rhf_b2);
+    rrect_y = fabs(rhf_y);
+    rlf_y = biquad_rlf(rrect_y, rlf_v1, rlf_v2, rlf_a1, rlf_a2, rlf_b0, rlf_b1, rlf_b2)/0.2;  
+    scope.set(1, llf_y);
+    scope.set(0, rlf_y);
     scope.send();
     
     }
-    
-void threshold(double lf_y, const double threshold_value){
-   if (lf_y > threshold_value){
-   go = !go
-   }
-   /* if (lf_y > threshold_value){
-        ledB = !ledB;
+   
+   //function that compares the filtered EMG signal to the set threshold and determines what colour the led should be
+   //This led is for feedback purposes only and should obviously be replaced by the motors 
+void threshold(){
+  //If the right signal exceeds the threshold, the led should turn blue  
+  if (rlf_y > threshold_value){
+        ledB = 0;
+        ledR = 1;
+        ledG = 1;
         }
-    else{
-        ledB = ledB;
+    //If the left signal exceeds the threshold, the led should turn red
+    else if (llf_y > threshold_value){
+            ledB = 1;
+            ledR = 0;
+            ledG = 1;
             }
+    // If both signals exceed the threshold, the led should turn green         
+    else if (rlf_y&&llf_y > threshold_value){
+            ledB = 1;
+            ledR = 1;
+            ledG = 0;       
+            }
+    //If no signal exceeds the threshold, the led should be off
+    else {
+        ledB=1;
+        ledG=1;
+        ledR=1;
         }
-*/
+    }
+
 
 int main(){
+    
+    
    
-  /**Attach the 'sample' function to the timer 'sample_timer'.
-    * this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz
-    */
-   // emgSampleTicker.attach(&sample, 0.002);
-    // empty loop, sample() is executed periodically
-    
+  /*Attach the 'sample' function to the timer 'sample_timer'.
+    this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz
+    emgSampleTicker.attach(&sample, 0.002);
+    empty loop, sample() is executed periodically
+    The same goes for the goTicker. It checks if the threshold is reached at the same rate new a new EMG signal comes in*/
     sampleTicker.attach(scopeSend,0.002);
     goTicker.attach(threshold,0.002);
     
     while(1) {
-        if (go == 0){
-            ledB = ledB;
-            }
-            else{
-                ledB = !ledB;
-    }
+ 
+}
 }
\ No newline at end of file