Diff: main.cpp

Revision:

2:bc6043623fb7

Parent:

1:ba14d8f4d444

Child:

3:766e9f13d84e

--- a/main.cpp	Thu Nov 01 10:08:23 2018 +0000
+++ b/main.cpp	Thu Nov 01 10:17:18 2018 +0000
@@ -273,102 +273,8 @@
         
         w_1 = vx*a+vy*b;
         w_2 = vx*c+vy*d;
-        
-        /*
-        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
-                referenceVelocity1 = (pot1.read()-0.5f) * 2.0f; 
-                }
-            
-            else if (pot1.read() == 0.5f)
-            {
-                referenceVelocity1 = pot1.read() * 0.0f; 
-            } 
-            
-            else if (pot1.read() < 0.5f)
-                {
-                // Counterclockwise rotation      
-                referenceVelocity1 = 2.0f * (pot1.read()-0.5f) ;
-                }
-}
-    
-void velocity2()
-{
-            if (pot2.read()>0.5f)
-                {
-                // Clockwise rotation
-                referenceVelocity2 = (pot2.read()-0.5f) * 2.0f; 
-                }
-            
-            else if (pot2.read() == 0.5f)
-            {
-                referenceVelocity2 = pot2.read() * 0.0f; 
-            } 
-            
-            else if (pot2.read() < 0.5f)
-                {
-                // Counterclockwise rotation      
-                referenceVelocity2 = 2.0f * (pot2.read()-0.5f) ;
-                }
-}    
-    
-void motor1()
-{          
-        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 = referenceVelocity2              ;                           //w_2
-        float u = u_v2;// (2.0f * pi);
-        DirectionPin2 = u > 0.0f;
-        PwmPin2 = fabs(u);
-}
-   
 void Calibrating()
 {
     static float n = 0.0;
@@ -376,7 +282,6 @@
     static float l = 0.0;
     static float k = 0.0;
     
-    //static int ii; 
     ii++;
     
             if (ii<=2500)
@@ -626,6 +531,53 @@
     }        
 }
 
+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 motor1()
+{          
+        float u = U_1;
+        DirectionPin1 = u < 0.0f;
+        PwmPin1 = fabs(u);
+}
+
+void motor2()
+{  
+        float u = U_2;
+        DirectionPin2 = u > 0.0f;
+        PwmPin2 = fabs(u);
+}
+
 void Printing()
 {
     float v1 = PwmPin1 * maxVelocity;
@@ -639,66 +591,7 @@
         pc.printf("%i \r\n%i \r\n\r\n",counts1,counts2);
 }
 
-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()
+void EMG_test()                                                                 // even nalopen of dit ook kan met i++!
 {
     led_G=led_R=led_B=1;
      
@@ -770,16 +663,15 @@
                     }
 
                     
-                Inverse();
                 sample();
-                EMG_Read();
-                Encoding();    
+                EMG_Read();  
         break;
         
         case Homing_M1:
 
                 Going_Home_Motor1();
                 OFF_m2();
+                Encoding();
                 
                     if (fabs(rad_m1)>(pi/6.0f) || fabs(rad_m2)>(pi/6.0f))          // pi/4 is a safe value, can/will be editted
                     {
@@ -802,18 +694,14 @@
                         Active_State = Calibration;
                         pc.printf("Re-entering Calibration State \r\n");
                     }
-                
-                
-                Inverse();
-                sample();
-                EMG_Read();
-                Encoding();
+                    
         break;
         
         case Homing_M2:
         
                     Going_Home_Motor2();
                     OFF_m1();
+                    Encoding();
                 
                     if (fabs(rad_m1)>(pi/6.0f) || fabs(rad_m2)>(pi/6.0f))          // pi/4 is a safe value, can/will be editted
                     {
@@ -824,12 +712,6 @@
                     {
                         Active_State = Post_Homing;
                     }
-                    
-                
-                Inverse();
-                sample();
-                EMG_Read();
-                Encoding();
             
         break;    
         
@@ -849,6 +731,18 @@
         break;
         
         case Function:
+                
+                EMG_test();                
+                Filter();
+                Inverse();
+                sample();
+                EMG_Read();
+                Encoding();
+                Checking_EMG();
+                Setting_Movement();
+                PID_controller();
+                motor1();
+                motor2();
                         
                 if (fabs(rad_m1)>(pi/6.0f) || fabs(rad_m2)>(pi/6.0f))          // pi/4 is a safe value, can/will be editted
                 {
@@ -872,23 +766,7 @@
                     Active_State = Starting;
                     iii = 40000;
                 }
-                
-                
-                EMG_test();                
-                Filter();
-                Inverse();
-                sample();
-                EMG_Read();
-                Encoding();
-                velocity1();
-                velocity2();
-                Checking_EMG();
-                Setting_Movement();
-                PID_controller();
-                motor1();
-                motor2();
-                
-                                
+                                       
         break;    
         
         case Safe: