Diff: main.cpp

Revision:

9:285499f48cdd

Parent:

8:c4ec359af35d

Child:

10:518a8617c86e

--- a/main.cpp	Tue Oct 31 20:43:42 2017 +0000
+++ b/main.cpp	Wed Nov 01 13:03:26 2017 +0000
@@ -26,31 +26,70 @@
 
 //buttons  en leds voor calibration
 DigitalIn button1(PTA4);
+
 DigitalOut ledred(LED_RED); 
 DigitalOut ledblue(LED_BLUE);
+DigitalOut ledgreen(LED_GREEN);
 
-//double maxiLB = 0;
-//double maxiRB = 0;
-//double maxiLT = 0;
-//double maxiRT = 0;
+
+double MVCLB = 0;
+double MVCRB = 0;
+double MVCLT = 0;
+double MVCRT = 0;
 
 bool caldone = false; 
 int CalibrationSample = 1000; //How long will we calibrate? Timersampletime*Calibrationsample
 
-int TimescalibrationLB = 0;
-int TimescalibrationRB = 0;
-int TimescalibrationLT = 0;
-int TimescalibrationRT = 0;
+int Timescalibration = 0;
+//int TimescalibrationRB = 0;
+//int TimescalibrationLT = 0;
+//int TimescalibrationRT = 0;
 
+// Biquad filters voor Left Bicep (LB)
+// Biquad filters van respectievelijk Notch, High-pass en Low-pass filter
+BiQuad N1LB( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
+BiQuadChain NFLB;
+BiQuad HP1LB( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
+BiQuad HP2LB( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 ); 
+BiQuadChain HPFLB;
+BiQuad LP1LB( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
+BiQuad LP2LB( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
+BiQuadChain LPFLB;
+
+// Biquad filters voor Right Bicep (RB)
 // Biquad filters van respectievelijk Notch, High-pass en Low-pass filter
-BiQuad N1( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
-BiQuadChain NF;
-BiQuad HP1( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
-BiQuad HP2( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 ); 
-BiQuadChain HPF;
-BiQuad LP1( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
-BiQuad LP2( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
-BiQuadChain LPF;
+BiQuad N1RB( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
+BiQuadChain NFRB;
+BiQuad HP1RB( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
+BiQuad HP2RB( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 ); 
+BiQuadChain HPFRB;
+BiQuad LP1RB( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
+BiQuad LP2RB( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
+BiQuadChain LPFRB;
+
+// Biquad filters voor Left Tricep (LT)
+// Biquad filters van respectievelijk Notch, High-pass en Low-pass filter
+BiQuad N1LT( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
+BiQuadChain NFLT;
+BiQuad HP1LT( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
+BiQuad HP2LT( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 ); 
+BiQuadChain HPFLT;
+BiQuad LP1LT( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
+BiQuad LP2LT( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
+BiQuadChain LPFLT;
+
+// Biquad filters voor Left Tricep (RT)
+// Biquad filters van respectievelijk Notch, High-pass en Low-pass filter
+BiQuad N1RT( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
+BiQuadChain NFRT;
+BiQuad HP1RT( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
+BiQuad HP2RT( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 ); 
+BiQuadChain HPFRT;
+BiQuad LP1RT( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
+BiQuad LP2RT( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
+BiQuadChain LPFRT;
+
+
 
 Timer looptime; //moetuiteindelijk weg
 
@@ -83,10 +122,10 @@
 AnalogIn emgLT(A2);
 AnalogIn emgRT(A3);
 
-float maxiLB = 0.3;
-float maxiRB = 0.3;
-float maxiLT = 0.3;
-float maxiRT = 0.3;
+//float MVCLB = 0.3;
+//float MVCRB = 0.3;
+//float MVCLT = 0.3;
+//float MVCRT = 0.3;
 
 void Filteren() 
 {
@@ -96,28 +135,28 @@
     //EMG 1
     
     emgNotchLB = NF.step(emgLB.read() );  // Notch filter
-    emgHPLB = HPF.step(emgNotchLB);       // High-pass filter: also normalises around 0.
+    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
-    LBF = emgLPLB/maxiLB;       // Scale to maximum signal: useful for motor   
-    /*
-    emgNotchRB = NF.step(emgRB.read() );  // Notch filter
+    LBF = emgLPLB/MVCLB;       // Scale to maximum signal: useful for motor   
+    
+    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
-    RBF = emgLPRB/maxiRB;       // Scale to maximum signal: useful for motor
+    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.
     emgAbsHPLT = abs(emgHPLT);            // Take absolute value
     emgLPLT = LPF.step(emgAbsHPLT);       // Low-pass filter: creates envelope
-    LTF = emgLPLT/maxiLT;       // Scale to maximum signal: useful for motor
+    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.
     emgAbsHPRT = abs(emgHPRT);            // Take absolute value
     emgLPRT = LPF.step(emgAbsHPRT);       // Low-pass filter: creates envelope
-    RTF = emgLPRT/maxiRT;       // Scale to maximum signal: useful for motor
+    RTF = emgLPRT/MVCRT;       // Scale to maximum signal: useful for motor
         
    //if (emgFiltered >1)
     //{
@@ -127,45 +166,76 @@
     //int maxwaarde = 4096;                   // = 64x64
     //double refP = emgFiltered*maxwaarde;
     //return refP;                            // value between 0 and 4096 
- */   
+   
 }
-/*
+
 void Calibration()
 {
     Timescalibration++;
-    emgNotch = NF.step(emg.read());             // Notch filter
-    emgHP = HPF.step(emgNotch);                 // High-pass filter: also normalises around 0.
-    emgAbsHP = abs(emgHP);                      // Take absolute value
-    emgLP = LPF.step(emgAbsHP);                 // Low-pass filter: creates envelope
-    double emgfinal = emgLP;
-    if (emgfinal > maxi)
+    
+    if(Timescalibration<4000)
+    {
+     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
+    double emgfinalLB = emgLPLB;
+    if (emgfinalLB > MVCLB)
+    {                                       //determine what the highest reachable emg signal is
+          MVCLB = emgfinalLB;
+    }    
+    }
+    
+    if(Timescalibration>4000 && Timescalibration<8000)
+    {
+    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
+    double emgfinalRB = emgLPRB;
+    if (emgfinalRB > MVCRB)
     {                                       //determine what the highest reachable emg signal is
-          maxi = emgfinal;
+          MVCRB = emgfinalRB;
+    }    
+    }
+    
+    if(Timescalibration>8000 && Timescalibration<12000)
+    {
+    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
+    double emgfinalLT = emgLPLT;
+    if (emgfinalLT > MVCLT)
+    {                                       //determine what the highest reachable emg signal is
+          MVCLT = emgfinalLT;
     }    
-    if(Timescalibration>5000)
+    }
+    
+    if(Timescalibration>12000 && Timescalibration<16000)
+    {
+    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
+    double emgfinalRT = emgLPRT;
+    if (emgfinalRT > MVCRT)
+    {                                       //determine what the highest reachable emg signal is
+          MVCRT = emgfinalRT;
+    }    
+    }
+    
+    if(Timescalibration>16000)
     {
          caldone=true; 
     }
-   // pc.printf("caldone = %i , Timescalibration = %i maxi = %f \r\n",caldone,Timescalibration,maxi);
-        
-        //for(int n =0; n<CalibrationSample;n++)          //read for 5000 samples as calibration
-        //{                          
-         //   emgNotch = NF.step(emg.read());             // Notch filter
-           // emgHP = HPF.step(emgNotch);                 // High-pass filter: also normalises around 0.
-            //emgAbsHP = abs(emgHP);                      // Take absolute value
-            //emgLP = LPF.step(emgAbsHP);                 // Low-pass filter: creates envelope
-            //double emgfinal = emgLP;
-            //if (emgfinal > maxi)
-              //  {                                       //determine what the highest reachable emg signal is
-               // maxi = emgfinal;
-                //}
-           // pc.printf("maxi waarde = %f emgfinal = %f \r\n",maxi,emgfinal);
-        //}
+   // pc.printf("maxi waarde = %f emgfinal = %f \r\n",maxi,emgfinal);
+   //}
                                          //PAS ALS DEZE TRUE IS, MOET DE MOTOR PAS BEWEGEN!!!
   //return maxi;
 }
         
-*/      
+ 
 
 /*
 double Encoder ()
@@ -218,19 +288,19 @@
 {
         // hier the control of the control system
      
-    //if(caldone==false)
-    //{
-    //    if(button1.read()==false)  
-    //    {
-    //        Calibration();
-    //    }
-    //}
-    //if (caldone==true)
+    if(caldone==false)
+    {
+       if(button1.read()==false)  
+        {
+            Calibration();
+       }
+    }
+    if (caldone==true)
     
-    //{
+    {
        Filteren();
         //rest
-    //}
+    }
     
     //double Huidigepositie = Encoder(); 
     //double error = (refP - Huidigepositie);// make an error
@@ -241,16 +311,20 @@
 
 void Tickerfunctie()
 {
-    pc.printf("emgreadLB = %f , emgFiltered = %f, maxi = %f, loop = %f \r\n",emgLB.read(), LBF, maxiLB,looptime.read());
-    //emgreadRB = %f , emgFiltered = %f, maxi = %f \r\n, emgreadLT = %f , emgFiltered = %f, maxi = %f \r\n, emgreadRT = %f , emgFiltered = %f, maxi = %f \r\n",emgLB.read(), LBF, maxiLB,looptime.read(),emgRB.read(), RBF, maxiRB,emgLT.read(), LTF, maxiLT, emgRT.read(), RTF, maxiRT);
+    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);
 }
 
 int main()
 {
     //voor EMG filteren
-    NF.add( &N1 );
-    HPF.add( &HP1 ).add( &HP2 );
-    LPF.add( &LP1 ).add( &LP2 );
+    NFLB.add( &N1LB );
+    HPFLB.add( &HP1LB ).add( &HPLB );
+    LPFLB.add( &LP1LB ).add( &LP2LB );
+    
+    
     
     //voor serial
     pc.baud(115200);
@@ -260,7 +334,7 @@
     //Ticker
     Treecko.attach(MeasureAndControl, tijdstap);   //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende 
                                             //functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd.
-    printer.attach(Tickerfunctie,0.1);
+    printer.attach(Tickerfunctie,0.4);
     while(true)
     {
         }