Revision 27:22bfc75f8d1a, committed 2018-11-01

Comitter:

EvaKrolis

Date:

Thu Nov 01 11:17:44 2018 +0000

Parent:

26:efd04dec7710

Child:

28:8bf22ccd11cc

Commit message:

Should work; Not been tested

Changed in this revision

--- a/main.cpp	Thu Nov 01 11:00:13 2018 +0000
+++ b/main.cpp	Thu Nov 01 11:17:44 2018 +0000
@@ -75,24 +75,26 @@
 float Threshold1 = 0;                                           //Threshold for the second EMG signal
 
 //Filter variables
-BiQuad Notch50_0(0.7887,0,0.7887,0,0.5774);                     //Make Notch filter around 50 Hz
-BiQuad Notch50_1(0.7887,0,0.7887,0,0.5774);                     //Make Notch filter around 50 Hz
-BiQuad High0(0.8006,-1.6012,0.8006,-1.561,0.6414);              //Make high-pass filter
-BiQuad High1(0.8006,-1.6012,0.8006,-1.561,0.6414);              //Make high-pass filter
+BiQuad Notch50_0(0.9049,-1.4641,0.9049,-1.4641,0.8098);         //Make Notch filter around 50 Hz
+BiQuad Notch50_1(0.9049,-1.4641,0.9049,-1.4641,0.8098);         //Make Notch filter around 50 Hz
+BiQuad Notch100_0(0.8427,-0.5097,0.8247,-0.5097,0.6494);        //Make Notch filter around 100 Hz
+BiQuad Notch100_1(0.8427,-0.5097,0.8247,-0.5097,0.6494);        //Make Notch filter around 100 Hz
+BiQuad Notch150_0(0.7548,0.4665,0.7544,0.4665,0.5095);          //Make Notch filter around 150 Hz
+BiQuad Notch150_1(0.7548,0.4665,0.7544,0.4665,0.5095);          //Make Notch filter around 150 Hz
+BiQuad Notch200_0(0.6919,1.1196,0.6919,1.1196,0.3839);          //Make Notch filter around 200 Hz
+BiQuad Notch200_1(0.6919,1.1196,0.6919,1.1196,0.3839);          //Make Notch filter around 200 Hz
+BiQuad High_0(0.9150,-1.8299,0.9150,-1.8227,0.8372);            //Make high-pass filter
+BiQuad High_1(0.9150,-1.8299,0.9150,-1.8227,0.8372);            //Make high-pass filter
+BiQuad Low_0(0.6389,1.2779,0.6389,1.143,0.4128);                //Make low-pass filter
+BiQuad Low_1(0.6389,1.2779,0.6389,1.143,0.4128);                //Make low-pass filter
 BiQuadChain filter0;                                            //Make chain of filters for the first EMG signal
 BiQuadChain filter1;                                            //Make chain of filters for the second EMG signal
 
 //Timers and Tickers
-Ticker kin;                                                     //Timer for calculating x,y,theta1,theta4
-Ticker simulateval;                                             //Timer that prints the values for x,y, and angles
-Ticker ReadUseEMG0_timer;                                       //Timer to read, filter and use the EMG
-Ticker EMGCalibration0_timer;                                   //Timer for the calibration of the EMG
 Ticker FindMax0_timer;                                          //Timer for finding the max muscle
-Ticker ReadUseEMG1_timer;                                       //Timer to read, filter and use the EMG
-Ticker EMGCalibration1_timer;                                   //Timer for the calibration of the EMG
 Ticker FindMax1_timer;                                          //Timer for finding the max muscle
-Ticker SwitchState_timer;                                       //Timer to switch from the Calibration to the working mode
 Timer local_timer;
+Ticker hoofdticker;
 
 //Bool for movement
 bool xMove = false;                                             //Bool for the x-movement
@@ -123,10 +125,10 @@
     EMG0Average = (float)Sum0/Parts;                            //Divide the sum by 50
 
     if (EMG0Average > Threshold0) {                             //If the value is higher than the threshold value
-        //redled = 0;                                             //Turn the LED on
+        redled = 0;                                             //Turn the LED on
         xMove = true;                                           //Set movement to true
     } else {
-        //redled = 1;                                             //Otherwise turn the LED off
+        redled = 1;                                             //Otherwise turn the LED off
         xMove = false;                                          //Otherwise set movement to false
     }
 }
@@ -300,7 +302,6 @@
 //State Machine
 void StateMachine()
 {
-    redled  = 1;
     switch(CurrentState) {                                      //Determine in which state you are
 
         case Calibration:
@@ -310,8 +311,7 @@
                 StateBool = false;                              //Set the bool for the start of a state to false
                 FindMax0_timer.attach(&FindMax0,15);            //Find the maximum value after 15 seconds
                 FindMax1_timer.attach(&FindMax1,15);            //Find the maximum value after 15 seconds
-                //SwitchState_timer.attach(&SwitchState,20);      //Switch to the next state after 16 seconds
-                local_timer.reset();
+                //local_timer.reset();
                 local_timer.start();
                 blueled = 0;
             }
@@ -322,7 +322,6 @@
             if (local_timer.read() > 20) {                              //If the bool is changed
                 CurrentState = WorkingMode;                     //Change the state to the working mode
                 StateBool = true;                               //Set the start of a state bool to true
-                //SwitchStateBool = false;                        //Set the switch bool to false
             }
             break;
 
@@ -330,10 +329,6 @@
             if (StateBool) {                                    //If you start to go in this state
                 pc.printf("You are know in the working mode. \r\n");    //Print in which mode you are
                 StateBool = false;                              //Set the start of state bool to true
-                //ReadUseEMG0_timer.attach(&ReadUseEMG0, 0.005);  //Start the use of EMG
-                //ReadUseEMG1_timer.attach(&ReadUseEMG1,0.005);   //Start the use of EMG
-                //kin.attach(kinematics, 0.005); // roep de ticker aan (
-                //simulateval.attach(printvalue, 0.1);
             }
             blueled = 1;
 
@@ -346,27 +341,21 @@
             //pc.printf("First EMG: %f, Second EMG: %f \n\r",EMG0Average,EMG1Average);
             break;
     }
-    redled = 0;
 }
 
 int main()
 {
-    Ticker hoofdticker;
-    //Initial conditions
-    theta1 = PI*0.49;                                           //Angle of the left motor
-    theta4 = PI*0.49;
     FK(xend,yend,theta1,theta4);
     pc.baud(115200);
     greenled = 1;                                               //First turn the LEDs off
     blueled = 1;
     redled = 1;
-    filter0.add(&Notch50_0).add(&High0);                        //Make filter chain for the first EMG
-    filter1.add(&Notch50_1).add(&High1);                        //Make filter chain for the second EMG
+    filter0.add(&Notch50_0).add(&Notch100_0).add(&Notch150_0).add(&Notch200_0).add(&Low_0).add(&High_0);                        //Make filter chain for the first EMG
+    filter1.add(&Notch50_1).add(&Notch100_1).add(&Notch150_1).add(&Notch200_1).add(&Low_1).add(&High_1);                        //Make filter chain for the second EMG
     button.rise(StopProgram);                                   //If the button is pressed, stop program
     hoofdticker.attach(&StateMachine,0.002);
     while(true) {
         printvalue();
         wait(0.75);
-        //Start the state machine
     }
 }
\ No newline at end of file