Diff: main.cpp

Revision:

4:c7be673eb4a1

Parent:

3:3a9fdac2ba69

Child:

5:19f59a855475

--- a/main.cpp	Thu Nov 01 08:46:04 2018 +0000
+++ b/main.cpp	Thu Nov 01 15:08:05 2018 +0000
@@ -18,6 +18,7 @@
 
 //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
+Ticker threshold_check_ticker;
 Timer t; //timer try out for Astrid
 Timer timer_calibration; //timer for EMG calibration
 
@@ -30,8 +31,8 @@
 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;
+volatile double emg1_filtered, emg2_filtered, emg3_filtered, emg4_filtered;
+int i = 0;
 
 void emgsample(){
     //All EMG signal through Highpass
@@ -84,18 +85,18 @@
     volatile double temp_highest_emg4 = 0;
     
     //Doubles for calculation threshold
-    double p_t; //set threshold at percentage of highest value
-    double threshold1;
-    double threshold2;
-    double threshold3;
-    double threshold4;
+    double p_t = 0.4; //set threshold at percentage of highest value
+    volatile double threshold1;
+    volatile double threshold2;
+    volatile double threshold3;
+    volatile 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)
                     {
@@ -109,11 +110,12 @@
                 led3=0;
                 }
             if(timer_calibration>15 && timer_calibration<25)
-                {
+                {    
                 led2=!led2;
                 if(emg2_filtered>temp_highest_emg2)
                     {
                     temp_highest_emg2= emg2_filtered;
+                    pc.printf("Temp2 = %f \r\n",temp_highest_emg2);
                     }
                 }
             if(timer_calibration>25 && timer_calibration<30)
@@ -157,57 +159,71 @@
     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;   
 }
 
+//Check if emg_filtered has reached their threshold
+    int bicepsR;
+    int tricepsR;
+    int bicepsL;
+    int tricepsL;
+
 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;
+        bicepsR = 1;
         }
     else{
-        bicepsR= false;
+        bicepsR= 0;
         }
     //EMG2 threshold check
     if(emg2_filtered>threshold2){
-        tricepsR = true;
+        tricepsR = 1;
         }
     else{
-        tricepsR= false;
+        tricepsR= 0;
         }
     //EMG3 threshold check
      if(emg3_filtered>threshold3){
-        bicepsL = true;
+        bicepsL = 1;
         }
     else{
-        bicepsL= false;
+        bicepsL= 0;
         }
     //EMG4 threshold check
      if(emg4_filtered>threshold4){
-        tricepsL = true;
+        tricepsL = 1;
         }
     else{
-        tricepsL= false;
+        tricepsL= 0;
         }
-        
-    pc.printf("Biceps Right = %B", bicepsR);
-    pc.printf("Triceps Right = %B",tricepsR);
-    pc.printf("Biceps Left = %B", bicepsL);
-    pc.printf("Triceps Left = %B", tricepsL);
+    
+    /*   
+    pc.printf("Biceps Right = %i", bicepsR);
+    pc.printf("Triceps Right = %i",tricepsR); 
+    pc.printf("Biceps Left = %i", bicepsL);
+    pc.printf("Triceps Left = %i", tricepsL);
+    */
+    
     
 }
 
+
+//Activate ticker for Movement state, filtering and Threshold checking
+void movement_ticker_activator(){
+    sample_ticker.attach(&emgsample, ts);
+    threshold_check_ticker.attach(&threshold_check, ts);
+    }
+void movement_ticker_deactivator(){
+    sample_ticker.detach();
+    threshold_check_ticker.detach();
+    }
+    
 enum states {MOTORS_OFF,CALIBRATION,HOMING,DEMO,MOVEMENT,CLICK}; 
 states currentState = MOTORS_OFF; //Chosen startingposition for states
 bool stateChanged = true; // Make sure the initialization of first state is executed
@@ -259,16 +275,12 @@
        
        timer_calibration.reset();
        timer_calibration.start();
-                 
-       
-        if(timer_calibration<55){
-            sample_ticker.attach(&emgsample, ts);
-            CalibrationEMG();
-            }
-        else{
-            sample_ticker.detach();
-            timer_calibration.stop();
-            }
+                
+        sample_ticker.attach(&emgsample, ts);
+        CalibrationEMG();
+        sample_ticker.detach();
+        timer_calibration.stop();
+        
         
         stateChanged = false;
       }
@@ -342,40 +354,93 @@
       if (stateChanged)
       {
         // state initialization: purple
+        t.reset();
         t.start();
+             
         led1 = 1;
         led2 = 0;
-        led3 = 0;
-        wait(1);
-        sample_ticker.attach(&threshold_check, ts);
-        sample_ticker.attach(&emgsample, ts);
+        led3 = 0; 
+        wait(2);            
+                       
+        movement_ticker_activator();
                 
+        led1 = 0;
+        led2 = 0;
+        led3 = 0; 
+        wait(2);  
+        
+             
         stateChanged = false;
       }
           
       // State transition logic: naar CLICK (button1), naar MOTORS_OFF(button2) anders naar MOVEMENT
         if (!button1)
         {        
+        movement_ticker_deactivator();
         currentState = CLICK;
         stateChanged = true; 
         }
         else if (!button2)
         {        
+        movement_ticker_deactivator();
         currentState = MOTORS_OFF  ;
         stateChanged = true;
         }
         else if (t>300)
         {        
+        movement_ticker_deactivator();
         t.stop();
         t.reset();
         currentState = HOMING  ;
         stateChanged = true;
         }
         else
-        {        
+        {
+            //For every muscle a different colour if threshold is passed
+        if(bicepsR==1){       
+        led1 = 0;
+        led2 = 1;
+        led3 = 1;
+        }
+        else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ){
+        led1 = 1;
+        led2 = 1;
+        led3 = 1;
+        }  
+        if(tricepsR==1){
+        led1 = 1;
+        led2 = 0;
+        led3 = 1;
+        }
+        else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ){
+        led1 = 1;
+        led2 = 1;
+        led3 = 1;
+        }  
+        if(bicepsL==1){
+        led1 = 1;
+        led2 = 1;
+        led3 = 0;
+        }
+        else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ){
+        led1 = 1;
+        led2 = 1;
+        led3 = 1;
+        } 
+        if(tricepsL==1){
+        led1 = 1;
+        led2 = 0;
+        led3 = 0;
+        }
+        else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ){
+        led1 = 1;
+        led2 = 1;
+        led3 = 1;
+        }              
         currentState = MOVEMENT  ;
-        stateChanged = true;
+        stateChanged = false;
         }
+        
         break;
         
         case CLICK:
@@ -397,17 +462,35 @@
         stateChanged = true; 
         break; 
          
-}
+    }
 }
  
 int main()
 {
+    //BiQuad Chain add
+    highp1.add( &highp1_1 ).add( &highp1_2 );
+    notch1.add( &notch1_1 ).add( &notch1_2 );
+    lowp1.add( &lowp1_1 ).add(&lowp1_2);
+    
+    highp2.add( &highp2_1 ).add( &highp2_2 );
+    notch2.add( &notch2_1 ).add( &notch2_2 );
+    lowp2.add( &lowp2_1 ).add(&lowp2_2);    
+    
+    highp3.add( &highp3_1 ).add( &highp3_2 );
+    notch3.add( &notch3_1 ).add( &notch3_2 );
+    lowp3.add( &lowp3_1 ).add(&lowp3_2);
+     
+    highp4.add( &highp4_1 ).add( &highp4_2 );
+    notch4.add( &notch4_1 ).add( &notch4_2 );
+    lowp4.add( &lowp4_1 ).add(&lowp4_2);
+    
     pc.baud(115200);
-    while (true)
-    {
     led1 = 1;
     led2 = 1;
     led3 = 1;
+    
+    while (true)
+    {
     ProcessStateMachine();
     
     }