Revision 4:a682fb1f37d2, committed 2018-11-01

Comitter:

ThomBMT

Date:

Thu Nov 01 23:50:06 2018 +0000

Parent:

3:766e9f13d84e

Commit message:

Laatste versie;

Changed in this revision

diff -r 766e9f13d84e -r a682fb1f37d2 main.cpp
--- a/main.cpp	Thu Nov 01 10:52:03 2018 +0000
+++ b/main.cpp	Thu Nov 01 23:50:06 2018 +0000
@@ -88,12 +88,18 @@
 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 Kp = 17.5;
-volatile const float Ki = 1.02;
-volatile const float Kd = 23.2;
-volatile const float Ts = 0.002;                                                      // Sample time in seconds
+volatile const float Kp = 3.2;
+volatile const float Ki = 0.000;
+volatile const float Kd = 0.0;
+volatile const float Ts = 0.002f;                                                      // Sample time in seconds
 volatile float       error_1;
 volatile float       error_2;
+volatile float       u_k_1;
+volatile float       u_k_2;
+volatile float       u_i_1;
+volatile float       u_i_2;
+volatile float       u_d_1;
+volatile float       u_d_2;
 volatile float       U_1;
 volatile float       U_2;
 
@@ -109,8 +115,10 @@
 volatile int iii = 0;                                                           // Start up timer
 
 // Variables for computations of joint angle and velocities:
-volatile float  q_1;
-volatile float  q_2;
+volatile float  q_ref1_o            = 7.0f*pi/4.0f;
+volatile float  q_ref1_n;
+volatile float  q_ref2_o            = 1.0f*pi/3.0f;
+volatile float  q_ref2_n;
 volatile float  r_1;
 volatile float  r_2;
 volatile float  w_1;
@@ -152,8 +160,8 @@
     counts1 = Encoder1.getPulses();
     counts2 = Encoder2.getPulses();
     
-    rad_m1 = rad_count * (float)counts1;
-    rad_m2 = rad_count * (float)counts2;
+    rad_m1 = (rad_count * (float)counts1) + (1.0f*pi/3.0f);
+    rad_m2 = (rad_count * (float)counts2) + (7.0f*pi/4.0f);
 }
 
 void Filter() 
@@ -271,35 +279,6 @@
     scope.send();
 }
 
-void Inverse()
-{
-    // Here we calculate the movement of each joint (Motor 1 and Motor 2) given 
-    // a certain linear velocity-input.
-    
-    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;
-    
-        float u = -r_2*sin(q_1)*cos(q_2)-(r_2)*cos(q_1)*sin(q_2);
-        float z = 2.0f*(r_2*cos(q_1)*cos(q_2))-r_3;
-        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);
-        
-        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
-        
-        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;
-}
-  
 void Calibrating()
 {
     // Calibration is done to ensure that threshold values not predetermined and
@@ -399,7 +378,7 @@
             }
             
 // Setting the Threshold:
-Threshold_Value = 0.8f;
+Threshold_Value = 0.85f;
             
 Threshold_Bi_R  = Threshold_Value * Flex_Bi_R;
 Threshold_Bi_L  = Threshold_Value * Flex_Bi_L;
@@ -413,7 +392,7 @@
             else if (ii == 20000)
             {
                 pc.printf("\r\nAutomatic switch to Homing State for Motor 1\r\n");
-                Active_State = Homing_M1;
+                Active_State = Function;
                 i = 0;
             }
 }
@@ -470,16 +449,16 @@
     if (counts1 == 0)
     {
         pc.printf("Switching to Homing State for Motor 2\r\n");
-        Active_State = Homing_M2;                                               // Statement here instead of statemachine because of checking speed
+        Active_State = Function;                                               // Statement here instead of statemachine because of checking speed
     }
     else if (counts1 > 0)
     { 
-        PwmPin1 = 0.8f;
+        PwmPin1 = 0.5f;
         DirectionPin1 = false;
     }
     else
     {
-        PwmPin1 = 0.8f;
+        PwmPin1 = 0.5f;
         DirectionPin1 = true;
     }    
 }
@@ -494,12 +473,12 @@
     }
     else if (counts2 > 0)
     {
-        PwmPin2 = 0.8f;
+        PwmPin2 = 0.5f;
         DirectionPin2 = true;
     }
     else
     {
-        PwmPin2 = 0.8f;
+        PwmPin2 = 0.5f;
         DirectionPin2 = false;
     }
 }
@@ -532,111 +511,205 @@
     
     if (Checking_Bi_R && Checking_Bi_L)                                         // sloping y =  x,  y > 0
     {
-        vx = 0.5f;
-        vy = 0.5f;
+        vx = 0.01f;
+        vy = 0.01f;
     }
     else if (Checking_Bi_R && Checking_Tri_L)                                   // sloping y = -x,  y > 0
     {
-        vx = -0.5f;
-        vy =  0.5f;
+        vx = -0.01f;
+        vy =  0.01f;
     }
     else if (Checking_Bi_L && Checking_Tri_R)                                   // sloping y = -x,  y < 0
     {
-        vx =  0.5f;
-        vy = -0.5f;
+        vx =  0.01f;
+        vy = -0.01f;
     }
     else if (Checking_Tri_R && Checking_Tri_L)                                  // sloping y =  x,  y < 0
     {
-        vx = -0.5f;
-        vy = -0.5f;
+        vx = -0.01f;
+        vy = -0.01f;
     }
-    else if (Checking_Bi_R)                                                     // y > 0
+    if (Checking_Bi_R)                                                          // y > 0
     {
         vx =  0.0f;
-        vy =  0.5f;
+        vy =  0.01f;
     }
     else if (Checking_Bi_L)                                                     // x > 0
     {
-        vx =  0.5f;
+        vx =  0.01f;
         vy =  0.0f;
     }
-    else if (Checking_Tri_R)                                                    // y < 0
+    else if (Checking_Tri_R)                                                    // x < 0
     {
         vx =  0.0f;
-        vy = -0.5f;
+        vy = -0.01f;
     }
     else if (Checking_Tri_L)                                                    // y < 0
     {
-        vx = -0.5f;
+        vx = -0.01f;
         vy =  0.0f;
     }        
 }
 
-void PID_controller()
+
+void Inverse()
+{
+    // Here we calculate the movement of each joint (Motor 1 and Motor 2) given 
+    // a certain linear velocity-input.
+    q_ref1_n = q_ref1_o+w_1*Ts;
+    q_ref2_n = q_ref2_o+w_2*Ts;
+    r_1= -0.2f;
+    r_2= -0.2f;
+    
+        float u = -r_2*sin(q_ref1_n)*cos(q_ref2_n)-(r_2)*cos(q_ref1_n)*sin(q_ref2_n);
+        float z = 2.0f*(r_2*cos(q_ref1_n)*cos(q_ref2_n))-r_3;
+        float y = r_2*cos(q_ref1_n)*cos(q_ref2_n)-r_2*sin(q_ref1_n)*sin(q_ref2_n)+2.0f*(r_1*cos(q_ref1_n))-r_3;
+        float x = (-2.0f)*r_2*sin(q_ref1_n)*cos(q_ref2_n);
+        float D = 1.0f/(u*z-x*y);                                               // Determinant
+        //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 referenceVelocity1;
+            if (pot1.read()>0.8f)
+                {
+                // Clockwise rotation
+                referenceVelocity1 = 0.005f; 
+                }
+            
+
+            
+            else if (pot1.read() < 0.2f)
+                {
+                // Counterclockwise rotation      
+                referenceVelocity1 = -0.005f ;
+                }
+                
+            else 
+                {
+                referenceVelocity1 = pot1.read() * 0.0f; 
+                } 
+        float referenceVelocity2;
+
+        // This is where Motor 2 is opperated.
+        
+             if (pot2.read()>0.8f)
+                {
+                // Clockwise rotation
+                referenceVelocity2 = 0.005f; 
+                }
+            
+            else if (pot2.read() < 0.2f)
+                {
+                // Counterclockwise rotation      
+                referenceVelocity2 = -0.005f ;
+                }
+                
+            else
+                {
+                referenceVelocity2 = pot2.read() * 0.0f; 
+                } 
+        
+       //vx = 0.0;//referenceVelocity1;                                                // uit emg data
+       //vy = 0.01;referenceVelocity2;                                                // uit emg data
+        
+        w_1 = vx*a+vy*b;
+        w_2 = vx*c+vy*d;
+        
+
+        q_ref1_o = q_ref1_n;
+        q_ref2_o = q_ref2_n;
+}
+  
+
+ void PID_controller()
 {
       // The PID-controller reduces the error between the reference signal 
       // and the actual value.
       
-      error_1 = (w_1*0.002f) - rad_m1;
-      error_2 = (w_2*0.002f) - rad_m2;
+      error_1 = q_ref1_o - rad_m2 + (w_1*Ts);
+      error_2 = q_ref2_o - rad_m1 + (w_2*Ts);
       
       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;
+      u_k_1 = Kp * error_1;
+      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;
+      u_i_1 = Ki * error_1_integral;
+      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;
+      //float filtered_error_1_derivative = LowPassFilter.step(error_1_derivative);
+      //float filtered_error_2_derivative = LowPassFilter.step(error_2_derivative);
+      
+      u_d_1 = Kd * error_1_derivative;
+      u_d_2 = Kd * 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;
+      U_1 = u_k_1; //+ u_i_1 + u_d_1;   
+      U_2 = u_k_2;//+ u_i_2 + u_d_2;
+      
 }
 
 void motor1()
-{         
-        // This is where Motor 1 is opperated.
+{       
+        // This is where Motor 1 is operated.
      
-        float u = U_1;
-        DirectionPin1 = u < 0.0f;
+        float u = 0.4f + 0.6f* fabs(U_1);
         PwmPin1 = fabs(u);
+        if (U_1 >= 0)
+        {
+            DirectionPin1 = false;
+        }
+        else if (U_1 < 0)
+        {
+            DirectionPin1 = true;
+        }
+       
 }
 
 void motor2()
-{  
-        // This is where Motor 2 is opperated.
-        
-        float u = U_2;
-        DirectionPin2 = u > 0.0f;
+{          
+        float u = 0.4f+ 0.6f* fabs(U_2);
         PwmPin2 = fabs(u);
+        if (U_2 <= 0)
+        {
+            DirectionPin2 = false;
+        }
+        else if (U_2 > 0)
+        {
+            DirectionPin2 = true;
+        }
+        //DirectionPin2 = u > 0.0f;
+
 }
 
 void Printing()
 {
     // This function is merely for printing feedback from the system to our
     // screen when we wish to see or check something.
-    
-    float v1 = PwmPin1 * maxVelocity;
-    float v2 = PwmPin2 * maxVelocity;
-    
+
         if (Active_State == Function )//|| Active_State == Homing_M1)
         {
-            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("U_K1 is %f \r\nU_K2 is %f \r\n\r\n   rad_m1 is %f \r\n   rad_m2 is %f \r\n\r\n     Bi_R is %f \r\n     Bi_L is %f \r\n\r\n   counts1 is %i \r\n   counts2 is %i\r\n\r\n", u_k_1, u_k_2, rad_m1, rad_m2, Filtered_Bi_R, Filtered_Bi_L, counts1, counts2);
+            //u_k_1 + u_i_1 + u_d_1;   
+            //u_k_2 + u_i_2 + u_d_2;
+            //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", rad_m1, rad_m2,counts1, counts2, w_1, w_2);
+           // pc.printf("U_1    = %f [error] \r\nU_2    = %f [error]\n\r\n",U_1,U_2);  
+            //pc.printf("u_k_1 = %f\r\nu_k_2 = %f\r\nu_i_1 = %f \r\nu_i_2 = %f \r\nu_d_1 = %f \r\nu_d_2 = %f\r\n\r\n",u_k_1,u_k_2,u_i_1,u_i_2,u_d_1,u_d_2); 
         }
 }
 
@@ -677,6 +750,7 @@
     // integration and the opperation of the system a lot simpeler. 
     
     // We have 5 main states: 
+    //
     //      - Starting
     //          - Start_Up
     //          - Sleep_Mode
@@ -730,7 +804,7 @@
                     else if (Knop1==false)
                     {
                         pc.printf("Manual switch to Homing state \r\n");
-                        Active_State = Homing_M1;
+                        Active_State = Function;
                     }
 
                     
@@ -744,7 +818,7 @@
                 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
+                    if (fabs(rad_m1)>(2.0f*pi/6.0f) || fabs(rad_m2)>(pi/6.0f))          // pi/4 is a safe value, can/will be editted
                     {
                         pc.printf("SAFE MODUS ACTIVE!\r\n RESET MANDATORY!\r\n");
                         Active_State = Safe;
@@ -815,7 +889,7 @@
                 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
+                if (fabs(rad_m1)>(2.0f*pi/3.0f) || fabs(rad_m2)>(25.0f*pi/12.0f))          // pi/4 is a safe value, can/will be editted
                 {
                     pc.printf("SAFE MODUS ACTIVE!\r\n RESET MANDATORY!\r\n");
                     Active_State = Safe;
@@ -873,7 +947,7 @@
     
     StateTicker.attach(&StateMachine, 0.002);
    
-    printTicker.attach(&Printing, 2);
+    printTicker.attach(&Printing, 0.5);
      
     while(true)
     {