Diff: main.cpp

Revision:

14:e3fe54f0a4b4

Parent:

13:f77c5f196161

Child:

15:38258e6b6e91

--- a/main.cpp	Wed Oct 31 17:36:26 2018 +0000
+++ b/main.cpp	Wed Oct 31 18:15:10 2018 +0000
@@ -6,8 +6,16 @@
 #include "BiQuad.h"
 #include <algorithm>
 #define PI 3.14159265
+
+//Inputs and outputs
 MODSERIAL pc(USBTX, USBRX);  // connecting to pc
-
+MODSERIAL pc(USBTX, USBRX);                                     //Use computer
+AnalogIn EMG0In(A0);                                            //EMG input 0
+AnalogIn EMG1In(A1);                                            //EMG input 1
+InterruptIn button(SW3);                                        //Define button
+DigitalOut greenled(LED_GREEN);                                 //Green led
+DigitalOut blueled(LED_BLUE);                                   //Blue led
+DigitalOut redled(LED_RED);                                     //Red led
 
 
 
@@ -17,24 +25,54 @@
 double omega4;
 
 
-//Joe dit zijn de inputsignalen (en tussenvariabelen)
+//parameters for kinematics
 //vorige theta
 double theta1 = PI*0.49;                  // huidige/nieuwe theta
 double theta4 = PI*0.49;
-bool emg1;
-bool emg2;
-bool emg3;
-double thetaflip = 0;
+double thetaflip = 0;                     //angle of the flipping motor
+double prefx;                             //Preference for x, will later be defined due to the motor
+double prefy;                             //"                                                        "
+double deltat = 0.01;                     //tijdstap(moet nog aangepast worden)
+
+//Parameters for the first EMG signal
+float EMG0;                                                     //float for EMG input
+float EMG0filt;                                                 //float for filtered EMG
+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 
+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
 
-double prefx;
-double prefy;
-double deltat = 0.01;                   //tijdstap(moet nog aangepast worden)
-//Joe dit zijn de constantes
-double ll = 200.0;
-double lu = 170.0;
-double lb = 10.0;
-double le = 79.0;
-double xbase = 340;
+//Parameters for the second EMG signal
+float EMG1;                                                     //float for EMG input
+float EMG1filt;                                                 //float for filtered EMG
+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 
+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
+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
+BiQuadChain filter0;                                            //Make chain of filters for the first EMG signal
+BiQuadChain filter1;                                            //Make chain of filters for the second EMG signal
+
 
 
 //forward kinematics, Check mathematica! Omdat mbed in paniek raakt met meerdere wortels, hebben we de vergelijking opgedeeld in 3 stukken
@@ -68,6 +106,45 @@
 Ticker SwitchState_timer;                                       //Timer to switch from the Calibration to the working mode
 
 
+//Bool for movement
+bool xMove = false;                                             //Bool for the x-movement
+bool yMove = false;                                             //Bool for the y-movement
+
+//Parameters for the state machine
+enum States {Calibration, WorkingMode};                         //Initialize state machine
+States CurrentState = Calibration;                              //Start in the calibration mode
+bool StateBool = true;                                          //Bool to first go in a state
+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
+        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
+        yMove = false;                                          //Otherwise set movement to false
+    }                                 
+}
+
+
+
 
 //dit wordt aangeroepen in de tickerfunctie
 void inverse(double prex, double prey){