State machine with EMG functions and parameters
Dependencies: biquadFilter FastPWM HIDScope MODSERIAL mbed
Fork of
StateMachine
by 
Tommie Verouden
Revision 13:abee61d15b5f, committed 2018-11-01
- Comitter:
- EvaKrolis
- Date:
- Thu Nov 01 09:26:12 2018 +0000
- Parent:
- 12:323ac4e27a0d
- Commit message:
- State machine with EMG functions and parameters
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Thu Nov 01 09:02:56 2018 +0000
+++ b/main.cpp Thu Nov 01 09:26:12 2018 +0000
@@ -5,6 +5,7 @@
#include "FastPWM.h"
#include "HIDScope.h"
#include "MODSERIAL.h"
+#include <algorithm>
// LEDs
DigitalOut ledRed(LED_RED,1); // red LED K64F
@@ -34,7 +35,58 @@
states currentState = calibratingMotors;// start in waiting mode
bool changeState = true; // initialise the first state
+// Parameters for the EMG
+//EMG inputs
+AnalogIn EMG0In(A0); //EMG input 0
+AnalogIn EMG1In(A1); //EMG input 1
+
+//Timers
+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
+
+//Constants
+const int Length = 10000; //Length of the array for the calibration
+const int Parts = 50; //Mean average filter over 50 values
+
+//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
+
+//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
+
+//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
+
+//Bool for movement
+bool xMove = false; //Bool for the x-movement
+bool yMove = false; //Bool for the y-movement
// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ FUNCTIONS ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
// ============================ GENERAL FUNCTIONS =============================
@@ -50,16 +102,121 @@
{ // blinkTimer.detach
led =! led; // toggle LED
}
-
-// ============================ MOTOR FUNCTIONS ==============================
-
// ============================= EMG FUNCTIONS ===============================
+//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
+ xMove = true; //Set movement to true
+ }
+ else{
+ 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
+ yMove = true; //Set y movement to true
+ }
+ else{
+ yMove = false; //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
+ Sum0 += EMG0filtArray[i]; //Sum the new value and the previous 49
+ }
+ 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
+ }
+ 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
+}
// ========================= KINEMATICS FUNCTIONS ============================
+// ============================ MOTOR FUNCTIONS ==============================
+
+
// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ STATE MACHINE ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
void stateMachine(void)
{