Diff: main.cpp

Revision:

4:8ed071e5e3c9

Parent:

3:3e5d899a3c8a

Child:

5:41d4aac93351

--- a/main.cpp	Thu Oct 12 13:26:02 2017 +0000
+++ b/main.cpp	Fri Oct 13 11:57:15 2017 +0000
@@ -10,67 +10,89 @@
 PwmOut      motor1MagnitudePin(D5);  //Motorkracht op D5 (connected op het bord)
 Timer       timer;                   //timer voor duur script        
 HIDScope    scope(2);                //Maakt de scopes aan   
+int n=0;
 
 //Benoemen van de variabelen die in de VOID's gebruikt gaan worden
 double emga = emg0.read();          //EMG1
 double emgb = emg1.read();          //EMG2
 
-//Aanmaken filter variabelen
-double ah[3]={1, 0, 0.1716};
-double bh[3]={0.2929, -0.5858, 0.2929};
+// coëfficienten notch filter
+double an[3]={1.0000, -0.0000, 0.9561};
+double bn[3]={0.9780, -0.0000, 0.9780};
+
+// innitial conditions notch filter
+double emg_nf[3]={0,0,0};
+double emg_in[3]={0, 0, 0};
+
+//Aanmaken filter coëfficienten
+double ah[3]={1.0000, -1.9778, 0.9780};
+double bh[3]={0.9890, -1.9779, 0.9890};
 
 //innitial conditions high pass filter
 double emg_hpf[3]={0, 0, 0};
-double emg_in[3]={0, 0, 0};
 
-// coëfficienten high pass filter
-double al[3]={1, -1.7347, 0.7660};
-double bl[3]={0.0078, 0.0156, 0.0078};
+// coëfficienten low pass filter
+double al[5]={1.0000, -3.5897, 4.8513, -2.9241, 0.6630};
+double bl[5]={0.0000312, 0.0001250, 0.0001874, 0.0001250, 0.0000312};
 
 //innitial conditions low pass filter
-double emg_lpf[3]={0, 0, 0};
-double emg_abs[3]={0, 0, 0};
+double emg_lpf[5]={0, 0, 0, 0, 0};
+double emg_abs[5]={0, 0, 0, 0, 0};
 
 double emg_lpfg;
 
 //Aanmaken van de verschillende tickers
 Ticker tick_sample;
 
+
 void aansturing()
     {
-    timer.reset();
-    timer.start();  
+    timer.reset(); 
 
         emga = emg0.read();
         emgb = emg1.read();
         emg_in[0]=emga-emgb;
         
-            //Filter
+            //Filter cascade
+                    // notch filter
+                    emg_nf[0]=bn[0]*emg_in[0]    +bn[1]*emg_in[1]  +bn[2]*emg_in[2]  -an[1]*emg_nf[1]  -an[2]*emg_nf[2];
+                                            
                     // high pass filter
-                    emg_hpf[0]=bh[0]*emg_in[0]    +bh[1]*emg_in[1]  +bh[2]*emg_in[2]  -ah[1]*emg_hpf[1]  -ah[2]*emg_hpf[2];
+                    emg_hpf[0]=bh[0]*emg_nf[0]    +bh[1]*emg_nf[1]  +bh[2]*emg_nf[2]  -ah[1]*emg_hpf[1]  -ah[2]*emg_hpf[2];
         
-                    emg_in[2]=emg_in[1];
-                    emg_in[1]=emg_in[0];
-                    emg_hpf[2]=emg_hpf[1];
-                    emg_hpf[2]=emg_hpf[0];
-        
-
                     //absolute value
                     emg_abs[0]=fabs(emg_hpf[0]);
-        
-        
+               
                     //low pass filter
-                    emg_lpf[0]=bl[0]*emg_abs[0]   +bl[1]*emg_abs[1] +bl[2]*emg_abs[2] -al[1]*emg_lpf[1] -al[2]*emg_lpf[2];
+                    emg_lpf[0]=bl[0]*emg_abs[0]   +bl[1]*emg_abs[1] +bl[2]*emg_abs[2] +bl[3]*emg_abs[3] +bl[4]*emg_abs[4] -al[1]*emg_lpf[1] -al[2]*emg_lpf[2] -al[3]*emg_lpf[3] -al[4]*emg_lpf[4];
         
+                    // RAM
+                    emg_in[2]=emg_in[1];
+                    emg_in[1]=emg_in[0];
+                    
+                    emg_nf[2]=emg_nf[1];
+                    emg_nf[1]=emg_nf[0];
+                    
+                    emg_hpf[2]=emg_hpf[1];
+                    emg_hpf[1]=emg_hpf[0];
+                    
+                    emg_abs[4]=emg_abs[3];
+                    emg_abs[3]=emg_abs[2];
                     emg_abs[2]=emg_abs[1];
                     emg_abs[1]=emg_abs[0];
+                    
+                    emg_lpf[4]=emg_lpf[3];
+                    emg_lpf[3]=emg_lpf[2];
                     emg_lpf[2]=emg_lpf[1];
                     emg_lpf[1]=emg_lpf[0];
-                    emg_lpfg = 5* emg_lpf[1];
+                    
+                      
+                    
+
                 
-        if (emg_lpf[1]>0.05)
+        if (emg_lpf[0]>0.01)
         {
-        motor1MagnitudePin = emg_lpfg;
+        motor1MagnitudePin = emg_lpf[0];
         motor1DirectionPin = 0;
         }
         else 
@@ -79,18 +101,26 @@
         motor1DirectionPin = 0;
         }
     scope.set(0, emg_in[0]);
-    scope.set(1, emg_lpfg);
+    scope.set(1, emg_lpf[0]);
     scope.send();
-
-        timer.stop();   
-    pc.printf("time taken was %d milliseconds\n\r", timer.read_us());
+  
+    if (n==100)
+    {
+    pc.printf("time taken was %d microseconds\n\r", timer.read_us());
+    n=0;
+    }
+    else 
+    {
+     n=n+1;       
+    }  
         
-    }
+}
 
 
 int main()
 {
     //Deze tickers roepen de verschillende voids aan
     pc.baud(115200);
+    timer.start(); 
     tick_sample.attach_us(&aansturing, 5000);    //Deze ticker roept de potmeter aan
 }