Diff: main.cpp

Revision:

16:196abf318ea4

Parent:

15:c43f0dfe7cdf

Child:

17:4cfa7951bfa2

--- a/main.cpp	Mon Oct 31 11:17:49 2016 +0000
+++ b/main.cpp	Mon Oct 31 14:27:19 2016 +0000
@@ -54,7 +54,8 @@
 double maximum_calibration_value_2=0;
 double maximum_calibration_value_3=0;
 double maximum_calibration_value_4=0;
-bool calibration_done=0;
+bool calibration_rotation_done=0;
+bool calibration_translation_gripper_done=1;
 
 volatile double emg_1_threshold = 0.2;           //Set the threshold for emg 1
 volatile double emg_2_threshold = 0.2;           //Set the threshold for emg 2
@@ -62,7 +63,8 @@
 volatile double emg_4_threshold = 0.2;           //Set the threshold for emg 4
 
 Ticker      filter_EMG_ticker;                  //Create a ticker for the filtering of all emg signals
-Ticker      calibration_ticker;
+Ticker      calibration_rotation_ticker;
+Ticker      calibration_translation_gripper_ticker;
 Ticker      check_threshold_crossing_ticker;    //Create a ticker for checking if the threshold is crossed
 Ticker      check_goflags_ticker;               //Create a ticker for checking if the go-flags are set true
 
@@ -80,7 +82,7 @@
 //BiQuad notch_low3(1.0000, -1.9023, 1.0000, -1.8795, 0.9819);
 //BiQuad notch_high3(1.0000, -1.9023, 1.0000, -1.8913, 0.9829);
 //BiQuad lowpass3(0.00003913, 0.00007826, 0.00003913, -1.9822, 0.9824);
-
+//
 //BiQuad highpass4(0.9565, -1.9131, 0.9565, -1.9112, 0.9150);
 //BiQuad notch_low4(1.0000, -1.9023, 1.0000, -1.8795, 0.9819);
 //BiQuad notch_high4(1.0000, -1.9023, 1.0000, -1.8913, 0.9829);
@@ -225,7 +227,7 @@
             break;
     }     
 }
-void calibration(){
+void calibration_rotation(){
     if(Switch_1.read()== false) {
         for(int n=0; n<5000; n++){
             signalpart1 = highpass1.step(emg_1.read());
@@ -248,14 +250,42 @@
             }
             emg_2_threshold = maximum_calibration_value_2*0.7;           //Set the threshold for emg 2
         }
-        
-        printf("%f \t %f \n",maximum_calibration_value_1, maximum_calibration_value_2);
-        calibration_done=1;  
+       pc.printf("%f \t %f \n",maximum_calibration_value_1, maximum_calibration_value_2);
+       calibration_rotation_done=1;  
     }
 }
 
+//void calibration_translation_gripper(){      
+//    if(Switch_2.read()== false) {
+//        for(int n=0; n<5000; n++){      
+//            signalpart1 = highpass3.step(emg_3.read());
+//            signalpart2 = notch_low3.step(signalpart1);
+//            signalpart3 = notch_high3.step(signalpart2);
+//            signalpart4 = fabs(signalpart3);
+//            emg_3_filtered = lowpass3.step(signalpart4);
+//            if (emg_3_filtered > maximum_calibration_value_3) {
+//                maximum_calibration_value_3 = emg_3_filtered;
+//            }
+//            emg_3_threshold = maximum_calibration_value_3*0.7;           //Set the threshold for emg 2
+//            
+//            signalpart1 = highpass4.step(emg_4.read());
+//            signalpart2 = notch_low4.step(signalpart1);
+//            signalpart3 = notch_high4.step(signalpart2);
+//            signalpart4 = fabs(signalpart3);
+//            emg_4_filtered = lowpass4.step(signalpart4);
+//            if (emg_4_filtered > maximum_calibration_value_4) {
+//                maximum_calibration_value_4 = emg_4_filtered;
+//            }
+//            emg_4_threshold = maximum_calibration_value_4*0.7;           //Set the threshold for emg 4
+//        }
+//        
+//        printf("%f \t %f \n", maximum_calibration_value_3, maximum_calibration_value_4);
+//        calibration_translation_gripper_done=1;  
+//    }
+//}
+
 void filter_emg(){
-    if(calibration_done==1) {
+    if(calibration_rotation_done==1 && calibration_translation_gripper_done==1) {
         signalpart1 = highpass1.step(emg_1.read());
         signalpart2 = notch_low1.step(signalpart1);
         signalpart3 = notch_high1.step(signalpart2);
@@ -268,54 +298,59 @@
         signalpart3 = notch_high2.step(signalpart2);
         signalpart4 = fabs(signalpart3);
         emg_2_filtered = lowpass2.step(signalpart4);
-//        pc.printf("%f \t %f \n", emg_1_filtered, emg_2_filtered);
+//        pc.printf("%f \n", emg_2_filtered);
+
+//        signalpart1 = highpass3.step(emg_3.read());
+//        signalpart2 = notch_low3.step(signalpart1);
+//        signalpart3 = notch_high3.step(signalpart2);
+//        signalpart4 = fabs(signalpart3);
+//        emg_3_filtered = lowpass3.step(signalpart4);
+////        pc.printf("%f \n", emg_3_filtered);
+//        
+//        signalpart1 = highpass4.step(emg_2.read());
+//        signalpart2 = notch_low4.step(signalpart1);
+//        signalpart3 = notch_high4.step(signalpart2);
+//        signalpart4 = fabs(signalpart3);
+//        emg_4_filtered = lowpass4.step(signalpart4);
+////        pc.printf("%f \n", emg_4_filtered);
         
 //        scope.set(0,emg_1_filtered);
 //        scope.set(1,emg_2_filtered);
+//        scope.set(2,emg_3_filtered);
+//        scope.set(3,emg_4_filtered);
 //        scope.send();
-
-//        if(emg_1_filtered >= emg_1_threshold && emg_1_activated == false){      //If the filtered emg 1 signal is above the threshold value and if the activate_rotation_left function is not activated yet
-//            emg_1_activated = true;
-//            activate_rotation_left();        //Execute the activate_rotation_left function
-//        } else if (emg_1_filtered <= emg_1_threshold){
-//            emg_1_activated = false;
-//        }
-//        
-//        if(emg_2_filtered >= emg_2_threshold && emg_2_activated == false){      //If the filtered emg 2 signal is above the threshold value and if the activate_rotation_right function is not activated yet
-//            activate_rotation_right();       //Execute the activate_rotation_right function
-//            emg_2_activated = true;          //Declare that the activate_rotation_right function is now activated
-//        } else if (emg_2_filtered <= emg_2_threshold){  //If the filtered emg 2 signal gets below the threshold value
-//            emg_2_activated = false;         //The activate_rotation_right function is now deactivated and can be activated again
-//        }
-
     }
 }
 
 void check_threshold_crossing (){        //Function to check if the emg thresholds are crossed
-    if(emg_1_filtered >= emg_1_threshold && calibration_done==1 && emg_1_activated == false){      //If the filtered emg 1 signal is above the threshold value and if the activate_rotation_left function is not activated yet
-        emg_1_activated = true;
-        activate_rotation_left();        //Execute the activate_rotation_left function
-    } else if (emg_1_filtered <= emg_1_threshold){
-        emg_1_activated = false;
-    }
-    if(emg_2_filtered >= emg_2_threshold && emg_2_activated == false){      //If the filtered emg 2 signal is above the threshold value and if the activate_rotation_right function is not activated yet
-        activate_rotation_right();       //Execute the activate_rotation_right function
-        emg_2_activated = true;          //Declare that the activate_rotation_right function is now activated
-    } else if (emg_2_filtered <= emg_2_threshold){  //If the filtered emg 2 signal gets below the threshold value
-        emg_2_activated = false;         //The activate_rotation_right function is now deactivated and can be activated again
+    if(calibration_rotation_done==1 && calibration_translation_gripper_done==1) {
+        if(emg_1_filtered >= emg_1_threshold && emg_1_activated == false) {     //If the filtered emg 1 signal is above the threshold value and if the activate_rotation_left function is not activated yet
+            emg_1_activated = true;
+            activate_rotation_left();        //Execute the activate_rotation_left function
+            wait(0.1f);
+        } else if (emg_1_filtered <= emg_1_threshold) {
+            emg_1_activated = false;
+        }
+        if(emg_2_filtered >= emg_2_threshold && emg_2_activated == false) {     //If the filtered emg 2 signal is above the threshold value and if the activate_rotation_right function is not activated yet
+            activate_rotation_right();       //Execute the activate_rotation_right function
+            emg_2_activated = true;          //Declare that the activate_rotation_right function is now activated
+            wait(0.1f);
+        } else if (emg_2_filtered <= emg_2_threshold) { //If the filtered emg 2 signal gets below the threshold value
+            emg_2_activated = false;         //The activate_rotation_right function is now deactivated and can be activated again
+        }
+//        if(emg_3_filtered >= emg_3_threshold && emg_3_activated == false) {     //If the filtered emg 3 signal is above the threshold value and if the activate_translation function is not activated yet
+//            activate_translation();          //Execute the activate_translation function
+//            emg_3_activated = true;          //Declare that the activate_translation function is now activated
+//        } else if (emg_3_filtered <= emg_3_threshold) { //If the filtered emg 3 signal gets below the threshold value
+//            emg_3_activated = false;         //The activate_translation function is now deactivated and can be activated again
+//        }
+//        if(emg_4_filtered >= emg_4_threshold && emg_4_activated == false) {     //If the filtered emg 4 signal is above the threshold value and if the activate_gripper function is not activated yet
+//            activate_gripper();              //Execute the activate_gripper function
+//            emg_4_activated = true;          //Declare that the activate_gripper function is now activated
+//        } else if (emg_4_filtered <= emg_4_threshold) { //If the filtered emg 4 signal gets below the threshold value
+//            emg_4_activated = false;         //The activate_gripper function is now deactivated and can be activated again
+//        }
     }
-//    if(emg_3_filtered >= emg_3_threshold && emg_3_activated == false){      //If the filtered emg 3 signal is above the threshold value and if the activate_translation function is not activated yet
-//        activate_translation();          //Execute the activate_translation function
-//        emg_3_activated = true;          //Declare that the activate_translation function is now activated
-//    } else if (emg_3_filtered <= emg_3_threshold){  //If the filtered emg 3 signal gets below the threshold value
-//        emg_3_activated = false;         //The activate_translation function is now deactivated and can be activated again
-//    }
-//    if(emg_4_filtered >= emg_4_threshold && emg_4_activated == false){      //If the filtered emg 4 signal is above the threshold value and if the activate_gripper function is not activated yet
-//        activate_gripper();              //Execute the activate_gripper function
-//        emg_4_activated = true;          //Declare that the activate_gripper function is now activated
-//    } else if (emg_4_filtered <= emg_4_threshold){  //If the filtered emg 4 signal gets below the threshold value
-//        emg_4_activated = false;         //The activate_gripper function is now deactivated and can be activated again
-//    }
 }
 
 void check_goflags (){          //Function to check if the go-flags are activated
@@ -358,7 +393,8 @@
     Switch_4.rise(&activate_gripper);                   //Use switch_4 to activate the counter_gripper go-flag
 
     filter_EMG_ticker.attach(&filter_emg, 0.001);        //Connect the filter_EMG_ticker to the filter_EMG funtion and execute at 1000Hz
-    calibration_ticker.attach(&calibration, 0.001);  
+    calibration_rotation_ticker.attach(&calibration_rotation, 0.001);  
+//    calibration_translation_gripper_ticker.attach(&calibration_translation_gripper, 0.001);  
     check_threshold_crossing_ticker.attach(&check_threshold_crossing, 0.01);    //Connect the check_threshold_crossing_ticker to the check_threshold_crossing function at 100Hz
     check_goflags_ticker.attach(&check_goflags, 0.01);         //Connect the check_goflags_ticker to the check_goflags