Diff: main.cpp

Revision:

13:746240466172

Parent:

12:35a81d6c6505

Child:

14:63f2a5165ffd

--- a/main.cpp	Wed Oct 19 11:54:55 2016 +0000
+++ b/main.cpp	Mon Oct 24 11:06:13 2016 +0000
@@ -1,7 +1,21 @@
-#include "mbed.h"
-#include "MODSERIAL.h"
-#include "Servo.h"
-#include "QEI.h"
+#include "mbed.h"           //Include the mbed library
+#include "MODSERIAL.h"      //Include the MODSERIAL library for communication with the pc
+#include "Servo.h"          //Include the Servo library for controlling the gripper
+#include "QEI.h"            //Include the QEI library for reading the encoder data of the DC-motors
+//#include "HIDScope.h"       //Include the HIDScope library for plotting the emg data
+#include "BiQuad.h"         //Include the BiQuad library for filtering the emg signal
+
+double emg_1_threshold = 1.5;           //Set the threshold for emg 1
+double emg_2_threshold = 1.5;           //Set the threshold for emg 2
+double emg_3_threshold = 1.5;           //Set the threshold for emg 3
+double emg_4_threshold = 1.5;           //Set the threshold for emg 4
+
+const double pi = 3.1415926535897;      //Declare the value of pi
+
+double speed_rotation=pi/5;             //Set the rotation speed in rad/sec -> NOTE: this has to be below 8.4 rad/sec
+double speed_translation=pi/5;          //Set the translation speed in rad/sec -> NOTE: this has to be below 8.4 rad/sec
+double speedM1=speed_rotation/8.4;      //Map the rotation speed from (0-8.4) to (0-1) by dividing by 8.4
+double speedM2=speed_translation/8.4;   //Map the translation speed from (0-8.4) to (0-1) by dividing by 8.4
 
 QEI encoder_M1 (D9, D10, NC, 8400);     //Define an encoder for motor 1 called encoder_M1
 QEI encoder_M2 (D11, D12, NC, 8400);    //Define an encoder for motor 2 called encoder_M2
@@ -15,43 +29,80 @@
 DigitalOut Direction_M1(D7);    //To control the translation speed of the arm
 Servo gripper_servo(D13);       //To control the gripper
 
-InterruptIn Switch_1(SW2);      //Switch 1 to control the rotation to the left
-InterruptIn Switch_2(SW3);      //Switch 2 to control the rotation to the right
-InterruptIn Switch_3(D2);       //Switch 3 to control the translation of the arm
-InterruptIn Switch_4(D3);       //Switch 4 to control the gripper
+InterruptIn Switch_1(SW3);  //Switch 1 to control the rotation to the left
+InterruptIn Switch_2(SW2);  //Switch 2 to control the rotation to the right
+InterruptIn Switch_3(D2);   //Switch 3 to control the translation of the arm
+InterruptIn Switch_4(D3);   //Switch 4 to control the gripper
+
+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
+
+double emg_1_value = 0;     //Initially the emg_1 value is zero
+double emg_2_value = 0;     //Initially the emg_2 value is zero
+double emg_3_value = 0;     //Initially the emg_3 value is zero
+double emg_4_value = 0;     //Initially the emg_4 value is zero
+
+double emg_1_filtered = 0;   //Initially the emg_1_filtered signal is zero
+double emg_2_filtered = 0;   //Initially the emg_2_filtered signal is zero
+double emg_3_filtered = 0;   //Initially the emg_3_filtered signal is zero
+double emg_4_filtered = 0;   //Initially the emg_4_filtered signal is zero
+
+Ticker      filter_EMG_ticker;                  //Create a ticker for the filtering of all emg signals
+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
+
+BiQuad highpass (1, -2, 1, -0.08609387372, 0.58927051568);                //Filter to remove all signal data above 120 Hz
+BiQuad bandpass (1,  2, 1, -1.89549506712, 0.91194493918);                //Filter to remove all signal data outside 10-50 Hz region
+BiQuad bandstop (1, -1.61816176147, 1, -1.58071559235, 0.97319685401);    //Filter to remove 50 Hz noise
+BiQuadChain bqc;                //Create a biquad chain to merge all biquad filters
 
 int counter_rotation_left=0;    //To count the number of times the rotation_left switch (switch_1) has been pushed
 int counter_rotation_right=0;   //To count the number of times the rotation_right switch (switch_2) has been pushed
 int counter_translation=0;      //To count the number of times the translation switch (switch_3) has been pushed
 int counter_gripper=0;          //To count the number of times the gripper switch (switch_4) has been pushed
 
+bool emg_1_activated = false;   //Initially the emg_1 has not crossed the threshold
+bool emg_2_activated = false;   //Initially the emg_2 has not crossed the threshold
+bool emg_3_activated = false;   //Initially the emg_3 has not crossed the threshold
+bool emg_4_activated = false;   //Initially the emg_4 has not crossed the threshold
+
 volatile bool rotation_left_go = false;     //Create a go-flag for the rotation_left and set it to false
 volatile bool rotation_right_go = false;    //Create a go-flag for the rotation_right and set it to false
 volatile bool translation_go = false;       //Create a go-flag for the translation and set it to false
 volatile bool gripper_go = false;           //Create a go-flag for the gripper and set it to false
 
-MODSERIAL pc(USBTX, USBRX);             //Make a connection with the PC
+MODSERIAL   pc(USBTX, USBRX);             //Make a connection with the PC
+//HIDScope    scope(4);                     //Create a 4-channel HIDScope object
 
-const double pi = 3.1415926535897;      //Declare the value of pi
+float angle_M1=0;       //The measured angle of motor 1 is initially zero
+float angle_M2=0;       //The measured angle of motor 2 is initially zero
 
-double speed_rotation=pi/5;             //Set the rotation speed in rad/sec -> NOTE: this has to be below 8.4 rad/sec
-double speed_translation=pi/5;          //Set the translation speed in rad/sec -> NOTE: this has to be below 8.4 rad/sec
-double speedM1=speed_rotation/8.4;      //Map the rotation speed from (0-8.4) to (0-1) by dividing by 8.4
-double speedM2=speed_translation/8.4;   //Map the translation speed from (0-8.4) to (0-1) by dividing by 8.4
+void filter_emg (){
+    emg_1_filtered = bqc.step(emg_1.read());                //Read the emg 1 data and apply the Biquad Chain to filter the data
+    pc.printf("%f \n",emg_1_filtered);
+    emg_2_filtered = bqc.step(emg_2.read());                //Read the emg 2 data and apply the Biquad Chain to filter the data
+    emg_3_filtered = bqc.step(emg_3.read());                //Read the emg 3 data and apply the Biquad Chain to filter the data
+    emg_4_filtered = bqc.step(emg_4.read());                //Read the emg 4 data and apply the Biquad Chain to filter the data
 
-float angle_M1=0;
-float angle_M2=0;
-
-void read_position_M1(){                            //Function to read the position of motor 1
-    int pulses_M1 = -encoder_M1.getPulses();         //Read the encoder data and store it in pulses_M1
-    angle_M1 = float(pulses_M1)/4200*2.0*pi;  //Calculate the angle that corresponds with the measured encoder pulses
-    pc.printf("%i \t%f \t", pulses_M1, angle_M1);
+//    scope.set(0,emg_1_filtered);    //set filtered emg 1 signal to scope in channel 1
+//    scope.set(1,emg_2_filtered);    //set filtered emg 2 signal to scope in channel 2
+//    scope.set(2,emg_3_filtered);    //set filtered emg 3 signal to scope in channel 3
+//    scope.set(3,emg_4_filtered);    //set filtered emg 4 signal to scope in channel 4
+//    scope.send();
 }
 
-void read_position_M2(){                            //Function to read the position of motor 2
+void read_position_M1 (){                             //Function to read the position of motor 1
+    int pulses_M1 = -encoder_M1.getPulses();         //Read the encoder data and store it in pulses_M1
+    angle_M1 = float(pulses_M1)/4200*2.0*pi;         //Calculate the angle that corresponds with the measured encoder pulses
+//    pc.printf("%i \t%f \t", pulses_M1, angle_M1);
+}
+
+void read_position_M2 (){                            //Function to read the position of motor 2
     int pulses_M2 = -encoder_M2.getPulses();         //Read the encoder data and store it in pulses_M2
     angle_M2 = float(pulses_M2)/4200*2.0*pi;  //Calculate the angle that corresponds with the measured encoder pulses
-    pc.printf("%i \t%f \n", pulses_M2, angle_M2);
+//    pc.printf("%i \t%f \n", pulses_M2, angle_M2);
 }
 
 void activate_rotation_left (){             //To activate the rotation_left
@@ -65,13 +116,13 @@
 void rotation_left (){                      //Function to control the rotation to the left
     switch (counter_rotation_left){         //Create a switch statement
         case 1:                             //For activating the rotation to the left
-            Direction_M1 = 0;               //The arm will rotate to the left  
+            Direction_M1 = 1;               //The arm will rotate to the left  
             Speed_M1 = speedM1;             //The motor is turned on at speed_rotation rad/sec
             pc.printf("The arm will now rotate to the left with %f rad/sec \n", speedM1);
             wait(0.1f);
             break;
         case 2:                             //For stopping the rotation to the left
-            Direction_M1 = 0;               //The arm will rotate to the left  
+            Direction_M1 = 1;               //The arm will rotate to the left  
             Speed_M1 = 0;                   //The motor is turned off
             pc.printf("The arm will now stop rotating to the left \n");
             wait(0.1f);
@@ -90,13 +141,13 @@
 void rotation_right (){                     //Function to control the rotation to the left
     switch (counter_rotation_right){        //Create a switch statement
         case 1:                             //For activation the rotation to the right
-            Direction_M1 = 1;               //The arm will rotate to the right 
+            Direction_M1 = 0;               //The arm will rotate to the right 
             Speed_M1 = speedM1;             //The motor is turned on at speed_rotation rad/sec
             pc.printf("The arm will now rotate to the right with %f rad/sec \n", speedM1);
             wait(0.1f);
             break;
         case 2:                              //For stopping the rotation to the right
-            Direction_M1 = 1;                //The arm will rotate to the right
+            Direction_M1 = 0;                //The arm will rotate to the right
             Speed_M1 = 0;                    //The motor is turned off
             pc.printf("The arm will now stop rotating to the right \n");
             wait(0.1f);
@@ -157,49 +208,84 @@
             wait(0.1f);
             break;
         case 2:                         //For opening the gripper
-            gripper_servo = 1;          //The gripper is now open
+            gripper_servo = 0.3;        //The gripper is now open
             pc.printf("The gripper will now open \n");
             wait(0.1f);
             break;
     }     
 }
 
-int main(){
+void check_threshold_crossing (){        //Function to check if the emg thresholds are crossed
+    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
+        activate_rotation_left();        //Execute the activate_rotation_left function
+        emg_1_activated = true;          //Declare that the activate_rotation_left function is now activated
+    } else if (emg_1_filtered <= emg_1_threshold){  //If the filtered emg 1 signal gets below the threshold value
+        emg_1_activated = false;         //The activate_rotation_left function is now deactivated and can be activated again
+    }
+//    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(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
+    if (rotation_left_go == true) {     //If the rotation_left go-flag is true
+        rotation_left_go = false;       //Set the rotation_left go-flag to false
+        rotation_left();                //Execute the rotation_left function
+    }
+    if (rotation_right_go == true) {    //If the rotation_right go-flag is true
+        rotation_right_go = false;      //Set the rotation_right go-flag to false
+        rotation_right();               //Execute the rotation_right function
+    }
+    if (translation_go == true) {       //If the translation go-flag is true
+        translation_go = false;         //Set the translation go-flag to false
+        translation();                  //Execute the translation function
+    }
+    if (gripper_go == true) {           //If the gripper go-flag is true
+        gripper_go = false;             //Set the gripper go-flag to false
+        gripper();                      //Execute the gripper function
+    }  
+}
+
+int main (){
     pc.baud(115200);                //Set the boud rate for serial communication
     pc.printf("RESET \n");          //Print "RESET"
     
     Direction_M1 = 1;               //The arm will initially get longer  
     Speed_M1 = 0;                   //The first motor is initially turned off
-    Direction_M2 = 255;             //The arm will initially turn left  
+    Direction_M2 = 1;               //The arm will initially turn left  
     Speed_M2 = 0;                   //The second motor is initially turned off
-    gripper_servo = 1;              //The gripper is initially open
+    gripper_servo = 0.3;            //The gripper is initially open
     encoder_M1.reset();             //Reset the encoder for motor 1
     encoder_M2.reset();             //Reset the encoder for motor 2
     
+    bqc.add(&highpass).add(&bandpass).add(&bandstop);       //Add all the filters to the Biquad Chain
+    
     encoder_M1_ticker.attach(&read_position_M1,0.01);   //Connect the encoder_M1_ticker to the read_position_M1 function and execute at 100Hz
     encoder_M2_ticker.attach(&read_position_M2,0.01);   //Connect the encoder_M2_ticker to the read_position_M2 function and execute at 100Hz
     
-    Switch_1.rise(&activate_rotation_left);             //Use switch_1 to activate the rotation_left go-flag
+//    Switch_1.rise(&activate_rotation_left);             //Use switch_1 to activate the counter_rotation_left go-flag
     Switch_2.rise(&activate_rotation_right);            //Use switch_2 to activate the counter_rotation_right go-flag
     Switch_3.rise(&activate_translation);               //Use switch_3 to activate the counter_translation go-flag
     Switch_4.rise(&activate_gripper);                   //Use switch_4 to activate the counter_gripper go-flag
 
-    while (true){
-        if (rotation_left_go == true) {                         //If the rotation_left go-flag is true
-            rotation_left_go = false;                       //Set the rotation_left go-flag to false
-            rotation_left();                                //Execute the rotation_left function
-        }
-        if (rotation_right_go == true) {                        //If the rotation_right go-flag is true
-            rotation_right_go = false;                      //Set the rotation_right go-flag to false
-            rotation_right();                               //Execute the rotation_right function
-        }
-        if (translation_go == true) {                           //If the translation go-flag is true
-            translation_go = false;                         //Set the translation go-flag to false
-            translation();                                  //Execute the translation function
-        }
-        if (gripper_go == true) {                               //If the gripper go-flag is true
-            gripper_go = false;                                 //Set the gripper go-flag to false
-            gripper();                                          //Execute the gripper function
-        }
-    }
+    filter_EMG_ticker.attach(&filter_emg, 0.1);        //Connect the filter_EMG_ticker to the filter_EMG funtion and execute at 100Hz
+    check_threshold_crossing_ticker.attach(&check_threshold_crossing, 0.1);    //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
+    
+    while (true){}      //Create a while loop to let the main loop run indefinitly
 }
\ No newline at end of file