Diff: main.cpp

Revision:

23:e5db011bd410

Parent:

22:68ab712b62b2

Child:

24:26659f1de039

--- a/main.cpp	Wed Nov 02 12:55:41 2016 +0000
+++ b/main.cpp	Thu Nov 03 12:17:16 2016 +0000
@@ -4,68 +4,185 @@
 #include "MODSERIAL.h"
 
 //Define objects
-AnalogIn    emg1_in( A0 );
-AnalogIn    emg2_in( A1 );
+    AnalogIn    emg1_in( A0 );
+    AnalogIn    emg2_in( A1 );
+    AnalogIn    emg3_in( A2 );
+    DigitalIn   max_reader1( SW2 );
+    DigitalIn   max_reader3( SW3 );
+
 
-Ticker      sample_timer;
-HIDScope    scope( 3 );
-DigitalOut  red(LED_RED);
-DigitalOut  blue(LED_BLUE);
-DigitalOut  green(LED_GREEN);
-DigitalOut  emg1_out( D8 );
-DigitalOut  emg2_out( D9 );
-MODSERIAL   pc(USBTX, USBRX);
+    Ticker      main_timer;
+    Ticker      max_read1;
+    Ticker      max_read3;
+    HIDScope    scope( 5 );
+    DigitalOut  red(LED_RED);
+    DigitalOut  blue(LED_BLUE);
+    DigitalOut  green(LED_GREEN);
+    MODSERIAL   pc(USBTX, USBRX);
 
 
 // EMG variables
-double emg1;
-double emg1highfilter;
-double emg1notchfilter;
-double emg1abs;
-double emg1lowfilter;
-double emgpeak;
-
-double emg2;
-double emg2highfilter;
-double emg2notchfilter;
-double emg2abs;
-double emg2lowfilter;
+    //Right Biceps
+    double emg1;
+    double emg1highfilter;
+    double emg1notchfilter;
+    double emg1abs;
+    double emg1lowfilter;
+    double emg1peak;
+    // Left Biceps
+    double emg2;
+    double emg2highfilter;
+    double emg2notchfilter;
+    double emg2abs;
+    double emg2lowfilter;
+    double emg2peak;
+    double max1;
+    double maxpart1;
+    // Left Lower Arm OR Triceps
+    double emg3;
+    double emg3highfilter;
+    double emg3notchfilter;
+    double emg3abs;
+    double emg3lowfilter;
+    double emg3peak;
+    double max3;
+    double maxpart3;
 
 // Filter settings
-BiQuad filterhigh(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01);
-BiQuad filternotch(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01);
-BiQuad filterlow(1.439e-02,2.794e-02,1.397e-02,-1.7229,7.788e-01);
-BiQuad filterpeak(1.0878,-1.950,8.771e-01,-1.95032,9.5499e-01);
-
-// Filtering
-void filter() {
-    emg1 = emg1_in.read();
-    emg1highfilter = filterhigh.step(emg1);
-    emg1notchfilter = filternotch.step(emg1highfilter);
-    emg1abs = fabs(emg1notchfilter);
-    emg1lowfilter = filterlow.step(emg1abs);
-    emgpeak = filterpeak.step(emg1lowfilter);
+    // Right Biceps
+    BiQuad filterhigh1(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01); /* Filter at 10 Hz */
+    BiQuad filternotch1(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01); /* Filter at 50 Hz */
+    BiQuad filterlow1(2.119e-03,4.238e-03,2.119e-03,-1.9016,9.101e-01); /* Filter at 15 Hz */
+    BiQuad filterpeak1(1.0123,-1.983,9.761e-01,-1.9838,9.8855e-01); /* Gain peak at 11 Hz */
+    // Left Biceps
+    BiQuad filterhigh2(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01);
+    BiQuad filternotch2(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01);
+    BiQuad filterlow3(2.119e-03,4.238e-03,2.119e-03,-1.9016,9.101e-01);
+    BiQuad filterpeak2(1.0123,-1.983,9.761e-01,-1.9838,9.8855e-01);
+    // Left Lower Arm OR Triceps
+    BiQuad filterhigh3(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01);
+    BiQuad filternotch3(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01);
+    BiQuad filterlow3(2.119e-03,4.238e-03,2.119e-03,-1.9016,9.101e-01);
+    BiQuad filterpeak3(1.0123,-1.983,9.761e-01,-1.9838,9.8855e-01);
+    //
 
-    emg2 = emg2_in.read();
-    emg2highfilter = filterhigh.step(emg2);
-    emg2notchfilter = filternotch.step(emg2highfilter);
-    emg2abs = fabs(emg2notchfilter);
-    emg2lowfilter = filterlow.step(emg2abs);
+// Finding max values for correct motor switch
+void get_max1(){
+    if (max_reader1==0){
+            green = !green;
+            red = 1;
+            blue = 1;
+            for(int n=0;n<2000;n++){
+                        
+            emg1 = emg1_in.read();
+            emg1highfilter = filterhigh1.step(emg1);
+            emg1notchfilter = filternotch1.step(emg1highfilter);
+            emg1abs = fabs(emg1notchfilter);
+            emg1lowfilter = filterlow1.step(emg1abs);
+            emg1peak = filterpeak1.step(emg1lowfilter);
+            
+            if (max1<emg1peak){
+                max1 = emg1peak;
+            }
+            wait(0.001f);    
+            }
+            wait(0.2f);
+            green = 1;
+    }
+    maxpart1 = 0.35*max1;
+}
 
-    /* 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, emg1_in.read() );
-    scope.set(1, emg1lowfilter );
-    scope.set(2, emgpeak );
-    /*  Finally, send all channels to the PC at once */
-    scope.send();
-    /* To indicate that the function is working, the LED is toggled */
-    green = !green;
+void get_max3(){
+    if (max_reader3==0){
+            green = 1;
+            blue = 1;
+            red = !red;
+            for(int n=0;n<2000;n++){
+                        
+            emg3 = emg3_in.read();
+            emg3highfilter = filterhigh3.step(emg3);
+            emg3notchfilter = filternotch3.step(emg3highfilter);
+            emg3abs = fabs(emg3notchfilter);
+            emg3lowfilter = filterlow3.step(emg3abs);
+            emg3peak = filterpeak3.step(emg3lowfilter);
+            
+            if (max3<emg3peak){
+                max3 = emg3peak;
+            }
+            wait(0.001f);    
+            }
+            wait(0.2f);
+            red = 1;
+    }
+    maxpart3 = 0.3*max3;
 }
 
-int main()
-{   
+// Filtering & Scope
+void filter() {
+    // Right Biceps
+    emg1 = emg1_in.read();
+    emg1highfilter = filterhigh1.step(emg1);
+    emg1notchfilter = filternotch1.step(emg1highfilter);
+    emg1abs = fabs(emg1notchfilter);
+    emg1lowfilter = filterlow1.step(emg1abs);
+    emg1peak = filterpeak1.step(emg1lowfilter);
+    // Left Biceps
+    emg2 = emg2_in.read();
+    emg2highfilter = filterhigh2.step(emg2);
+    emg2notchfilter = filternotch2.step(emg2highfilter);
+    emg2abs = fabs(emg2notchfilter);
+    emg2lowfilter = filterlow2.step(emg2abs);
+    emg2peak = filterpeak2.step(emg2lowfilter);
+    // Left Lower Arm OR Triceps
+    emg3 = emg3_in.read();
+    emg3highfilter = filterhigh3.step(emg3);
+    emg3notchfilter = filternotch3.step(emg3highfilter);
+    emg3abs = fabs(emg3notchfilter);
+    emg3lowfilter = filterlow3.step(emg3abs);
+    emg3peak = filterpeak3.step(emg3lowfilter);
+    
+    
+    /* Compare measurement to the calibrated value to decide actions */
+    if (maxpart1<emg1peak){
+        red = 0;
+        blue = 1;
+        green = 1;
+        }
+    else {
+        if (maxpart1<emg2peak){
+            red = 1;
+            blue = 0;
+            green = 1;
+            }
+        
+    else {
+        if (maxpart3<emg2peak){
+            red = 1;
+            blue = 1;
+            green = 0;
+            }
+        
+    else {
+        red = 1;
+        blue = 1;
+        green = 1;
+        }
+        }
+        }
+    /* 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 );
+    scope.set(1, emg2peak );
+    scope.set(2, maxpart1 );
+    scope.set(3, emg3peak );
+    scope.set(4, maxpart3 );    
+ 
+    scope.send();
+}
 
-    sample_timer.attach(&filter, 0.001);
+int main(){   
 
+    main_timer.attach(&filter, 0.001);
+    max_read1.attach(&get_max1, 2);
+    max_read3.attach(&get_max3, 2);
     while(1) {}
 }
\ No newline at end of file