Filter werkt eindelijk, echter zijn alle kanalen hetzelfde

Dependencies:   HIDScope MODSERIAL Matrix QEI biquadFilter mbed

Fork of Turning_Motor_V3 by Thom Kuenen

Files at this revision

API Documentation at this revision

Comitter:
JurrienBos
Date:
Tue Oct 30 15:41:58 2018 +0000
Parent:
6:056ad27636ff
Commit message:
Filter werkt, echter niet op alle kanalen.

Changed in this revision

Matrix.lib Show annotated file Show diff for this revision Revisions of this file
biquadFilter.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 056ad27636ff -r c5c648898881 Matrix.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Matrix.lib	Tue Oct 30 15:41:58 2018 +0000
@@ -0,0 +1,1 @@
+http://os.mbed.com/users/Yo_Robot/code/Matrix/#a4014ab0a8cf
diff -r 056ad27636ff -r c5c648898881 biquadFilter.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/biquadFilter.lib	Tue Oct 30 15:41:58 2018 +0000
@@ -0,0 +1,1 @@
+http://os.mbed.com/users/tomlankhorst/code/biquadFilter/#26861979d305
diff -r 056ad27636ff -r c5c648898881 main.cpp
--- a/main.cpp	Fri Oct 26 06:56:18 2018 +0000
+++ b/main.cpp	Tue Oct 30 15:41:58 2018 +0000
@@ -2,6 +2,7 @@
 #include "MODSERIAL.h"
 #include "HIDScope.h"
 #include "QEI.h"
+#include "BiQuad.h"
 
 MODSERIAL pc(USBTX, USBRX);
 DigitalOut DirectionPin1(D4);
@@ -17,7 +18,7 @@
 AnalogIn emg2( A2 );
 AnalogIn emg3( A3 );
 
-QEI Encoder1(D12,D13,NC,64);
+QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING);
 QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
 
 //DigitalOut LED(LED_RED);
@@ -25,7 +26,20 @@
 Ticker      StateTicker;
 Ticker      printTicker;
 
-HIDScope    scope( 4 );
+HIDScope    scope(4);
+
+BiQuadChain bqc1;
+BiQuadChain bqc2;
+BiQuadChain bqc3;
+BiQuadChain bqc4;
+BiQuadChain bqc5;
+BiQuadChain bqc6;
+BiQuadChain bqc7;
+BiQuadChain bqc8;
+BiQuad BqNotch1( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01  );
+BiQuad BqNotch2(  1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01);
+BiQuad BqHP( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 );
+BiQuad BqLP( 8.76555e-05, 1.75311e-04, 8.76555e-05, -1.97334e+00, 9.73695e-01 );
 
 volatile float Bicep_Right          = 0.0;
 volatile float Bicep_Left           = 0.0;
@@ -44,15 +58,52 @@
 volatile int counts2 ;
 volatile float rad_m1;
 volatile float rad_m2;
+volatile float q_1;
+volatile float q_2;
+volatile float r_1;
+volatile float r_2;
+volatile const float r_3            = 2.0; //
+
+volatile float FilterHP_Bi_R;
+volatile float Filterabs_Bi_R;
+volatile float Filtered_Bi_R;
+volatile float FilterHP_Bi_L;
+volatile float Filterabs_Bi_L;
+volatile float Filtered_Bi_L;
+volatile float FilterHP_Tri_R;
+volatile float Filterabs_Tri_R;
+volatile float Filtered_Tri_R;
+volatile float FilterHP_Tri_L;
+volatile float Filterabs_Tri_L;
+volatile float Filtered_Tri_L;
     
+void filter() 
+{
+    FilterHP_Bi_R = bqc1.step( emg0.read() );
+    Filterabs_Bi_R = fabs(FilterHP_Bi_R); 
+    Filtered_Bi_R = bqc2.step( Filterabs_Bi_R );
+    
+    FilterHP_Bi_L = bqc3.step( emg1.read() );
+    Filterabs_Bi_L = fabs(FilterHP_Bi_L);
+    Filtered_Bi_L = bqc4.step( Filterabs_Bi_L );
+    
+    FilterHP_Tri_R = bqc5.step( emg2.read() );
+    Filterabs_Tri_R = fabs(FilterHP_Tri_R); 
+    Filtered_Tri_R = bqc6.step( Filterabs_Tri_R );
+    
+    FilterHP_Tri_L = bqc7.step( emg3.read() );
+    Filterabs_Tri_L = fabs(FilterHP_Tri_L);
+    Filtered_Tri_L = bqc8.step( Filterabs_Tri_L );
+}
+
 void Encoding()
 {
  
     counts1 = Encoder1.getPulses();
     counts2 = Encoder2.getPulses();
     // Hier gaat iets fout waardoor het 0 wordt!!!
-    float rad_m1 = ((2.0*pi)/32.0)* (float)counts1;
-    float rad_m2 = ((2.0*pi)/32.0)* (float)counts2;
+    rad_m1 = ((2.0*pi)/32.0)* (float)counts1;
+    rad_m2 = ((2.0*pi)/32.0)* (float)counts2;
     
    // pc.printf("%f  &  %f ....\n",rad_m1, rad_m2);
 }
@@ -68,10 +119,10 @@
 void sample()
 {
     
-    scope.set(0, emg0.read() );
-    scope.set(1, emg1.read() );
-    scope.set(2, emg2.read() );
-    scope.set(3, emg3.read() );
+    scope.set(0, Filtered_Bi_R );
+    scope.set(1, Filtered_Bi_L );
+    scope.set(2, Filtered_Tri_R );
+    scope.set(3, Filtered_Tri_L );
   
     scope.send();
 }
@@ -142,6 +193,18 @@
     pc.printf("%i    %i \n",counts1,counts2);
 }
 
+void inverse_kinematics()
+{
+    float JacPs [2][2];
+    JacPs[0][0] = 2.0;
+    JacPs[0][1] = 3.0;
+    JacPs[1][0] = 4.0;
+    JacPs[1][1] = 5.0;
+    pc.printf("%f    ", JacPs[0][0]);
+}
+
+
+
 void StateMachine() 
 {
     switch (Active_State)
@@ -154,10 +217,10 @@
                     pc.printf("Entering Homing state \n");
                     Active_State = Homing;
                 }
-                
+                filter();
                 sample();
-                EMG_Read();
-                Encoding();    
+                //EMG_Read();
+                Encoding();  
                 
         break;
         
@@ -169,9 +232,9 @@
                     pc.printf("Entering Funtioning State \n");
                     Active_State = Function;
                 }
-                
+                filter();
                 sample();
-                EMG_Read();
+                //EMG_Read();
                 Encoding();
         break;
         
@@ -189,9 +252,9 @@
                     Active_State = Calibration;
                 }
                 
-                
+                filter();
                 sample();
-                EMG_Read();
+                //EMG_Read();
                 Encoding();
                 velocity1();
                 velocity2();
@@ -209,7 +272,14 @@
 {
     pc.baud(115200);    
     PwmPin1.period_us(30); //60 microseconds pwm period, 16.7 kHz 
-    
+    bqc1.add( &BqNotch1 ).add( &BqNotch2 ).add( &BqHP );
+    bqc2.add(&BqLP);
+    bqc3.add( &BqNotch1 ).add( &BqNotch2 ).add( &BqHP );
+    bqc4.add(&BqLP);
+    bqc5.add( &BqNotch1 ).add( &BqNotch2 ).add( &BqHP );
+    bqc6.add(&BqLP);
+    bqc7.add( &BqNotch1 ).add( &BqNotch2 ).add( &BqHP );
+    bqc8.add(&BqLP);
     StateTicker.attach(StateMachine, 0.002);
    
     printTicker.attach(&Printing, 2.0);