Diff: main.cpp

Revision:

7:c5c648898881

Parent:

6:056ad27636ff

Child:

8:b6b09226a421

--- 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);