Diff: main.cpp

Revision:

8:c4ec359af35d

Parent:

7:05c71a859d27

Child:

9:285499f48cdd

--- a/main.cpp	Tue Oct 31 15:42:05 2017 +0000
+++ b/main.cpp	Tue Oct 31 20:43:42 2017 +0000
@@ -7,27 +7,40 @@
 
 //globalvariables Motor
 Ticker Treecko;             //We make a awesome ticker for our control system
-PwmOut M1E(D6);             //Biorobotics Motor 1 PWM control of the speed 
-DigitalOut M1D(D7);         //Biorobotics Motor 1 diraction control
+Ticker printer;
+//PwmOut M1E(D6);             //Biorobotics Motor 1 PWM control of the speed 
+//DigitalOut M1D(D7);         //Biorobotics Motor 1 diraction control
 
-Encoder motor1(D13,D12,true);
+//Encoder motor1(D13,D12,true);
 MODSERIAL pc(USBTX,USBRX);
 
-double PwmPeriod = 1.0/5000.0;           //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde)
+//double PwmPeriod = 1.0/5000.0;           //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde)
 const double Ts = 0.1;                   // tickettijd/ sample time
-double e_prev = 0; 
-double e_int = 0;
+//double e_prev = 0; 
+//double e_int = 0;
 double tijdstap = 0.002;
-volatile double emgFiltered;
+volatile double LBF;
+volatile double RBF;
+volatile double LTF;
+volatile double RTF;
 
 //buttons  en leds voor calibration
 DigitalIn button1(PTA4);
-DigitalOut ledred(LED_RED);
+DigitalOut ledred(LED_RED); 
 DigitalOut ledblue(LED_BLUE);
-double maxi = 0;
+
+//double maxiLB = 0;
+//double maxiRB = 0;
+//double maxiLT = 0;
+//double maxiRT = 0;
+
 bool caldone = false; 
-int CalibrationSample = 5000; //How long will we calibrate? Timersampletime*Calibrationsample
-int Timescalibration = 0;
+int CalibrationSample = 1000; //How long will we calibrate? Timersampletime*Calibrationsample
+
+int TimescalibrationLB = 0;
+int TimescalibrationRB = 0;
+int TimescalibrationLT = 0;
+int TimescalibrationRT = 0;
 
 // 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 );
@@ -42,39 +55,81 @@
 Timer looptime; //moetuiteindelijk weg
 
 //filters
-double emgNotch;
-double emgHP;
-double emgAbsHP;
-double emgLP;
-double emgMax; //magdezeweg??
+double emgNotchLB;
+double emgHPLB;
+double emgAbsHPLB;
+double emgLPLB;
+
+double emgNotchRB;
+double emgHPRB;
+double emgAbsHPRB;
+double emgLPRB;
 
+double emgNotchLT;
+double emgHPLT;
+double emgAbsHPLT;
+double emgLPLT;
 
+double emgNotchRT;
+double emgHPRT;
+double emgAbsHPRT;
+double emgLPRT;
 
 double f = 500;       // frequency
 double dt = 1/f;      // sample frequency
-AnalogIn emg(A0);   // EMG lezen
+
+AnalogIn emgLB(A0);   // EMG lezen
+AnalogIn emgRB(A1);
+AnalogIn emgLT(A2);
+AnalogIn emgRT(A3);
 
-double Filteren() 
+float maxiLB = 0.3;
+float maxiRB = 0.3;
+float maxiLT = 0.3;
+float maxiRT = 0.3;
+
+void Filteren() 
 {
     looptime.reset();
     looptime.start();
-    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
-    emgFiltered = emgLP/maxi;       // Scale to maximum signal: useful for motor
-   
-    /*if (emgFiltered >1)
-    {
-        emgFiltered=1.00;
-    } */
-pc.printf("emgread = %f , emgFiltered = %f, loop = %f, maxi = %f \r\n",emg.read(), emgFiltered, looptime.read(),maxi);
+    
+    //EMG 1
+    
+    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
+    LBF = emgLPLB/maxiLB;       // 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
+    
+    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
+    
+    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
+        
+   //if (emgFiltered >1)
+    //{
+    //    emgFiltered=1.00;
+    //}
+//pc.printf("emgreadLB = %f , emgFiltered = %f, maxi = %f, loop = %f \r\n, 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);
     //int maxwaarde = 4096;                   // = 64x64
     //double refP = emgFiltered*maxwaarde;
     //return refP;                            // value between 0 and 4096 
-     return emgFiltered;
+ */   
 }
-
+/*
 void Calibration()
 {
     Timescalibration++;
@@ -110,7 +165,7 @@
   //return maxi;
 }
         
-        
+*/      
 
 /*
 double Encoder ()
@@ -163,19 +218,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)
     
-    {
-        double EMGfilter = Filteren();
+    //{
+       Filteren();
         //rest
-    }
+    //}
     
     //double Huidigepositie = Encoder(); 
     //double error = (refP - Huidigepositie);// make an error
@@ -184,6 +239,11 @@
     //SetMotor1(motorValue);
 }
 
+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);
+}
 
 int main()
 {
@@ -196,11 +256,11 @@
     pc.baud(115200);
     
     //motor
-    M1E.period(PwmPeriod); //set PWMposition at 5000hz
+   // M1E.period(PwmPeriod); //set PWMposition at 5000hz
     //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);
     while(true)
     {
         }