This is with minumum of 10 milliseconds threshold delay 00:53

Dependencies:   biquadFilter MODSERIAL QEI Servo mbed

Fork of StateMachine_EMg_RKI_PID_MOTOR_metklikenalles by Gaston Gabriël

Files at this revision

API Documentation at this revision

Comitter:
gastongab
Date:
Thu Nov 01 08:10:21 2018 +0000
Parent:
1:070092564648
Child:
3:3a9fdac2ba69
Commit message:
Deze is voor Astrid 09:10

Changed in this revision

BiQuadchains_zelfbeun.h Show annotated file Show diff for this revision Revisions of this file
BiQuadfilterTomLankhorst.lib Show annotated file Show diff for this revision Revisions of this file
MODSERIAL.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BiQuadchains_zelfbeun.h	Thu Nov 01 08:10:21 2018 +0000
@@ -0,0 +1,66 @@
+#include "mbed.h"
+
+//BiQuad Chains for each EMG, total 4 chains
+//EMG1!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+//Highpass
+BiQuadChain highp1;
+BiQuad highp1_1( 9.21171e-01, -1.84234e+00, 9.21171e-01, -1.88661e+00, 8.90340e-01 );
+BiQuad highp1_2( 1.00000e+00, -2.00000e+00, 1.00000e+00, -1.94922e+00, 9.53070e-01 );
+
+//Notch
+BiQuadChain notch1;
+BiQuad notch1_1( 9.56543e-01, -1.82035e+00, 9.56543e-01, -1.84459e+00, 9.53626e-01 );
+BiQuad notch1_2( 1.00000e+00, -1.90305e+00, 1.00000e+00, -1.87702e+00, 9.59471e-01 );
+
+//Lowpass 4th order cutoff 3Hz
+BiQuadChain lowp1;
+BiQuad lowp1_1( 7.69910e-09, 1.53982e-08, 7.69910e-09, -1.96542e+00, 9.65769e-01 );
+BiQuad lowp1_2( 1.00000e+00, 2.00000e+00, 1.00000e+00, -1.98532e+00, 9.85677e-01 );
+
+//EMG2!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+//Highpass
+BiQuadChain highp2;
+BiQuad highp2_1( 9.21171e-01, -1.84234e+00, 9.21171e-01, -1.88661e+00, 8.90340e-01 );
+BiQuad highp2_2( 1.00000e+00, -2.00000e+00, 1.00000e+00, -1.94922e+00, 9.53070e-01 );
+
+//Notch
+BiQuadChain notch2;
+BiQuad notch2_1( 9.56543e-01, -1.82035e+00, 9.56543e-01, -1.84459e+00, 9.53626e-01 );
+BiQuad notch2_2( 1.00000e+00, -1.90305e+00, 1.00000e+00, -1.87702e+00, 9.59471e-01 );
+
+//Lowpass 4th order cutoff 3Hz
+BiQuadChain lowp2;
+BiQuad lowp2_1( 7.69910e-09, 1.53982e-08, 7.69910e-09, -1.96542e+00, 9.65769e-01 );
+BiQuad lowp2_2( 1.00000e+00, 2.00000e+00, 1.00000e+00, -1.98532e+00, 9.85677e-01 );
+
+//EMG3!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+//Highpass
+BiQuadChain highp3;
+BiQuad highp3_1( 9.21171e-01, -1.84234e+00, 9.21171e-01, -1.88661e+00, 8.90340e-01 );
+BiQuad highp3_2( 1.00000e+00, -2.00000e+00, 1.00000e+00, -1.94922e+00, 9.53070e-01 );
+
+//Notch
+BiQuadChain notch3;
+BiQuad notch3_1( 9.56543e-01, -1.82035e+00, 9.56543e-01, -1.84459e+00, 9.53626e-01 );
+BiQuad notch3_2( 1.00000e+00, -1.90305e+00, 1.00000e+00, -1.87702e+00, 9.59471e-01 );
+
+//Lowpass 4th order cutoff 3Hz
+BiQuadChain lowp3;
+BiQuad lowp3_1( 7.69910e-09, 1.53982e-08, 7.69910e-09, -1.96542e+00, 9.65769e-01 );
+BiQuad lowp3_2( 1.00000e+00, 2.00000e+00, 1.00000e+00, -1.98532e+00, 9.85677e-01 );
+
+//EMG4!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+//Highpass
+BiQuadChain highp4;
+BiQuad highp4_1( 9.21171e-01, -1.84234e+00, 9.21171e-01, -1.88661e+00, 8.90340e-01 );
+BiQuad highp4_2( 1.00000e+00, -2.00000e+00, 1.00000e+00, -1.94922e+00, 9.53070e-01 );
+
+//Notch
+BiQuadChain notch4;
+BiQuad notch4_1( 9.56543e-01, -1.82035e+00, 9.56543e-01, -1.84459e+00, 9.53626e-01 );
+BiQuad notch4_2( 1.00000e+00, -1.90305e+00, 1.00000e+00, -1.87702e+00, 9.59471e-01 );
+
+//Lowpass 4th order cutoff 3Hz
+BiQuadChain lowp4;
+BiQuad lowp4_1( 7.69910e-09, 1.53982e-08, 7.69910e-09, -1.96542e+00, 9.65769e-01 );
+BiQuad lowp4_2( 1.00000e+00, 2.00000e+00, 1.00000e+00, -1.98532e+00, 9.85677e-01 );
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BiQuadfilterTomLankhorst.lib	Thu Nov 01 08:10:21 2018 +0000
@@ -0,0 +1,1 @@
+http://os.mbed.com/users/tomlankhorst/code/biquadFilter/#26861979d305
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MODSERIAL.lib	Thu Nov 01 08:10:21 2018 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/Sissors/code/MODSERIAL/#a3b2bc878529
--- a/main.cpp	Wed Oct 31 19:09:22 2018 +0000
+++ b/main.cpp	Thu Nov 01 08:10:21 2018 +0000
@@ -1,6 +1,12 @@
 //Voor het toevoegen van een button:
 #include "mbed.h"
 #include <iostream>
+#include "BiQuad.h"
+#include "BiQuadchains_zelfbeun.h"
+#include "MODSERIAL.h"
+
+MODSERIAL pc(USBTX, USBRX);
+
 DigitalOut gpo(D0);
 
 DigitalIn button2(SW3);  
@@ -10,11 +16,200 @@
 DigitalOut led2(LED_RED);
 DigitalOut led3(LED_BLUE);
 
-Timer t;
+//EMG tickers, these tickers are called in the mainscript with fsample 500Hz, also sends to HIDscope with same fsample
+Ticker sample_ticker; //ticker for filtering pref. with 1000Hz, define in tick.attach
+Timer t; //timer try out for Astrid
+Timer timer_calibration; //timer for EMG calibration
+
+
+
+//Input system
+AnalogIn emg1(A0); //right biceps
+AnalogIn emg2(A1); //right triceps
+AnalogIn emg3(A2); //left biceps
+AnalogIn emg4(A3); //left triceps
+
+//Filtered EMG signals from the end of the chains
+double emg1_filtered, emg2_filtered, emg3_filtered, emg4_filtered;
+volatile int i = 0;
+
+void emgsample(){
+    //All EMG signal through Highpass
+    double emgread1 = emg1.read();
+    double emgread2 = emg2.read();
+    double emgread3 = emg3.read();
+    double emgread4 = emg4.read();
+  
+    double emg1_highpassed = highp1.step(emgread1);
+    double emg2_highpassed = highp2.step(emgread2);
+    double emg3_highpassed = highp3.step(emgread3);
+    double emg4_highpassed = highp4.step(emgread4);
+    
+    //All EMG highpassed through Notch
+    double emg1_notched = notch1.step(emg1_highpassed);
+    double emg2_notched = notch2.step(emg2_highpassed);
+    double emg3_notched = notch3.step(emg3_highpassed);
+    double emg4_notched = notch4.step(emg4_highpassed);
+    
+    //All EMG notched rectify
+    double emg1_abs = abs(emg1_notched);
+    double emg2_abs = abs(emg2_notched);
+    double emg3_abs = abs(emg3_notched);
+    double emg4_abs = abs(emg4_notched);
+    
+    //All EMG abs into lowpass
+    emg1_filtered = lowp1.step(emg1_abs);
+    emg2_filtered = lowp2.step(emg2_abs);
+    emg3_filtered = lowp3.step(emg3_abs);
+    emg4_filtered = lowp4.step(emg4_abs);
+    
+    
+    //Send data to HIDScope
+    //scope.set(0,emg1_filtered ); ONLY FOR VISUALIZATION
+    //scope.set(1,emg2_filtered);
+    //scope.set(2,emg3_filtered);
+    //scope.set(3,emg4_filtered);
+    //scope.send();
+          
+    }
+    
+
+//Define doubles for calibration and ticker
+    double ts = 0.001; //tijdsstap
+    double calibration_time = 55; //time EMG calibration should take
+    
+    volatile double temp_highest_emg1 = 0; //highest detected value right biceps
+    volatile double temp_highest_emg2 = 0;
+    volatile double temp_highest_emg3 = 0;
+    volatile double temp_highest_emg4 = 0;
+    
+    //Doubles for calculation threshold
+    double p_t;
+    double threshold1;
+    double threshold2;
+    double threshold3;
+    double threshold4;
+        
+        void CalibrationEMG()
+    {
+        //static float samples = calibration_time/ts;
+        while(timer_calibration<55){
+            if(timer_calibration>0 && timer_calibration<10)
+                {
+                led1=!led1;
+                if(emg1_filtered>temp_highest_emg1)
+                    {
+                    temp_highest_emg1= emg1_filtered;
+                    }
+                }
+            if(timer_calibration>10 && timer_calibration<15)
+                {
+                led1=0;   
+                led2=0;
+                led3=0;
+                }
+            if(timer_calibration>15 && timer_calibration<25)
+                {
+                led2=!led2;
+                if(emg2_filtered>temp_highest_emg2)
+                    {
+                    temp_highest_emg2= emg2_filtered;
+                    }
+                }
+            if(timer_calibration>25 && timer_calibration<30)
+                {
+                led1=0;   
+                led2=0;
+                led3=0;          
+                }
+            if(timer_calibration>30 && timer_calibration<40)
+                {
+                led3=!led3;
+                if(emg3_filtered>temp_highest_emg3)
+                    {
+                    temp_highest_emg3= emg3_filtered;
+                    }
+                }
+            if(timer_calibration>40 && timer_calibration<45)
+                {
+                led1=0;   
+                led2=0;
+                led3=0;       
+                }
+            if(timer_calibration>45 && timer_calibration<55)
+                {
+                led2=!led2;
+                led3=!led3;
+                if(emg3_filtered>temp_highest_emg3)
+                    {
+                    temp_highest_emg3= emg3_filtered;
+                    }
+                }
+    led1=1;
+    led2=1;
+    led3=1;
+    
+
+    }
+    
+    pc.printf("Highest value right biceps= %f \r\n", temp_highest_emg1);
+    pc.printf("Highest value right triceps= %f \r\n", temp_highest_emg2);
+    pc.printf("Highest value left biceps= %f \r\n", temp_highest_emg3);
+    pc.printf("Highest value left triceps= %f \r\n", temp_highest_emg4);
+    
+    p_t = 0.8;
+    threshold1 = temp_highest_emg1*p_t;
+    threshold2 = temp_highest_emg2*p_t; 
+    threshold3 = temp_highest_emg3*p_t;
+    threshold4 = temp_highest_emg4*p_t;   
+}
+
+void threshold_check(){
+     
+    //Check if emg_filtered has reached their threshold
+    bool bicepsR;
+    bool tricepsR;
+    bool bicepsL;
+    bool tricepsL;
+    
+    //EMG1 threshold check
+    if(emg1_filtered>threshold1){
+        bicepsR = true;
+        }
+    else{
+        bicepsR= false;
+        }
+    //EMG2 threshold check
+    if(emg2_filtered>threshold2){
+        tricepsR = true;
+        }
+    else{
+        tricepsR= false;
+        }
+    //EMG3 threshold check
+     if(emg3_filtered>threshold3){
+        bicepsL = true;
+        }
+    else{
+        bicepsL= false;
+        }
+    //EMG4 threshold check
+     if(emg4_filtered>threshold4){
+        tricepsL = true;
+        }
+    else{
+        tricepsL= false;
+        }
+        
+    pc.printf("Biceps Right = %d", bicepsR);
+    pc.printf("Triceps Right = %d",tricepsR);
+    pc.printf("Biceps Left = %d", bicepsL);
+    pc.printf("Triceps Left = %d", tricepsL);
+    
+}
 
 enum states {MOTORS_OFF,CALIBRATION,HOMING,DEMO,MOVEMENT,CLICK}; 
-int f = 1;
-states currentState = MOTORS_OFF; 
+states currentState = MOTORS_OFF; //Chosen startingposition for states
 bool stateChanged = true; // Make sure the initialization of first state is executed
 
 void ProcessStateMachine(void)
@@ -57,10 +252,23 @@
       if (stateChanged)
       {
         // state initialization: oranje
-        led1 = 0;
-        led2 = 0;
-        led3 = 1;
-        wait (1);
+        temp_highest_emg1 = 0; //highest detected value right biceps
+        temp_highest_emg2 = 0;
+        temp_highest_emg3 = 0;
+        temp_highest_emg4 = 0;
+       
+       timer_calibration.reset();
+       timer_calibration.start();
+                 
+       
+        if(timer_calibration<55){
+            sample_ticker.attach(&emgsample, ts);
+            CalibrationEMG();
+            }
+        else{
+            sample_ticker.detach();
+            timer_calibration.stop();
+            }
         
         stateChanged = false;
       }
@@ -137,7 +345,9 @@
         led1 = 1;
         led2 = 0;
         led3 = 0;
-        wait (1);
+        pc.printf("De kleur is paars ok");
+        //sample_ticker.attach(&threshold_check, ts);
+        //sample_ticker.attach(&emgsample, ts);
         
         stateChanged = false;
       }
@@ -191,6 +401,7 @@
  
 int main()
 {
+    pc.baud(115200);
     while (true)
     {
     led1 = 1;