Diff: main.cpp

Revision:

29:09c1567d6148

Parent:

28:ec0763106a2e

Child:

30:2c67abcdb892

--- a/main.cpp	Tue Oct 10 14:17:03 2017 +0000
+++ b/main.cpp	Mon Oct 30 13:51:43 2017 +0000
@@ -1,15 +1,15 @@
 #include "mbed.h"
 #include "HIDScope.h"
+#include "MODSERIAL.h"
 #include "BiQuad.h"
-#include "MODSERIAL.h"
-#include "Calibration.h"
+#include "QEI.h"
 
 //Define objects
     AnalogIn    emg1_in( A0 ); /* read out the signal */
     AnalogIn    emg2_in( A1 );
     AnalogIn    emg3_in( A2 );
     AnalogIn    emg4_in( A3 );
-    DigitalIn   max_reader12( SW2 );
+    DigitalIn   max_reader12( SW2 ); /* define button press */
     DigitalIn   max_reader34( SW3 );
 
     Ticker      main_timer;
@@ -24,6 +24,8 @@
 
 // EMG variables
     //Right Biceps
+
+
     double emg1;
     double emg1highfilter;
     double emg1notchfilter;
@@ -59,7 +61,7 @@
     double emg4peak;
     double max4;
     double maxpart4;
-
+   
 // BiQuad Filter Settings
     // Right Biceps
     BiQuad filterhigh1(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01); /* Filter at 10 Hz */
@@ -83,6 +85,43 @@
     BiQuad filterlow4(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
     //
 
+// Finding max values for correct motor switch if the button is pressed
+void get_max1(){
+    if (max_reader12==0){
+            green = !green;
+            red = 1;
+            blue = 1;
+            for(int n=0;n<2000;n++){  /* measure 2000 samples and filter it */
+
+            emg1 = emg1_in.read();      /* read out emg */
+            emg1highfilter = filterhigh1.step(emg1); /* high pass filtered */
+            emg1notchfilter = filternotch1.step(emg1highfilter); /* notch filtered */
+            emg1abs = fabs(emg1notchfilter); /* take the absolute value */
+            emg1lowfilter = filterlow1.step(emg1abs); /* low pass filtered */
+            emg1peak = filterpeak1.step(emg1lowfilter); /* 4dB gain peak */
+            
+            emg2 = emg2_in.read();      /* read out emg */
+            emg2highfilter = filterhigh2.step(emg2); /* high pass filtered */
+            emg2notchfilter = filternotch2.step(emg2highfilter); /* notch filtered */
+            emg2abs = fabs(emg2notchfilter); /* take the absolute value */
+            emg2lowfilter = filterlow2.step(emg2abs); /* low pass filtered */
+            emg2peak = filterpeak2.step(emg2lowfilter); /* 4dB gain peak */
+
+            if (max1<emg1peak){
+                max1 = emg1peak; /* set the max value at the highest measured value */
+            }
+            if (max2<emg2peak){
+                max2 = emg2peak; /* set the max value at the highest measured value */                
+            }
+            wait(0.001f); /* measure at 1000Hz */   
+            }
+            wait(0.2f);
+            green = 1;
+    }
+    maxpart1 = 0.12*max1; /* set cut off voltage at 15% of max for right biceps */
+    maxpart2 = 0.12*max2; /* set cut off votage at 15% of max for left biceps */
+}
+
 void get_max3(){
     if (max_reader34==0){
             green = 1;
@@ -115,8 +154,8 @@
             wait(0.2f);
             red = 1;
     }
-    maxpart3 = 0.2*max3; /* set cut off voltage at 25% of max for left lower arm */
-    maxpart4 = 0.2*max4; /* set cut off voltage at 25% of max for right lower arm */
+    maxpart3 = 0.18*max3; /* set cut off voltage at 25% of max for left lower arm */
+    maxpart4 = 0.18*max4; /* set cut off voltage at 25% of max for right lower arm */
 }
 
 // Filtering & Scope
@@ -148,42 +187,43 @@
     emg4notchfilter = filternotch4.step(emg4highfilter);
     emg4abs = fabs(emg4notchfilter);
     emg4lowfilter = filterlow4.step(emg4abs);
-    emg4peak = filterpeak4.step(emg4lowfilter); /* Final Lower Arm values to be sent */
-    
+    emg4peak = filterpeak4.step(emg4lowfilter); /* Final Lower Arm values to be sent */  
+
     
-    /* Compare measurement to the calibrated value to decide actions */
-    if (maxpart1<emg1peak){ /* See if right biceps is contracting */
-        red = 0;
-        blue = 1;
-        green = 1;
+ /* Compare measurement to the calibrated value to decide actions */
+ 
+    /* This part checks for right biceps contractions*/
+if (maxpart1<emg1peak && maxpart2>emg2peak && maxpart3>emg3peak && maxpart4>emg4peak){
+            red = 1;
+            blue = 1;
+            green = 0;
+}            
+    /* This part checks for left biceps contractions */   
+else if (maxpart1>emg1peak && maxpart2<emg2peak && maxpart3>emg3peak && maxpart4>emg4peak){
+            red = 0;
+            blue = 1;
+            green = 1;
         }
-    else {
-        if (maxpart2<emg2peak){ /* See if left biceps is contracting */
+    /* This part checks for left lower arm contractions */   
+else if (maxpart1>emg1peak && maxpart2>emg2peak && maxpart3<emg3peak && maxpart4>emg4peak){
             red = 1;
             blue = 0;
             green = 1;
-            }
-        
-    else {
-        if (maxpart3<emg3peak){ /* See if lower left arm is contracting */
-            red = 1;
+        }
+            
+    /* This part checks for right lower arm contractions */
+else if (maxpart1>emg1peak && maxpart2>emg2peak && maxpart3>emg3peak && maxpart4<emg4peak){
+            red = 0;
             blue = 1;
             green = 0;
-            }
-    else {
-        if (maxpart4<emg3peak){ /* See if lower right arm is contracting */
-            red = 0;
-            blue = 0;
-            green = 0;
-            }
-    else {
+        }               
+else {
         red = 1; /* Shut down all led colors if no movement is registered */
         blue = 1;
         green = 1;
-        }
-        }
-        }
-        }
+        //pc.printf( "No contraction registered\n");
+    }
+
     /* 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, emg1peak ); /* plot Right biceps voltage */
     scope.set(1, emg2peak ); /* Plot Left biceps voltage */