Diff: Calibrationscript.cpp

Revision:

7:7c6a9bb2d30e

Parent:

6:b9a84c1cb4f9

Child:

8:237b1e262ebd

--- a/Calibrationscript.cpp	Wed Nov 01 11:40:55 2017 +0000
+++ b/Calibrationscript.cpp	Wed Nov 01 13:36:54 2017 +0000
@@ -3,10 +3,14 @@
 #include "MODSERIAL.h"
 #include "BiQuad.h"
 # include "math.h"
-
+//                       Rechterarm
 //                       DEFINING                  
 //Define Inputs
-AnalogIn emg(A0);                               //analog of EMG input
+//rechterarm
+AnalogIn emg_r(A0);                               //analog of emg_r input
+//linkerarm
+AnalogIn emg_l(A1);
+
 InterruptIn button1(PTA4);                        //test button for starting motor 1
 InterruptIn button2(SW2);                         //FOR DEBUGGING
 
@@ -18,10 +22,11 @@
 
 
 //Define Tickers
-Ticker      sample_timer;                       // Taking samples
-Ticker      LED_timer;                          // Write the LED
-Ticker      calibration_timer;                  // Check when to start calibration
+//rechterarm
+Ticker      LED_timer_r;                          // Write the LED
 
+//linkerarm
+Ticker      LED_timer_l;                          // Write the LED
 
 //Define HIDscope
 //HIDScope    scope(2);                           //instantize a 2-channel HIDScope object
@@ -33,61 +38,135 @@
 BiQuad bq2_high( 0.973868, -1.947737,   0.97386, -1.947737, 0.948429);
 BiQuad bq3_notch(0.91859, -1.82269, 0.91859, -1.82269, 0.83718);
 
-double print; 
-double emgFiltered;
-double filteredAbs;
-double emg_value;
-double onoffsignal;
-bool check_calibration=0;
-double avg_emg;
+
+
+// Rechterarm
+double emgFiltered_r;
+double filteredAbs_r;
+double emg_value_r;
+double onoffsignal_r;
+bool check_calibration_r=0;
+double avg_emg_r;
+bool rechterarm_positief_r = false;
+bool rechterarm_negatief_r = false;
 
-//                      FUNCTIONS                
+//Linkerarm 
+double emgFiltered_l;
+double filteredAbs_l;
+double emg_value_l;
+double onoffsignal_l;
+bool check_calibration_l=0;
+double avg_emg_l;
+bool linkerarm_positief_l = false;
+bool linkerarm_negatief_l = false;
+
+//                      FUNCTIONS  
+
+//Rechterarm              
 //function for filtering
 
-void filter(){                                      
-    if(check_calibration==1){
+void filter_r(){                                      
+    if(check_calibration_r==1){
+        
+        
+        
+        
+        emg_value_r = emg_r.read();
+        emgFiltered_r = bqc.step( emg_value_r );
+        filteredAbs_r = abs( emgFiltered_r );
+    if (avg_emg_r != 0){
+        onoffsignal_r=filteredAbs_r/avg_emg_r;             //divide the emg_r signal by the max emg_r to calibrate the signal per person
+      }
+       // scope.set(0,emg_value_r);                     //set emg_r signal to scope in channel 1
+       //scope.set(1,onoffsignal_r);                   //set filtered signal to scope in channel 2
+      //  scope.send();                               //send the signals to the scope
+//            pc.printf("emg_r signal %f, filtered signal %f \n",emg_value_r,onoffsignal_r);
+    }
+}
+
+//Linkerarm
+//function for filtering
+
+void filter_l(){                                      
+    if(check_calibration_l==1){
         
         
         
         
-        emg_value = emg.read();
-        emgFiltered = bqc.step( emg_value );
-        filteredAbs = abs( emgFiltered );
-    if (avg_emg != 0){
-        onoffsignal=filteredAbs/avg_emg;             //divide the emg signal by the max EMG to calibrate the signal per person
+        emg_value_l = emg_l.read();
+        emgFiltered_l = bqc.step( emg_value_l );
+        filteredAbs_l = abs( emgFiltered_l );
+    if (avg_emg_l != 0){
+        onoffsignal_l=filteredAbs_l/avg_emg_l;             //divide the emg_r signal by the max emg_r to calibrate the signal per person
       }
-       // scope.set(0,emg_value);                     //set emg signal to scope in channel 1
-       //scope.set(1,onoffsignal);                   //set filtered signal to scope in channel 2
-      //  scope.send();                               //send the signals to the scope
-//            pc.printf("emg signal %f, filtered signal %f \n",emg_value,onoffsignal);
+ 
     }
 }
 
+
+//Rechterarm
 //function to check the threshold
-void check_emg(){           
-double filteredAbs_temp;
+void check_emg_r(){           
+double filteredAbs_temp_r;
           
-    if(check_calibration==1){ 
+    if(check_calibration_r==1){ 
     for( int i = 0; i<1000;i++){
-               filter();
-               filteredAbs_temp = filteredAbs_temp + onoffsignal;
+               filter_r();
+               filteredAbs_temp_r = filteredAbs_temp_r + onoffsignal_r;
                wait(0.0004);
                }    
-               filteredAbs_temp = filteredAbs_temp/1000;                                 
-            if(filteredAbs_temp<=0.55){                         //if signal is lower then 0.5 the blue light goes on
+               filteredAbs_temp_r = filteredAbs_temp_r/1000;                                 
+            if(filteredAbs_temp_r<=0.55){                         //if signal is lower then 0.5 the blue light goes on
                  led1.write(1);          //led 1 is rood en uit
                     led2.write(0);          //led 2 is blauw en aan
+                    rechterarm_positief_r = false;
+                    rechterarm_negatief_r = true;
+                   
             }
-            else if(filteredAbs_temp > 0.55){                //if signal does not pass threshold value, blue light goes on
+            else if(filteredAbs_temp_r > 0.55){                //if signal does not pass threshold value, blue light goes on
                     led1.write(0);
                     led2.write(1);
+                    rechterarm_negatief_r = false;
+                    rechterarm_positief_r = true;
             }
+            
     }
 }
 
+//Linkerarm
+//function to check the threshold
+void check_emg_l(){           
+double filteredAbs_temp_l;
+          
+    if(check_calibration_l==1){ 
+    for( int i = 0; i<1000;i++){
+               filter_l();
+               filteredAbs_temp_l = filteredAbs_temp_l + onoffsignal_l;
+               wait(0.0004);
+               }    
+               filteredAbs_temp_l = filteredAbs_temp_l/1000;                                 
+            if(filteredAbs_temp_l<=0.55){                         //if signal is lower then 0.5 the blue light goes on
+                 led1.write(1);          //led 1 is rood en uit
+                    led2.write(0);          //led 2 is blauw en aan
+                    linkerarm_positief_l = false;
+                    linkerarm_negatief_l = true;
+                   
+            }
+            else if(filteredAbs_temp_l > 0.55){                //if signal does not pass threshold value, blue light goes on
+                    led1.write(0);
+                    led2.write(1);
+                    linkerarm_negatief_l = false;
+                    linkerarm_positief_l = true;
+            }
+            
+    }
+}
+
+
+//rechterarm
 //function to calibrate
-int calibration(){
-   pc.printf("check1\n\r");
+int calibration_r(){
+  
                                             
    // if(button1.read()==false){
     
@@ -95,22 +174,22 @@
         
         led3.write(0);
        
-        double signal_verzameling = 0;
+        double signal_verzameling_r = 0;
         for(int n =0; n<5000;n++){   
-           filter();
+           filter_r();
                               //read for 5000 samples as calibration
-           emg_value = emg.read();
-            emgFiltered = bqc.step( emg_value );
-            filteredAbs = abs(emgFiltered);
+           emg_value_r = emg_r.read();
+            emgFiltered_r = bqc.step( emg_value_r );
+            filteredAbs_r = abs(emgFiltered_r);
          
           
-           // signal_verzameling[n]=  filteredAbs;
-             signal_verzameling = signal_verzameling + filteredAbs ;
+           // signal_verzameling[n]=  filteredAbs_r;
+             signal_verzameling_r = signal_verzameling_r + filteredAbs_r ;
              wait(0.0004);
              
               if (n == 4999){
-                  avg_emg = signal_verzameling / n;
-                  pc.printf("avg_emg = %f\n\r",avg_emg);
+                  avg_emg_r = signal_verzameling_r / n;
+                  
                   }
             }  
           
@@ -123,13 +202,64 @@
      
       //    sum_array = sum_array + signal_verzameling[i] ;
       //      }
-//avg_emg = sum_array / lengte_array;
-     //   pc.printf("avg_emg = %f\n\r",avg_emg);
+//avg_emg_r = sum_array / lengte_array;
+     //   pc.printf("avg_emg_r = %f\n\r",avg_emg_r);
         
         
         
         
-        check_calibration=1;
+        check_calibration_r=1;
+        led3.write(1);
+   // }
+    return 0;  
+}
+
+//linkeram
+//function to calibrate
+int calibration_l(){
+  
+                                            
+   // if(button1.read()==false){
+    
+        
+        
+        led3.write(0);
+       
+        double signal_verzameling_l= 0;
+        for(int n =0; n<5000;n++){   
+           filter_l();
+                              //read for 5000 samples as calibration
+           emg_value_l = emg_l.read();
+            emgFiltered_l = bqc.step( emg_value_l );
+            filteredAbs_l = abs(emgFiltered_l);
+         
+          
+           // signal_verzameling[n]=  filteredAbs_r;
+             signal_verzameling_l = signal_verzameling_l + filteredAbs_l ;
+             wait(0.0004);
+             
+              if (n == 4999){
+                  avg_emg_l = signal_verzameling_l / n;
+                  
+                  }
+            }  
+          
+             led3.write(1);
+           // double lengte_array = sizeof(signal_verzameling);
+      //     pc.printf("lengte_array = %f\n\r",lengte_array);
+            
+        //    for(int i = 0; i < lengte_array; i++){
+      
+     
+      //    sum_array = sum_array + signal_verzameling[i] ;
+      //      }
+//avg_emg_r = sum_array / lengte_array;
+     //   pc.printf("avg_emg_r = %f\n\r",avg_emg_r);
+        
+        
+        
+        
+        check_calibration_l=1;
         led3.write(1);
    // }
     return 0;  
@@ -149,12 +279,13 @@
     
     bqc.add( &bq1_low ).add( &bq2_high ).add( &bq3_notch );
 
-   // sample_timer.attach(&filter, 0.02);                //continously execute the EMG reader and filter
-    LED_timer.attach(&check_emg, 0.1);         //continously execute the motor controller
-   //calibration_timer.attach(&calibration, 0.002);              //ticker to check if EMG is being calibrated
-//    pc.printf("%d",filteredAbs);
+  
+    LED_timer_r.attach(&check_emg_r, 0.1);         //continously execute the motor controller
+    LED_timer_l.attach(&check_emg_l, 0.1);         //continously execute the motor controller
+
     
-    calibration();
+    calibration_r();
+    calibration_l();
     
     while(1){     //while loop to keep system going