Diff: main.cpp

Revision:

2:0a8622662f6d

Parent:

1:070092564648

Child:

3:3a9fdac2ba69

--- 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;