Diff: main.cpp

Revision:

20:17a0c462caf0

Parent:

19:2a74dd8dc4fa

--- a/main.cpp	Tue Nov 01 14:23:12 2016 +0000
+++ b/main.cpp	Wed Nov 02 08:34:55 2016 +0000
@@ -32,13 +32,13 @@
 InterruptIn Switch_3(D2);       //Switch 3 to control the translation of the arm
 InterruptIn Switch_4(D3);       //Switch 4 to control the gripper
 
-DigitalOut  red(LED_RED);       //Led for calibration_biceps (RED)
-DigitalOut  blue(LED_BLUE);     //Led for calibration_triceps (BLUE)
+DigitalOut  red(LED_RED);       //LED for calibration_biceps (red)
+DigitalOut  blue(LED_BLUE);     //LED for calibration_triceps (blue)
 
 AnalogIn    emg_1(A0);          //Analog of EMG 1    
 AnalogIn    emg_2(A1);          //Analog of EMG 2
 AnalogIn    emg_3(A2);          //Analog of EMG 3
-//AnalogIn    emg_4(A3);        //Analog of EMG 4
+//AnalogIn    emg_4(A3);          //Analog of EMG 4
 
 double emg_1_value = 0;         //Initially the emg_1 value is zero
 double emg_2_value = 0;         //Initially the emg_2 value is zero
@@ -73,26 +73,22 @@
 
 //Create Biquad filters for the filtering of emg_1
 BiQuad highpass1(0.9565, -1.9131, 0.9565, -1.9112, 0.9150);             //Create a highpass filter to remove low-frequent noise
-BiQuad notch_low1(1.0000, -1.9023, 1.0000, -1.8795, 0.9819);            //Create a notch-filter to remove 50Hz noise
-BiQuad notch_high1(1.0000, -1.9023, 1.0000, -1.8913, 0.9829);
+BiQuad notch1(0.9938, -1.8902, 0.9938, -1.8902, 0.9875);                //Create a notch-filter to remove 50Hz noise
 BiQuad lowpass1(0.00003913, 0.00007826, 0.00003913, -1.9822, 0.9824);   //Create a lowpass filter to envelope the signal
 
 //Create Biquad filters for the filtering of emg_2
 BiQuad highpass2(0.9565, -1.9131, 0.9565, -1.9112, 0.9150);             //Create a highpass filter to remove low-frequent noise
-BiQuad notch_low2(1.0000, -1.9023, 1.0000, -1.8795, 0.9819);            //Create a notch-filter to remove 50Hz noise
-BiQuad notch_high2(1.0000, -1.9023, 1.0000, -1.8913, 0.9829);
+BiQuad notch2(0.9938, -1.8902, 0.9938, -1.8902, 0.9875);                //Create a notch-filter to remove 50Hz noise
 BiQuad lowpass2(0.00003913, 0.00007826, 0.00003913, -1.9822, 0.9824);   //Create a lowpass filter to envelope the signal
 
 //Create Biquad filters for the filtering of emg_3
 BiQuad highpass3(0.9565, -1.9131, 0.9565, -1.9112, 0.9150);             //Create a highpass filter to remove low-frequent noise
-BiQuad notch_low3(1.0000, -1.9023, 1.0000, -1.8795, 0.9819);            //Create a notch-filter to remove 50Hz noise
-BiQuad notch_high3(1.0000, -1.9023, 1.0000, -1.8913, 0.9829);
+BiQuad notch3(0.9938, -1.8902, 0.9938, -1.8902, 0.9875);                //Create a notch-filter to remove 50Hz noise
 BiQuad lowpass3(0.00003913, 0.00007826, 0.00003913, -1.9822, 0.9824);   //Create a lowpass filter to envelope the signal
 
-//Create Biquad filters for the filtering of emg_4
+////Create Biquad filters for the filtering of emg_4
 //BiQuad highpass4(0.9565, -1.9131, 0.9565, -1.9112, 0.9150);             //Create a highpass filter to remove low-frequent noise
-//BiQuad notch_low4(1.0000, -1.9023, 1.0000, -1.8795, 0.9819);            //Create a notch-filter to remove 50Hz noise
-//BiQuad notch_high4(1.0000, -1.9023, 1.0000, -1.8913, 0.9829);
+//BiQuad notch4(0.9938, -1.8902, 0.9938, -1.8902, 0.9875);                //Create a notch-filter to remove 50Hz noise
 //BiQuad lowpass4(0.00003913, 0.00007826, 0.00003913, -1.9822, 0.9824);   //Create a lowpass filter to envelope the signal
 
 int counter_rotation_left=0;    //To count the number of times the rotation_left switch (switch_1) has been pushed
@@ -239,20 +235,18 @@
     if(Switch_1.read()== 0) {                                       //If Switch_1 is pressed
         for(int n=0; n<3000; n++){                                  //For 3000 samples
             signalpart1 = highpass1.step(emg_1.read());             //Filter the signal with the highpass filter
-            signalpart2 = notch_low1.step(signalpart1);             //Filter the signal with the notch filter
-            signalpart3 = notch_high1.step(signalpart2);
-            signalpart4 = fabs(signalpart3);                        //Rectify the signal
-            emg_1_filtered = lowpass1.step(signalpart4);            //Filter the signal with the lowpass filter
+            signalpart2 = notch1.step(signalpart1);                 //Filter the signal with the notch filter
+            signalpart3 = fabs(signalpart2);                        //Rectify the signal
+            emg_1_filtered = lowpass1.step(signalpart3);            //Filter the signal with the lowpass filter
             if (emg_1_filtered > maximum_calibration_value_1) {     //If the measured value is higher than the maximum value
                 maximum_calibration_value_1 = emg_1_filtered;       //The measured value is the new maximum value
             }
             emg_1_threshold = maximum_calibration_value_1*0.7;      //Set the threshold for emg 1
             
             signalpart1 = highpass2.step(emg_2.read());             //Filter the signal with the highpass filter
-            signalpart2 = notch_low2.step(signalpart1);             //Filter the signal with the notch filter
-            signalpart3 = notch_high2.step(signalpart2);
-            signalpart4 = fabs(signalpart3);                        //Rectify the signal
-            emg_2_filtered = lowpass2.step(signalpart4);            //Filter the signal with the lowpass filter
+            signalpart2 = notch2.step(signalpart1);                 //Filter the signal with the notch filter
+            signalpart3 = fabs(signalpart2);                        //Rectify the signal
+            emg_2_filtered = lowpass2.step(signalpart3);            //Filter the signal with the lowpass filter
             if (emg_2_filtered > maximum_calibration_value_2) {     //If the measured value is higher than the maximum value
                 maximum_calibration_value_2 = emg_2_filtered;       //The measured value is the new maximum value
             }
@@ -268,25 +262,23 @@
     if(Switch_2.read()== 0) {                                       //If Switch_2 is pressed
         for(int n=0; n<3000; n++){                                  //For 3000 samples
             signalpart1 = highpass3.step(emg_3.read());             //Filter the signal with the highpass filter
-            signalpart2 = notch_low3.step(signalpart1);             //Filter the signal with the notch filter
-            signalpart3 = notch_high3.step(signalpart2);
-            signalpart4 = fabs(signalpart3);                        //Rectify the signal
-            emg_3_filtered = lowpass3.step(signalpart4);            //Filter the signal with the lowpass filter
+            signalpart2 = notch3.step(signalpart1);                 //Filter the signal with the notch filter
+            signalpart3 = fabs(signalpart2);                        //Rectify the signal
+            emg_3_filtered = lowpass3.step(signalpart3);            //Filter the signal with the lowpass filter
             if (emg_3_filtered > maximum_calibration_value_3) {     //If the measured value is higher than the maximum value
                 maximum_calibration_value_3 = emg_3_filtered;       //The measured value is the new maximum value
             }
             emg_3_threshold = maximum_calibration_value_3*0.4;      //Set the threshold for emg 3
             
 //            signalpart1 = highpass4.step(emg_4.read());             //Filter the signal with the highpass filter
-//            signalpart2 = notch_low4.step(signalpart1);             //Filter the signal with the notch filter
-//            signalpart3 = notch_high4.step(signalpart2);
-//            signalpart4 = fabs(signalpart3);                        //Rectify the signal
-//            emg_4_filtered = lowpass4.step(signalpart4);            //Filter the signal with the lowpass filter
+//            signalpart2 = notch4.step(signalpart1);                 //Filter the signal with the notch filter
+//            signalpart3 = fabs(signalpart2);                        //Rectify the signal
+//            emg_4_filtered = lowpass4.step(signalpart3);            //Filter the signal with the lowpass filter
 //            if (emg_4_filtered > maximum_calibration_value_4) {     //If the measured value is higher than the maximum value
 //                maximum_calibration_value_4 = emg_4_filtered;       //The measured value is the new maximum value
 //            }
 //            emg_4_threshold = maximum_calibration_value_4*0.4;      //Set the threshold for emg 4
-//        }
+        }
         
         blue = 0;                                                   //Switch the blue light on to indicate the triceps calibration is done
         calibration_triceps_done=1;                                 //Declare that the triceps calibration is done
@@ -296,31 +288,27 @@
 void filter_emg(){                                                      //Function to filter the emg signals
     if(calibration_biceps_done==1 && calibration_triceps_done==1) {     //If both calibrations are done
         signalpart1 = highpass1.step(emg_1.read());                     //Filter the signal with the highpass filter
-        signalpart2 = notch_low1.step(signalpart1);                     //Filter the signal with the notch filter
-        signalpart3 = notch_high1.step(signalpart2);
-        signalpart4 = fabs(signalpart3);                                //Rectify the signal
-        emg_1_filtered = lowpass1.step(signalpart4);                    //Filter the signal with the lowpass filter
+        signalpart2 = notch1.step(signalpart1);                         //Filter the signal with the notch filter
+        signalpart3 = fabs(signalpart2);                                //Rectify the signal
+        emg_1_filtered = lowpass1.step(signalpart3);                    //Filter the signal with the lowpass filter
 //        pc.printf("%f \n", emg_1_filtered);  
   
         signalpart1 = highpass2.step(emg_2.read());                     //Filter the signal with the highpass filter
-        signalpart2 = notch_low2.step(signalpart1);                     //Filter the signal with the notch filter
-        signalpart3 = notch_high2step(signalpart2);
-        signalpart4 = fabs(signalpart3);                                //Rectify the signal
-        emg_2_filtered = lowpass2.step(signalpart4);                    //Filter the signal with the lowpass filter
+        signalpart2 = notch2.step(signalpart1);                         //Filter the signal with the notch filter
+        signalpart3 = fabs(signalpart2);                                //Rectify the signal
+        emg_2_filtered = lowpass2.step(signalpart3);                    //Filter the signal with the lowpass filter
 //        pc.printf("%f \n", emg_2_filtered);  
 
         signalpart1 = highpass3.step(emg_1.read());                     //Filter the signal with the highpass filter
-        signalpart2 = notch_low3.step(signalpart1);                     //Filter the signal with the notch filter
-        signalpart3 = notch_high3.step(signalpart2);
-        signalpart4 = fabs(signalpart3);                                //Rectify the signal
-        emg_3_filtered = lowpass3.step(signalpart4);                    //Filter the signal with the lowpass filter
+        signalpart2 = notch3.step(signalpart1);                         //Filter the signal with the notch filter
+        signalpart3 = fabs(signalpart2);                                //Rectify the signal
+        emg_3_filtered = lowpass3.step(signalpart3);                    //Filter the signal with the lowpass filter
 //        pc.printf("%f \n", emg_3_filtered);  
         
 //        signalpart1 = highpass4.step(emg_1.read());                     //Filter the signal with the highpass filter
 //        signalpart2 = notch_low4.step(signalpart1);                     //Filter the signal with the notch filter
-//        signalpart3 = notch_high4.step(signalpart2);
-//        signalpart4 = fabs(signalpart3);                                //Rectify the signal
-//        emg_4_filtered = lowpass4.step(signalpart4);                    //Filter the signal with the lowpass filter
+//        signalpart3 = fabs(signalpart2);                                //Rectify the signal
+//        emg_4_filtered = lowpass4.step(signalpart3);                    //Filter the signal with the lowpass filter
 ////        pc.printf("%f \n", emg_4_filtered);  
         
 //        scope.set(0,emg_1_filtered);          //Plot the emg_1_filtered in the first scope of HIDScope
@@ -332,7 +320,7 @@
 }
 
 void check_threshold_crossing (){               //Function to check if the emg thresholds are crossed
-    if(calibration_rotation_done==1 && calibration_translation_gripper_done==1) {       //If both calibrations are done
+    if(calibration_biceps_done==1 && calibration_triceps_done==1) {       //If both calibrations are done
         if(emg_1_filtered >= emg_1_threshold && emg_1_activated == 0) {                 //If the filtered emg 1 signal is above the threshold value and if emg_1 is not activated yet 
             emg_1_activated = 1;                                                        //Declare that emg_1 is activated
             activate_rotation_left();                                                   //Execute the activate_rotation_left function