Revision 10:05ad15c48388, committed 2018-10-31

Comitter:

ThomBMT

Date:

Wed Oct 31 22:52:09 2018 +0000

Parent:

9:355dd95199c3

Child:

11:d525527c0b7d

Commit message:

Working version;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/biquadFilter.lib	Wed Oct 31 22:52:09 2018 +0000
@@ -0,0 +1,1 @@
+http://os.mbed.com/users/tomlankhorst/code/biquadFilter/#26861979d305

--- a/main.cpp	Tue Oct 30 13:57:00 2018 +0000
+++ b/main.cpp	Wed Oct 31 22:52:09 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);
@@ -25,19 +26,66 @@
 QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING);
 QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
 
+BiQuadChain bqc1;
+BiQuadChain bqc2;
+BiQuadChain bqc3;
+BiQuadChain bqc4;
+BiQuadChain bqc5;
+BiQuadChain bqc6;
+BiQuadChain bqc7;
+BiQuadChain bqc8;
+
+BiQuad      BqNotch1_1( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01  );
+BiQuad      BqNotch2_1(  1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01);
+BiQuad      BqNotch1_2( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01  );
+BiQuad      BqNotch2_2(  1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01);
+BiQuad      BqNotch1_3( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01  );
+BiQuad      BqNotch2_3(  1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01);
+BiQuad      BqNotch1_4( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01  );
+BiQuad      BqNotch2_4(  1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01);
+BiQuad      BqHP1( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 );
+BiQuad      BqHP2( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 );
+BiQuad      BqHP3( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 );
+BiQuad      BqHP4( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 );
+
+BiQuad      BqLP1( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
+BiQuad      BqLP2( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
+BiQuad      BqLP3( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
+BiQuad      BqLP4( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
+
+BiQuad      LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+
 Ticker      StateTicker;
 Ticker      printTicker;
 
 HIDScope    scope( 4 );
 
-volatile float Bicep_Right          = 0.0;
-volatile float Bicep_Left           = 0.0;
-volatile float Tricep_Right         = 0.0;
-volatile float Tricep_Left          = 0.0;
+volatile float      Bicep_Right          = 0.0;
+volatile float      Bicep_Left           = 0.0;
+volatile float      Tricep_Right         = 0.0;
+volatile float      Tricep_Left          = 0.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;
+
+volatile float error_1_integral = 0;
+volatile float error_2_integral = 0;
+volatile float error_1_prev; // initialization with this value only done once!
+volatile float error_2_prev;
 
 volatile const float pi             = 3.1415926; 
 volatile const float rad_count      = 0.0007479;                                // 2pi/8400;
 volatile const float maxVelocity    = 2.0f * pi;                                // in rad/s
+volatile const float r_3            = 0.035;                                
 
 volatile float referenceVelocity1   = 0.5;                                      // dit is de gecentreerde waarde en dus de nulstand
 volatile float referenceVelocity2   = 0.5;
@@ -46,11 +94,19 @@
 volatile int ii  = 0;                                                           // Calibratie timer
 volatile int iii = 0;                                                           // Start up timer
 
+volatile float Kp = 17.5;
+volatile float Ki = 1.02;
+volatile float Kd = 23.2;
+volatile float Ts = 0.01; // Sample time in seconds
+volatile float error_1;
+volatile float error_2;
+volatile float U_1;
+volatile float U_2; 
+
 volatile float q_1;
 volatile float q_2;
 volatile float r_1;
 volatile float r_2;
-volatile const float r_3 = 0.035;
 volatile float w_1;
 volatile float w_2;
 
@@ -58,19 +114,20 @@
 volatile float Flex_Bi_L;
 volatile float Flex_Tri_R;
 volatile float Flex_Tri_L;
+volatile float Threshold_Value;
 volatile float Threshold_Bi_R;
 volatile float Threshold_Bi_L;
 volatile float Threshold_Tri_R;
 volatile float Threshold_Tri_L;
 
-enum states{Starting, Calibration, Homing, Function};
+enum states{Starting, Calibration, Homing_M1, Homing_M2, Post_Homing, Function, Safe};
     
 volatile states Active_State = Starting;
 
 volatile float vx;
 volatile float vy;
-volatile int counts1 ;
-volatile int counts2 ;
+volatile int counts1;
+volatile int counts2;
 volatile float rad_m1;
 volatile float rad_m2;
     
@@ -84,43 +141,66 @@
     rad_m2 = rad_count * (float)counts2;
 }
 
+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 BlinkLed()
 {
     if(i==1)
     {
+            led_G=led_B=1;
             static int rr = 0;
             rr++;
             if (rr == 1)
             {
                 led_R = !led_R;
             }
-            else if (rr == 100)
+            else if (rr == 50)
             {
                 rr = 0;
             }
     }     
     else if(i==2)
     {        
+            led_R=led_B=1;
+            
             static int gg = 0;
             gg++;
             if (gg == 1)
             {
                 led_G = !led_G;
             }
-            else if (gg == 100)
+            else if (gg == 250)
             { 
                 gg = 0;
             }
     }
     else if (i==3)
     {            
+            led_R=1;
             static int bb = 0;
             bb++;
             if (bb == 1)
             {
                 led_B = !led_B;
             }
-            else if (bb == 100)
+            else if (bb == 500)
             {
                 bb = 0;
             }
@@ -142,18 +222,18 @@
 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*10.0f );
+    scope.set(1, Filtered_Bi_L*10.0f );
+    scope.set(2, Filtered_Tri_R*10.0f );
+    scope.set(3, Filtered_Tri_L*10.0f );
   
     scope.send();
 }
 
 void Inverse()
 {
-    q_1= rad_m1;                                                                // uit Encoder
-    q_2= rad_m2+(pi/2.0f);                                                      // uit Encoder
+    q_1= rad_m1+(pi/6.0f);                                                       // uit Encoder
+    q_2= rad_m2+(pi/6.0f);                                                      // uit Encoder
     r_1= -0.2f;
     r_2= -0.2f;
     
@@ -162,27 +242,60 @@
         float y = r_2*cos(q_1)*cos(q_2)-r_2*sin(q_1)*sin(q_2)+2.0f*(r_1*cos(q_1))-r_3;
         float x = (-2.0f)*r_2*sin(q_1)*cos(q_2);
         float D = 1.0f/(u*z-x*y);                                               // Determinant
-        printf("Determinant is %f\r\n", D);
+        //printf("Determinant is %f\r\n", D);
         
         float a = D*z;                                                          // Inverse jacobian a,b,c,d vormen 2 bij 2 matrix
         float b = -D*x;                                                         // Inverse jacobian
         float c = -D*y;                                                         // Inverse jacobian
         float d = D*u;                                                          // Inverse jacobian
         
-        float vx = pot1.read();//0.01f;                                                       // uit emg data
-        float vy = pot2.read();//0.0f;                                                        // uit emg data
+        vx = pot1.read()/5.0f;                                                 // uit emg data
+        vy = pot2.read()/5.0f;                                                 // uit emg data
         
         w_1 = vx*a+vy*b;
         w_2 = vx*c+vy*d;
         
         /*
-        printf("%f\r\n", w_1);
-        printf("%f\r\n", w_2);
-        */     
+        printf("%f\r\n", vx);
+        printf("%f\r\n", vy);
+          */   
 }
-/*   
+
+void PID_controller()
+{
+      error_1 = (w_1*0.002f) - rad_m1;
+      error_2 = (w_2*0.002f) - rad_m2;
+      
+      error_1_prev = error_1;
+      error_2_prev = error_2;
+    
+      // Proportional part:
+      float u_k_1 = Kp * error_1;
+      float u_k_2 = Kp * error_2;
+    
+      // Integral part
+      error_1_integral = error_1_integral + error_1 * Ts;
+      error_2_integral = error_2_integral + error_2 * Ts;
+      float u_i_1 = Ki * error_1_integral;
+      float u_i_2 = Ki * error_2_integral;
+    
+      // Derivative part
+      float error_1_derivative = (error_1 - error_1_prev)/Ts;
+      float error_2_derivative = (error_2 - error_2_prev)/Ts;
+      float filtered_error_1_derivative = LowPassFilter.step(error_1_derivative);
+      float filtered_error_2_derivative = LowPassFilter.step(error_2_derivative);
+      float u_d_1 = Kd * filtered_error_1_derivative;
+      float u_d_2 = Kd * filtered_error_2_derivative;
+      error_1_prev = error_1;
+      error_2_prev = error_2;
+    
+      // Sum all parts and return it
+      U_1 = u_k_1 + u_i_1 + u_d_1;   
+      U_2 = u_k_2 + u_i_2 + u_d_2;
+}
+  
 void velocity1()
-    {
+{
             if (pot1.read()>0.5f)
                 {
                 // Clockwise rotation
@@ -199,10 +312,10 @@
                 // Counterclockwise rotation      
                 referenceVelocity1 = 2.0f * (pot1.read()-0.5f) ;
                 }
-    }
+}
     
 void velocity2()
-    {
+{
             if (pot2.read()>0.5f)
                 {
                 // Clockwise rotation
@@ -219,23 +332,23 @@
                 // Counterclockwise rotation      
                 referenceVelocity2 = 2.0f * (pot2.read()-0.5f) ;
                 }
-    }    
-*/    
+}    
+    
 void motor1()
-    {          
-        float u_v1 = w_1; //referenceVelocity1       
-        float u = u_v1/ (2.0f * pi);
+{          
+        float u_v1 = referenceVelocity1             ;                            //w_1
+        float u = u_v1;// (2.0f * pi);
         DirectionPin1 = u < 0.0f;
         PwmPin1 = fabs(u);
-    }
+}
 
 void motor2()
-    {  
-        float u_v2 = w_2; //referenceVelocity2
-        float u = u_v2/ (2.0f * pi);
+{  
+        float u_v2 = referenceVelocity2              ;                           //w_2
+        float u = u_v2;// (2.0f * pi);
         DirectionPin2 = u > 0.0f;
         PwmPin2 = fabs(u);
-    }
+}
    
 void Calibrating()
 {
@@ -252,78 +365,93 @@
                 if (ii == 0)
                 {
                     pc.printf("Relax your muscles please. \r\n");
+                    i = 2;
                 }
                 else if (ii == 2250)
                 {
                     pc.printf("Flex your right bicep now please.\r\n");
+                    i = 3; 
                 }
-                //chillen
+                                                                                //chillen
             }
             else if (ii>2500 && ii<5000) //
             {
-                n = n + emg0.read();// dit wordt de variable naam na het filter
+                n = n + Filtered_Bi_R;                                            // dit wordt de variable naam na het filter
+                i = 1;
             }
             else if (ii == 5000)
             {
                 Flex_Bi_R = n / 2500.0f;
                 pc.printf("You can relax your right bicep, thank you. \r\nYour mean flexing value was %f\r\n\r\n", Flex_Bi_R);
+                i = 2;
             }
             else if (ii>5000 && ii<=6000)
             {
                 if (ii == 5750)
                 {
                     pc.printf("Flex your left bicep now please. \r\n");
+                    i = 3;
                 }
                 //chillen
             }
             else if(ii>6000 && ii<8500)
             {
-                m = m + emg1.read();
+                m = m + Filtered_Bi_L;
+                i = 1;
             }
             else if (ii == 8500)
             {
                 Flex_Bi_L = m / 2500.0f;
                 pc.printf("You can relax your left bicep, thank you. \r\nYour mean flexing value was %f\r\n\r\n", Flex_Bi_L);
+                i = 2;
             }
             else if (ii>8500 && ii<=9500)
             {
                 if (ii == 9250)
                 {
                     pc.printf("Flex your right tricep now please. \r\n");
+                    i = 3;
                 }
                 //chillen
             }
             else if (ii>9500 && ii<12000)
             {
-                l = l + emg2.read();
+                l = l + Filtered_Tri_R;
+                i = 1;
             }
             else if (ii == 12000)
             {
                 Flex_Tri_R = l / 2500.0f;
                 pc.printf("You can relax your right tricep, thank you. \r\nYour mean flexing value was %f\r\n\r\n", Flex_Tri_R);
+                i = 2;
             }
             else if (ii>12000 && ii <=13000)
             {
                 if (ii == 12750)
                 {
                     pc.printf("Flex your left tricep now please. \r\n");
+                    i = 3; 
                 }
                 //chillen
             }
             else if (ii>13000 && ii<15500)
             {
-                k = k + emg3.read();
+                k = k + Filtered_Tri_L;
+                i = 1;
             }
             else if (ii == 15500)
             {
                 Flex_Tri_L = k / 2500.0f;
                 pc.printf("You can relax your left tricep, thank you. \r\nYour mean flexing value was %f\r\n\r\nThe calibration has been completed, the system is now operatable. \r\n",Flex_Tri_L);
+                i = 2;
             }
+
+Threshold_Value = 0.8f;
             
-Threshold_Bi_R  = 0.75f * Flex_Bi_R;
-Threshold_Bi_L  = 0.75f * Flex_Bi_L;
-Threshold_Tri_R = 0.75f * Flex_Tri_R;
-Threshold_Tri_L = 0.75f * Flex_Tri_L;                  
+Threshold_Bi_R  = Threshold_Value * Flex_Bi_R;
+Threshold_Bi_L  = Threshold_Value * Flex_Bi_L;
+Threshold_Tri_R = Threshold_Value * Flex_Tri_R;
+Threshold_Tri_L = Threshold_Value * Flex_Tri_L;                  
 
             if (ii == 16500)
             {
@@ -332,17 +460,20 @@
             else if (ii == 20000)
             {
                 pc.printf("\r\nAutomatic switch to Homing State\r\n");
-                Active_State = Homing;
+                Active_State = Homing_M1;
+                i = 0;
             }
 }
 
+
+
 void Start_Up()
 {
     i++;
     iii++;
     if (iii == 1)
     {
-        pc.printf("System is starting...\r\nWaiting for further input...\r\n");
+        pc.printf("\r\n\r\nSystem is starting...\r\nWaiting for further input...\r\n");
     }
     
     else if (iii == 30000)
@@ -361,49 +492,48 @@
     }
 }
 
-void OFF()
+void OFF_m1()
 {
-    PwmPin1 = PwmPin2 = 0;
+    PwmPin1 = 0;
+}
+void OFF_m2()
+{
+    PwmPin2 = 0;
 }
 
-void Going_Home()
+void Going_Home_Motor1()
 {
-    //Here we will reset the position of the arm back to the home state
-    
-    if (counts1 == 0)                                                           // this 0 is a filler value and can later be determined to the angle of the 
-    {                                                                           // 0-state of the arm
-        PwmPin1=0.0f;
+    if (counts1 == 0)
+    {
+        Active_State = Homing_M2;
     }
     else if (counts1 > 0)
-    {
-        DirectionPin1 = true;                                                   //uitzoeken of dit klopt, is afhankelijk welke richting opgedraaid moet worden..
-        PwmPin1 = 0.6f;
-    }
-    else if (counts1 < 0)
-    {
+    { 
+        PwmPin1 = 0.8f;
         DirectionPin1 = false;
-        PwmPin1 = 0.6f;
     }
-    
-    if (counts2 == 0)                                                           //Homing for M1 naar begin staat
+    else
     {
-        PwmPin2=0.0f;
+        PwmPin1 = 0.8f;
+        DirectionPin1 = true;
+    }    
+}
+
+void Going_Home_Motor2()
+{
+    if (counts2 == 0)
+    { 
+        Active_State = Post_Homing;
     }
     else if (counts2 > 0)
     {
-        DirectionPin2 = true;                                                   //uitzoeken of dit klopt, is afhankelijk welke richting opgedraaid moet worden..
-        PwmPin2 = 0.6f;
+        PwmPin2 = 0.8f;
+        DirectionPin2 = true;
     }
-    else if (counts2 < 0)
+    else
     {
+        PwmPin2 = 0.8f;
         DirectionPin2 = false;
-        PwmPin2 = 0.6f;
-    } 
-    
-    if (counts1 == 0 && counts2 == 0)
-    {
-        pc.printf("Homing is completed\r\nAutomatic switch to the Functioning State\r\n");
-        Active_State = Function;
     }
 }
 
@@ -412,12 +542,98 @@
     float v1 = PwmPin1 * maxVelocity;
     float v2 = PwmPin2 * maxVelocity;
     
-        if (Active_State == Function)
+        if (Active_State == Function || Active_State == Homing_M1)
         {
-            pc.printf("q1    = %f [rad] \r\nq2    = %f [rad] \r\nq1dot = %f [rad/s] \r\nq2dot = %f [rad/s] \r\n\r\n\r\n\r\n\r\n", rad_m1, rad_m2, v1, v2);
+            pc.printf("q1    = %f [rad] \r\nq2    = %f [rad] \r\ncount1= %i\r\ncount2= %i\r\nq1dot = %f [rad/s] \r\nq2dot = %f [rad/s] \r\n\r\n\r\n\r\n\r\n", rad_m1, rad_m2,counts1, counts2, v1, v2);
         }
-        
-    pc.printf("%f %f",vx, vy);
+}
+
+void Test()
+{/*
+    if (fabs(rad_m1) == 0.01f)
+    {
+        PwmPin1 = 0.0f;
+    }
+    else if (rad_m1 < 0.01f)
+    {
+        DirectionPin1 = true;
+        PwmPin1=0.5f;
+    }
+    else if (rad_m1 > -0.01f)
+    {
+        DirectionPin1 = false;
+        PwmPin1=0.5f;
+    }
+    if (fabs(rad_m2) == 0.01f)
+    {
+        PwmPin2 = 0.0f;
+    }
+    else if (rad_m2 < 0.01f)
+    {
+        DirectionPin2 = false;
+    }*/   
+   
+   
+    
+    if (counts1 == 0)
+    {
+        PwmPin1 = 0.0f;
+    }
+    else if (counts1 > 0)
+    {
+        DirectionPin1 = true;
+        PwmPin1 = 0.4f * ((float)counts1/1000.0f);
+    }
+    else if (counts1 < 0)
+    {
+        DirectionPin1 = false;
+        PwmPin1 = 0.4f * ((float)counts1/1000.0f);
+    }
+   
+    if (counts2 == 0)
+    {
+        PwmPin2 = 0.0f;
+    }   
+    if (counts2 < 0)
+    {
+        DirectionPin2 = false;
+        PwmPin2 = 0.4f * ((float)counts2/1000.0f);
+    }
+    else if (counts2 > 0)
+    { 
+        DirectionPin2 = true;
+        PwmPin2 = 0.4f * ((float)counts2/1000.0f);
+    }
+
+}
+
+void EMG_test()
+{
+    led_G=led_R=led_B=1;
+    /*
+    Threshold_Bi_R  = 0.75f * Flex_Bi_R;
+Threshold_Bi_L  = 0.75f * Flex_Bi_L;
+Threshold_Tri_R = 0.75f * Flex_Tri_R;
+Threshold_Tri_L = 0.75f * Flex_Tri_L;
+    */    
+    if (Filtered_Bi_R >= Threshold_Bi_R)
+    {
+        led_R = 0;
+    }
+    if (Filtered_Bi_L >= Threshold_Bi_L)
+    {
+        led_B = 0;
+    }
+    if (Filtered_Tri_R >= Threshold_Tri_R)
+    {
+        led_G = 0;
+    }
+    if (Filtered_Tri_L >= Threshold_Tri_L)
+    {
+        led_B = 0;
+        led_R = 0;
+    }
+            
 }
 
 void StateMachine() 
@@ -425,9 +641,7 @@
     switch (Active_State)
     {
         case Starting:   
-        OFF();            
         Start_Up();
-        BlinkLed();
         
         if (!Knop4 == true)
         {
@@ -436,9 +650,14 @@
         }
         else if (!Knop3 == true)
         {
-            Active_State = Homing;
+            Active_State = Homing_M1;
             pc.printf("Entering Homing State \r\n");
         }
+                        else if (fabs(rad_m1)>(3.0f *pi) || fabs(rad_m2)>(3.0f *pi))          // pi/4 is a safe value, can/will be editted
+                {
+                    pc.printf("SAFE MODUS ACTIVE!\r\n RESET MANDATORY!\r\n");
+                    Active_State = Safe;
+                }
                                    
         break;
         
@@ -446,8 +665,11 @@
                 //calibration actions
                 //pc.printf("Calibration State");
                 
+                Filter();
                 Calibrating();
-                OFF();
+                OFF_m1();
+                OFF_m2();
+                BlinkLed();
                 
                 if (!Knop1 && !Knop2)
                 {
@@ -458,20 +680,21 @@
                 else if (Knop1==false)
                 {
                     pc.printf("Manual switch to Homing state \r\n");
-                    Active_State = Homing;
+                    Active_State = Homing_M1;
                 }
 
                     
-                
+                Inverse();
                 sample();
                 EMG_Read();
                 Encoding();    
         break;
         
-        case Homing:
+        case Homing_M1:
                 //Homing actions
                 //pc.printf("Homing State");
-                Going_Home();
+                Going_Home_Motor1();
+                OFF_m2();
                 
                 if (!Knop1 && !Knop2)
                 {
@@ -484,18 +707,61 @@
                     pc.printf("Manual switch to Funtioning State \r\n");
                     Active_State = Function;
                 }
-                else if (Knop3==false)
+                else if (Knop4==false)
                 {
                     Active_State = Calibration;
                     pc.printf("Re-entering Calibration State \r\n");
                 }
+                else if (fabs(rad_m1)>(3.0f *pi) || fabs(rad_m2)>(3.0f *pi))          // pi/4 is a safe value, can/will be editted
+                {
+                    pc.printf("SAFE MODUS ACTIVE!\r\n RESET MANDATORY!\r\n");
+                    Active_State = Safe;
+                }
+                else if (counts1 == 0)
+                {
+                    Active_State = Homing_M2;
+                }
                 
-                
+                Inverse();
                 sample();
                 EMG_Read();
                 Encoding();
         break;
         
+        case Homing_M2:
+        
+            Going_Home_Motor2();
+            OFF_m1();
+            
+            if (counts2 == 0 && counts1 == 0)
+            {
+                Active_State = Post_Homing;
+            }
+            else if (counts2 == 0 && counts1 == !0);
+            {
+                // dit zou niet moeten kunnen maar er gebeuren wel meer rare dingen
+                Active_State = Homing_M1;
+            }
+            
+            Inverse();
+            sample();
+            EMG_Read();
+            Encoding();
+            
+        break;    
+        
+        case Post_Homing:
+        
+            static int mm = 0;
+            mm++;
+            if (mm == 1000);
+            {
+                Active_State = Function;
+                pc.printf("Homing was succesfull\r\n\r\nAutomatic switch to Funtioning state\r\n\r\n");
+                mm=0;                                                           // reseting the state
+            }
+        break;
+        
         case Function:
             //pc.printf("Funtioning State");
                 
@@ -508,7 +774,7 @@
                 else if (Knop3==false)
                 {
                     pc.printf("Re-entering Homing State \r\n");
-                    Active_State = Homing;
+                    Active_State = Homing_M1;
                 }
                 else if (!Knop1 && !Knop2)
                 {
@@ -516,15 +782,29 @@
                     Active_State = Starting;
                     iii = 40000;
                 }
-               
+                else if (fabs(rad_m1)>(3.0f *pi) || fabs(rad_m2)>(3.0f *pi))          // pi/4 is a safe value, can/will be editted
+                {
+                    pc.printf("SAFE MODUS ACTIVE!\r\n RESET MANDATORY!\r\n");
+                    Active_State = Safe;
+                }
+                
+                EMG_test();                
+                Filter();
+                Inverse();
                 sample();
                 EMG_Read();
                 Encoding();
-                //velocity1();
-                //velocity2();
+                velocity1();
+                velocity2();
                 motor1();
-                motor2();                
+                motor2();
+                PID_controller();                
         break;    
+        
+        case Safe:
+        OFF_m1();
+        OFF_m2();
+        break;
             
         default:
         pc.printf("UNKNOWN COMMAND");
@@ -536,9 +816,18 @@
     pc.baud(115200);    
     PwmPin1.period_us(30); //60 microseconds pwm period, 16.7 kHz 
     
+    bqc1.add( &BqNotch1_1 ).add( &BqNotch2_1 ).add( &BqHP1 );                   //Oh wat lelijk...
+    bqc2.add(&BqLP1);
+    bqc3.add( &BqNotch1_2 ).add( &BqNotch2_2 ).add( &BqHP2 );
+    bqc4.add(&BqLP2);
+    bqc5.add( &BqNotch1_3 ).add( &BqNotch2_3 ).add( &BqHP3 );
+    bqc6.add(&BqLP3);
+    bqc7.add( &BqNotch1_4 ).add( &BqNotch2_4 ).add( &BqHP4 );
+    bqc8.add(&BqLP4);
+    
     StateTicker.attach(&StateMachine, 0.002);
    
-    printTicker.attach(&Printing, 4.0);
+    printTicker.attach(&Printing, 2);
     
     while(true)
     {