Diff: main.cpp

Revision:

10:518a8617c86e

Parent:

9:285499f48cdd

Child:

11:b46a4c48c08f

--- a/main.cpp	Wed Nov 01 13:03:26 2017 +0000
+++ b/main.cpp	Wed Nov 01 14:58:20 2017 +0000
@@ -31,19 +31,21 @@
 DigitalOut ledblue(LED_BLUE);
 DigitalOut ledgreen(LED_GREEN);
 
+//MVC for calibration
+double MVCLB = 0; double MVCRB = 0; double MVCLT = 0; double MVCRT = 0;
+//MEAN for calibration - rest
+double RESTMEANLB = 0; double RESTMEANRB =0; double RESTMEANLT = 0; double RESTMEANRT = 0;
 
-double MVCLB = 0;
-double MVCRB = 0;
-double MVCLT = 0;
-double MVCRT = 0;
+double emgMEANSUBLB;double emgMEANSUBRB ;double emgMEANSUBLT ;double emgMEANSUBRT ;
+double emgSUMLB;double emgSUMRB;double emgSUMLT;double emgSUMRT;
+
 
 bool caldone = false; 
 int CalibrationSample = 1000; //How long will we calibrate? Timersampletime*Calibrationsample
 
 int Timescalibration = 0;
-//int TimescalibrationRB = 0;
-//int TimescalibrationLT = 0;
-//int TimescalibrationRT = 0;
+int TimescalibrationREST = 0;
+
 
 // Biquad filters voor Left Bicep (LB)
 // Biquad filters van respectievelijk Notch, High-pass en Low-pass filter
@@ -134,28 +136,32 @@
     
     //EMG 1
     
-    emgNotchLB = NF.step(emgLB.read() );  // Notch filter
-    emgHPLB = HPF.step(emgNotchLB);         // High-pass filter: also normalises around 0.
+    emgNotchLB = NFLB.step(emgLB.read() );  // Notch filter
+    emgHPLB = HPFLB.step(emgNotchLB);         // High-pass filter: also normalises around 0.
     emgAbsHPLB = abs(emgHPLB);            // Take absolute value
-    emgLPLB = LPF.step(emgAbsHPLB);       // Low-pass filter: creates envelope
-    LBF = emgLPLB/MVCLB;       // Scale to maximum signal: useful for motor   
+    emgLPLB = LPFLB.step(emgAbsHPLB);       // Low-pass filter: creates envelope
+    emgMEANSUBLB = emgLPLB - RESTMEANLB;    //substract the restmean value
+    LBF = emgLPLB/MVCLB;       // Scale to maximum signal: useful for motor. LBF should now be between 0-1.
     
-    emgNotchRB = NF.step(emgRB.read());  // Notch filter
-    emgHPRB = HPF.step(emgNotchRB);       // High-pass filter: also normalises around 0.
+    emgNotchRB = NFRB.step(emgRB.read());  // Notch filter
+    emgHPRB = HPFRB.step(emgNotchRB);       // High-pass filter: also normalises around 0.
     emgAbsHPRB = abs(emgHPRB);            // Take absolute value
-    emgLPRB = LPF.step(emgAbsHPRB);       // Low-pass filter: creates envelope
+    emgLPRB = LPFRB.step(emgAbsHPRB);       // Low-pass filter: creates envelope
+    emgMEANSUBLB = emgLPLB - RESTMEANLB;
     RBF = emgLPRB/MVCRB;       // Scale to maximum signal: useful for motor
     
-    emgNotchLT = NF.step(emgLT.read() );  // Notch filter
-    emgHPLT = HPF.step(emgNotchLT);       // High-pass filter: also normalises around 0.
+    emgNotchLT = NFLT.step(emgLT.read() );  // Notch filter
+    emgHPLT = HPFLT.step(emgNotchLT);       // High-pass filter: also normalises around 0.
     emgAbsHPLT = abs(emgHPLT);            // Take absolute value
-    emgLPLT = LPF.step(emgAbsHPLT);       // Low-pass filter: creates envelope
+    emgLPLT = LPFLT.step(emgAbsHPLT);       // Low-pass filter: creates envelope
+    emgMEANSUBLT = emgLPLT - RESTMEANLT;    //substract the restmean value
     LTF = emgLPLT/MVCLT;       // Scale to maximum signal: useful for motor
     
-    emgNotchRT = NF.step(emgRT.read() );  // Notch filter
-    emgHPRT = HPF.step(emgNotchRT);       // High-pass filter: also normalises around 0.
+    emgNotchRT = NFRT.step(emgRT.read() );  // Notch filter
+    emgHPRT = HPFRT.step(emgNotchRT);       // High-pass filter: also normalises around 0.
     emgAbsHPRT = abs(emgHPRT);            // Take absolute value
-    emgLPRT = LPF.step(emgAbsHPRT);       // Low-pass filter: creates envelope
+    emgLPRT = LPFRT.step(emgAbsHPRT);       // Low-pass filter: creates envelope
+    emgMEANSUBRT = emgLPRT - RESTMEANRT;    //substract the restmean value
     RTF = emgLPRT/MVCRT;       // Scale to maximum signal: useful for motor
         
    //if (emgFiltered >1)
@@ -169,16 +175,50 @@
    
 }
 
-void Calibration()
+void CalibrationEMG()
 {
     Timescalibration++;
     
-    if(Timescalibration<4000)
+    if(Timescalibration<2000)
     {
-     emgNotchLB = NF.step(emgLB.read() );  // Notch filter
-    emgHPLB = HPF.step(emgNotchLB);         // High-pass filter: also normalises around 0.
-    emgAbsHPLB = abs(emgHPLB);            // Take absolute value
-    emgLPLB = LPF.step(emgAbsHPLB);       // Low-pass filter: creates envelope
+        
+    emgNotchLB = NFLB.step(emgLB.read() );
+    emgHPLB = HPFLB.step(emgNotchLB);   
+    emgAbsHPLB = abs(emgHPLB);
+    emgLPLB = LPFLB.step(emgAbsHPLB);
+    emgSUMLB += emgLPLB;                         //SUM all rest values LB
+    
+     emgNotchRB = NFRB.step(emgRB.read()); 
+    emgHPRB = HPFRB.step(emgNotchRB);
+    emgAbsHPRB = abs(emgHPRB);
+    emgLPRB = LPFRB.step(emgAbsHPRB);
+    emgSUMRB += emgLPRB;                        //SUM all rest values RB
+    
+    emgNotchLT = NFLT.step(emgLT.read() );
+    emgHPLT = HPFLT.step(emgNotchLT);
+    emgAbsHPLT = abs(emgHPLT);
+    emgLPLT = LPFLT.step(emgAbsHPLT);
+    emgSUMLT += emgLPLT;                         //SUM all rest values LT
+    
+    emgNotchRT = NFRT.step(emgRT.read() );  
+    emgHPRT = HPFRT.step(emgNotchRT);       
+    emgAbsHPRT = abs(emgHPRT);           
+    emgLPRT = LPFRT.step(emgAbsHPRT);
+    emgSUMRT += emgLPRT;                         //SUM all rest values RT
+    }
+    if(Timescalibration==1999)
+    {
+        RESTMEANLB = emgSUMLB/Timescalibration; //determine the mean rest value
+        RESTMEANRB = emgSUMRB/Timescalibration; //determine the mean rest value
+        RESTMEANRT = emgSUMRT/Timescalibration; //determine the mean rest value
+        RESTMEANLT = emgSUMLT/Timescalibration; //determine the mean rest value
+    }
+    if(Timescalibration>2000 && Timescalibration<6000)
+    {
+    emgNotchLB = NFLB.step(emgLB.read() ); 
+    emgHPLB = HPFLB.step(emgNotchLB);        
+    emgAbsHPLB = abs(emgHPLB);          
+    emgLPLB = LPFLB.step(emgAbsHPLB);       
     double emgfinalLB = emgLPLB;
     if (emgfinalLB > MVCLB)
     {                                       //determine what the highest reachable emg signal is
@@ -186,12 +226,12 @@
     }    
     }
     
-    if(Timescalibration>4000 && Timescalibration<8000)
+    if(Timescalibration>6000 && Timescalibration<10000)
     {
-    emgNotchRB = NF.step(emgRB.read());  // Notch filter
-    emgHPRB = HPF.step(emgNotchRB);       // High-pass filter: also normalises around 0.
-    emgAbsHPRB = abs(emgHPRB);            // Take absolute value
-    emgLPRB = LPF.step(emgAbsHPRB);       // Low-pass filter: creates envelope
+    emgNotchRB = NFRB.step(emgRB.read()); 
+    emgHPRB = HPFRB.step(emgNotchRB); 
+    emgAbsHPRB = abs(emgHPRB);           
+    emgLPRB = LPFRB.step(emgAbsHPRB);      
     double emgfinalRB = emgLPRB;
     if (emgfinalRB > MVCRB)
     {                                       //determine what the highest reachable emg signal is
@@ -199,12 +239,12 @@
     }    
     }
     
-    if(Timescalibration>8000 && Timescalibration<12000)
+    if(Timescalibration>10000 && Timescalibration<14000)
     {
-    emgNotchLT = NF.step(emgLT.read() );  // Notch filter
-    emgHPLT = HPF.step(emgNotchLT);       // High-pass filter: also normalises around 0.
-    emgAbsHPLT = abs(emgHPLT);            // Take absolute value
-    emgLPLT = LPF.step(emgAbsHPLT);       // Low-pass filter: creates envelope
+    emgNotchLT = NFLT.step(emgLT.read() );
+    emgHPLT = HPFLT.step(emgNotchLT);
+    emgAbsHPLT = abs(emgHPLT);
+    emgLPLT = LPFLT.step(emgAbsHPLT);
     double emgfinalLT = emgLPLT;
     if (emgfinalLT > MVCLT)
     {                                       //determine what the highest reachable emg signal is
@@ -212,12 +252,12 @@
     }    
     }
     
-    if(Timescalibration>12000 && Timescalibration<16000)
+    if(Timescalibration>14000 && Timescalibration<18000)
     {
-    emgNotchRT = NF.step(emgRT.read() );  // Notch filter
-    emgHPRT = HPF.step(emgNotchRT);       // High-pass filter: also normalises around 0.
-    emgAbsHPRT = abs(emgHPRT);            // Take absolute value
-    emgLPRT = LPF.step(emgAbsHPRT);       // Low-pass filter: creates envelope
+    emgNotchRT = NFRT.step(emgRT.read() );  
+    emgHPRT = HPFRT.step(emgNotchRT);       
+    emgAbsHPRT = abs(emgHPRT);           
+    emgLPRT = LPFRT.step(emgAbsHPRT);
     double emgfinalRT = emgLPRT;
     if (emgfinalRT > MVCRT)
     {                                       //determine what the highest reachable emg signal is
@@ -225,7 +265,7 @@
     }    
     }
     
-    if(Timescalibration>16000)
+    if(Timescalibration>18000)
     {
          caldone=true; 
     }
@@ -292,7 +332,7 @@
     {
        if(button1.read()==false)  
         {
-            Calibration();
+            CalibrationEMG();
        }
     }
     if (caldone==true)
@@ -311,20 +351,34 @@
 
 void Tickerfunctie()
 {
-    pc.printf("emgreadRB = %f , emgFiltered = %f, maxi = %f \r\n",emgRB.read(), RBF, MVCRB);
-    pc.printf("emgreadLB = %f , emgFiltered = %f, maxi = %f, loop = %f \r\n",emgLB.read(), LBF, MVCLB);
-    pc.printf("emgreadRT = %f , emgFilteredRT = %f, maxiRT = %f \r\n",emgRT.read(), RTF, MVCRT);
-    pc.printf("emgreadLT = %f , emgFilteredLT = %f, maxiLT = %f \r\n",emgLT.read(), LTF, MVCLT);
+    pc.printf("emgreadRB = %f , emgFiltered = %f, maxi = %f meanrest = %f\r\n",emgRB.read(), RBF, MVCRB, RESTMEANLB);
+    pc.printf("emgreadLB = %f , emgFiltered = %f, maxi = %f, loop = %f meanrest = %f \r\n",emgLB.read(), LBF, MVCLB,RESTMEANRB);
+    pc.printf("emgreadRT = %f , emgFilteredRT = %f, maxiRT = %f meanrest = %f \r\n",emgRT.read(), RTF, MVCRT,RESTMEANRT);
+    pc.printf("emgreadLT = %f , emgFilteredLT = %f, maxiLT = %f meanrest = %f \r\n",emgLT.read(), LTF, MVCLT,RESTMEANLT);
 }
 
 int main()
 {
     //voor EMG filteren
+    //Left Bicep
     NFLB.add( &N1LB );
-    HPFLB.add( &HP1LB ).add( &HPLB );
+    HPFLB.add( &HP1LB ).add( &HP2LB );
     LPFLB.add( &LP1LB ).add( &LP2LB );
     
+    //Right Bicep
+    NFRB.add( &N1RB );
+    HPFRB.add( &HP1RB ).add( &HP2RB );
+    LPFRB.add( &LP1RB ).add( &LP2RB );  
     
+    //Left Tricep
+    NFLT.add( &N1LT );
+    HPFLT.add( &HP1LT ).add( &HP2LT );
+    LPFLT.add( &LP1LT ).add( &LP2LT ); 
+    
+    //Right Tricep
+    NFRT.add( &N1RT );
+    HPFRT.add( &HP1RT ).add( &HP2RT );
+    LPFRT.add( &LP1RT ).add( &LP2RT ); 
     
     //voor serial
     pc.baud(115200);