Diff: main.cpp

Revision:

15:38258e6b6e91

Parent:

14:e3fe54f0a4b4

Child:

16:deb42ce3c3a1

--- a/main.cpp	Wed Oct 31 18:15:10 2018 +0000
+++ b/main.cpp	Wed Oct 31 18:55:04 2018 +0000
@@ -1,4 +1,4 @@
-//Libraries 
+//Libraries
 #include "mbed.h"
 #include <math.h>
 #include <cmath>
@@ -8,8 +8,7 @@
 #define PI 3.14159265
 
 //Inputs and outputs
-MODSERIAL pc(USBTX, USBRX);  // connecting to pc
-MODSERIAL pc(USBTX, USBRX);                                     //Use computer
+MODSERIAL pc(USBTX, USBRX);                                     // connecting to pc
 AnalogIn EMG0In(A0);                                            //EMG input 0
 AnalogIn EMG1In(A1);                                            //EMG input 1
 InterruptIn button(SW3);                                        //Define button
@@ -24,6 +23,15 @@
 double omega1;
 double omega4;
 
+//Constants
+const double ll = 200.0;
+const double lu = 170.0;
+const double lb = 10.0;
+const double le = 79.0;
+const double xbase = 340;
+const int Length = 10000;                                       //Length of the array for the calibration
+const int Parts = 50;                                           //Mean average filter over 50 values
+
 
 //parameters for kinematics
 //vorige theta
@@ -40,7 +48,7 @@
 float EMG0filtArray[Parts];                                     //Array for the filtered array
 float EMG0Average;                                              //float for the value after Moving Average Filter
 float Sum0 = 0;                                                 //Sum0 for the moving average filter
-float EMG0Calibrate[Length];                                    //Array for the calibration 
+float EMG0Calibrate[Length];                                    //Array for the calibration
 int ReadCal0 = 0;                                               //Integer to read over the calibration array
 float MaxValue0 = 0;                                            //float to save the max muscle
 float Threshold0 = 0;                                           //Threshold for the first EMG signal
@@ -51,19 +59,11 @@
 float EMG1filtArray[Parts];                                     //Array for the filtered array
 float EMG1Average;                                              //float for the value after Moving Average Filter
 float Sum1 = 0;                                                 //Sum0 for the moving average filter
-float EMG1Calibrate[Length];                                    //Array for the calibration 
+float EMG1Calibrate[Length];                                    //Array for the calibration
 int ReadCal1 = 0;                                               //Integer to read over the calibration array
 float MaxValue1 = 0;                                            //float to save the max muscle
 float Threshold1 = 0;                                           //Threshold for the second EMG signal
 
-//Constants
-const double ll = 200.0;
-const double lu = 170.0;
-const double lb = 10.0;
-const double le = 79.0;
-const double xbase = 340;
-const int Length = 10000;                                       //Length of the array for the calibration
-const int Parts = 50;                                           //Mean average filter over 50 values
 
 //Filter variables
 BiQuad Notch50_0(0.7887,0,0.7887,0,0.5774);                     //Make Notch filter around 50 Hz
@@ -98,10 +98,10 @@
 Ticker simulateval;                                             //Timer that prints the values for x,y, and angles
 Ticker rekenen;                                                 //Not used right now
 Ticker ReadUseEMG0_timer;                                       //Timer to read, filter and use the EMG
-Ticker EMGCalibration0_timer;                                   //Timer for the calibration of 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 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
 
@@ -117,48 +117,151 @@
 bool SwitchStateBool = false;                                   //Bool to switch from calibration to working mode
 
 //Function to read and filter the EMG
-void ReadUseEMG0(){
-    for(int i = Parts ; i > 0 ; i--){                           //Make a first in, first out array
+void ReadUseEMG0()
+{
+    for(int i = Parts ; i > 0 ; i--) {                          //Make a first in, first out array
         EMG0filtArray[i] = EMG0filtArray[i-1];                  //Every value moves one up
     }
-    
+
+    Sum0 = 0;
+    EMG0 = EMG0In;                                              //Save EMG input in float
+    EMG0filt = filter0.step(EMG0);                              //Filter the signal
+    EMG0filt = abs(EMG0filt);                                   //Take the absolute value
+    EMG0filtArray[0] = EMG0filt;                                //Save the filtered signal on the first place in the array
+
+    for(int i = 0 ; i < Parts ; i++) {                          //Moving Average filter
+        Sum0 += EMG0filtArray[i];                               //Sum the new value and the previous 49
+    }
+    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
+        xMove = true;                                           //Set movement to true
+    } else {
+        redled = 1;                                             //Otherwise turn the LED off
+        xMove = false;                                          //Otherwise set movement to false
+    }
+}
+//Function to read and filter the EMG
+void ReadUseEMG1()
+{
+    for(int i = Parts ; i > 0 ; i--) {                          //Make a first in, first out array
+        EMG1filtArray[i] = EMG1filtArray[i-1];                  //Every value moves one up
+    }
+
+    Sum1 = 0;
+    EMG1 = EMG1In;                                              //Save EMG input in float
+    EMG1filt = filter1.step(EMG1);                              //Filter the signal
+    EMG1filt = abs(EMG1filt);                                   //Take the absolute value
+    EMG1filtArray[0] = EMG1filt;                                //Save the filtered signal on the first place in the array
+
+    for(int i = 0 ; i < Parts ; i++) {                          //Moving Average filter
+        Sum1 += EMG1filtArray[i];                               //Sum the new value and the previous 49
+    }
+    EMG1Average = (float)Sum1/Parts;                            //Divide the sum by 50
+
+    if (EMG1Average > Threshold1) {                             //If the value is higher than the threshold value
+        greenled = 0;                                           //Turn the LED on
+        yMove = true;                                           //Set y movement to true
+    } else {
+        greenled = 1;                                           //Otherwise turn the LED off
+        yMove = true;                                           //Otherwise set y movement to false
+    }
+}
+
+
+//Function to make an array during the calibration
+void CalibrateEMG0()
+{
+    for(int i = Parts ; i > 0 ; i--) {                          //Make a first in, first out array
+        EMG0filtArray[i] = EMG0filtArray[i-1];                  //Every value moves one up
+    }
+
     Sum0 = 0;
     EMG0 = EMG0In;                                              //Save EMG input in float
     EMG0filt = filter0.step(EMG0);                              //Filter the signal
     EMG0filt = abs(EMG0filt);                                   //Take the absolute value
     EMG0filtArray[0] = EMG0filt;                                //Save the filtered signal on the first place in the array
-                                             
-    for(int i = 0 ; i < Parts ; i++){                           //Moving Average filter
+
+    for(int i = 0 ; i < Parts ; i++) {                          //Moving Average filter
         Sum0 += EMG0filtArray[i];                               //Sum the new value and the previous 49
     }
-    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
-        xMove = true;                                           //Set movement to true
+    EMG0Calibrate[ReadCal0] = (float)Sum0/Parts;                //Divide the sum by 50
+
+    ReadCal0++;
+}
+
+//Function to make an array during the calibration
+void CalibrateEMG1()
+{
+    for(int i = Parts ; i > 0 ; i--) {                          //Make a first in, first out array
+        EMG1filtArray[i] = EMG1filtArray[i-1];                  //Every value moves one up
+    }
+
+    Sum1 = 0;
+    EMG1 = EMG1In;                                              //Save EMG input in float
+    EMG1filt = filter1.step(EMG1);                              //Filter the signal
+    EMG1filt = abs(EMG1filt);                                   //Take the absolute value
+    EMG1filtArray[0] = EMG1filt;                                //Save the filtered signal on the first place in the array
+
+    for(int i = 0 ; i < Parts ; i++) {                          //Moving Average filter
+        Sum1 += EMG1filtArray[i];                               //Sum the new value and the previous 49
     }
-    else{
-        redled = 1;                                             //Otherwise turn the LED off
-        yMove = false;                                          //Otherwise set movement to false
-    }                                 
+    EMG1Calibrate[ReadCal1] = (float)Sum1/Parts;                //Divide the sum by 50
+
+    ReadCal1++;
+}
+
+//Function to find the max value during the calibration
+void FindMax0()
+{
+    MaxValue0 = *max_element(EMG0Calibrate+500,EMG0Calibrate+Length);    //Find max value, but discard the first 100 values
+    Threshold0 = 0.30f*MaxValue0;                               //The threshold is a percentage of the max value
+    pc.printf("The calibration value of the first EMG is %f.\n\r The threshold is %f. \n\r",MaxValue0,Threshold0);              //Print the max value and the threshold
+    FindMax0_timer.detach();                                    //Detach the timer, so you only use this once
+}
+
+//Function to find the max value during the calibration
+void FindMax1()
+{
+    MaxValue1 = *max_element(EMG1Calibrate+500,EMG1Calibrate+Length);    //Find max value, but discard the first 100 values
+    Threshold1 = 0.30f*MaxValue1;                               //The threshold is a percentage of the max value
+    pc.printf("The calibration value of the second EMG is %f.\n\r The threshold is %f. \n\r",MaxValue1,Threshold1); //Print the Max value and the threshold
+    FindMax1_timer.detach();                                    //Detach the timer, so you only use this once
+}
+
+//Function to stop the reading of the EMG
+void StopProgram()
+{
+    greenled = 1;                                               //Turn the LEDs off
+    blueled = 1;
+    redled = 1;
+    exit (0);                                                   //Abort mission!!
+}
+
+//Function to switch a state
+void SwitchState()
+{
+    SwitchStateBool = true;                                     //Set the bool for the start of a state to true
+    SwitchState_timer.detach();                                 //Use this function once
 }
 
 
 
-
 //dit wordt aangeroepen in de tickerfunctie
-void inverse(double prex, double prey){
+void inverse(double prex, double prey)
+{
     /*
                                     qn = qn-1 + (jacobian^-1)*dPref/dt *deltaT
                                     ofwel
                                     thetai+1 = thetai +(jacobian)^-1*vector(deltaX, DeltaY)
                                     waar Pref = emg signaal
                                     */ //achtergrondinfo hierboven...
-            //
-                                    
+    //
+
     theta1 += (prefx*jacobiana+jacobianb*prey)*deltat; //theta 1 is zichzelf plus wat hier staat (is kinematics)
     theta4 += (prefx*jacobianc+jacobiand*prey)*deltat;//"                                                       "
-     //Hier worden xend en yend doorgerekend, die formules kan je overslaan
+    //Hier worden xend en yend doorgerekend, die formules kan je overslaan
     xendsum = lb + xbase +ll*(cos(theta1) - cos(theta4));
     xendsqrt1 = 2*sqrt(-xbase*xbase/4 + lu*lu + ll*(xbase*(cos(theta1)+cos(theta4))/2) -ll*(1+ cos(theta1+theta4)))*(-sin(theta1)+sin(theta4));
     xendsqrt2 = sqrt(pow((-xbase/ll+cos(theta1)+cos(theta4)),2)+ pow(sin(theta1) - sin(theta4),2));
@@ -275,14 +378,56 @@
                   (lb + xbase + ll*(cos(theta1) - cos(0.001 + theta4)) + (2*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.001 + theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.)*(-sin(theta1) + sin(0.001 + theta4)))/
                    sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2)))/2.));
 
-            //vanaf hier weer door met lezen!
-    prefx = 1*(!button1); //sw3, dit is belangrijk! prefx staat voor P_(reference) en het is de snelheid van de endeffector als 
-                        // de button ingedrukt wordt (als emg = boven treshold) is de prefx 1 (da's de rode 1)
-    prefy = 1*(!button2); //sw2, 
+    //vanaf hier weer door met lezen!
+    prefx = 1*(!xMove); //sw3, dit is belangrijk! prefx staat voor P_(reference) en het is de snelheid van de endeffector als
+    // de button ingedrukt wordt (als emg = boven treshold) is de prefx 1 (da's de rode 1)
+    prefy = 1*(!yMove); //sw2,
     inverse(prefx, prefy);
 }
 
 
+//State Machine
+void StateMachine()
+{
+    switch(CurrentState) {                                      //Determine in which state you are
+
+        case Calibration:                                       //Calibration mode
+            if(StateBool) {                                     //If you go into this state
+                pc.printf("You can start calibrating. \n\r");   //Print that you are in this state
+                StateBool = false;                              //Set the bool for the start of a state to false
+                EMGCalibration0_timer.attach(&CalibrateEMG0,0.005);     //Start EMG calibration every 0.005 seconds
+                EMGCalibration1_timer.attach(&CalibrateEMG1,0.005);     //Start EMG calibration every 0.005 seconds
+                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
+                blueled = 0;
+            }
+
+            if (SwitchStateBool) {                              //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;
+
+        case WorkingMode:                                       //Mode to get the robot working
+            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
+                EMGCalibration0_timer.detach();                 //Detach the the calibration
+                EMGCalibration1_timer.detach();                 //Detach the calibration
+                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.01); // roep de ticker aan (
+            }
+            blueled = 1;                                        //Set the blue led off
+            pc.printf("First EMG: %f, Second EMG: %f \n\r",EMG0Average,EMG1Average);
+            break;
+    }
+}
+
+
+
 
 
 
@@ -317,7 +462,8 @@
 
 
 //tot aan hier overslaan
-void printvalue(){
+void printvalue()
+{
     pc.printf("\n\r %f %f \n\r %f %f", theta4,theta1, xend, yend);           // in teraterm zijn de bovenste twee waardes hoeken, de onderste twee zijn de x en y coordinaat
 
 }
@@ -330,15 +476,14 @@
     theta1 = PI*0.49;
     theta4 = PI*0.49;
     pc.baud(115200);
-    //default = theta1 = theta4 = pi/2, 
-    kin.attach(kinematics, 0.01); // roep de ticker aan (
-    simulateval.attach(printvalue, 1);
+    //default = theta1 = theta4 = pi/2,
     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
     button.rise(StopProgram);                                   //If the button is pressed, stop program
+    simulateval.attach(printvalue, 1);
 
     pc.printf("%f", theta1);
     while(true) {