Revision 5:1e7dfd3c55ca, committed 2017-11-01

Comitter:

mhomsma

Date:

Wed Nov 01 14:13:11 2017 +0000

Parent:

4:c2017b3a7adb

Commit message:

In before publishing;

Changed in this revision

diff -r c2017b3a7adb -r 1e7dfd3c55ca main.cpp
--- a/main.cpp	Wed Nov 01 13:29:57 2017 +0000
+++ b/main.cpp	Wed Nov 01 14:13:11 2017 +0000
@@ -44,8 +44,8 @@
 const double M2_Ts = 0.01;
 
 // Controller gains (motor1-Kp,-Ki,...)
-const double M1_Kp = 0.1, M1_Ki = 0.0, M1_Kd = 0.00125,  M1_N = 100; // THESE VALUES ARE ARBITRARY AT THIS POINT Ki = 0.02
-const double M2_Kp = 0.1, M2_Ki = 0.0, M2_Kd = 0.00125,  M2_N = 100; // Inspired by the Ziegler-Nichols Method
+const double M1_Kp = 0.1, M1_Ki = 0.02, M1_Kd = 0.00125,  M1_N = 100; // THESE VALUES ARE ARBITRARY AT THIS POINT Ki = 0.02
+const double M2_Kp = 0.1, M2_Ki = 0.02, M2_Kd = 0.00125,  M2_N = 100; // Inspired by the Ziegler-Nichols Method
 
 // Filter variables (motor1-filter-v1,-v2)
 double M1_f_v1 = 0.0, M1_f_v2 = 0.0;
@@ -56,9 +56,9 @@
 //Kinematic model
 void Kinematic_referencer(double &q1, double &q2, double a1, double a2, bool dir1, bool dir2)
     {
-    double qt = q1 + q2, vx = 0, vy = 0;                       // current total angle
-    double xcurrent = L1 * cos (q1) + L2 * cos (qt);   // current x position
-    double ycurrent = L1 * sin (q1) + L2 * sin (qt);   // current y position
+    double at = a1 + a2, vx = 0, vy = 0;                       // current total angle
+    double xcurrent = L1 * cos (a1) + L2 * cos (at);   // current x position
+    double ycurrent = L1 * sin (a1) + L2 * sin (at);   // current y position
     
     //pc.printf("x = %.1f, y = %.1f \r\n", xcurrent, ycurrent);
     double velocity = 5;
@@ -76,23 +76,23 @@
     //pc.printf("vx = %.2f, vy = %.2f \r\n", vx,vy);
     
     //Jacobians
-    double J11 = -L1 * sin (q1) - L2 * sin (qt);
-    double J12 = -L2 * sin (qt);
-    double J21 = L1 * cos (q1) + L2 * cos (qt);
-    double J22 = L2 * cos (qt);
+    double J11 = -L1 * sin (a1) - L2 * sin (at);
+    double J12 = -L2 * sin (at);
+    double J21 = L1 * cos (a1) + L2 * cos (at);
+    double J22 = L2 * cos (at);
     double Determinant = J11 * J22 - J21 * J12;   // calculate determinant
     
     //pc.printf("D = %.3f \r\n", Determinant);
  
     //Calculate angular velocities
-    double q1der = ((J22 * vx) - (J12 * vy)) / Determinant;
-    double q2der = ((-J21 * vx) + (J11 * vy)) / Determinant;
+    double a1der = ((J22 * vx) - (J12 * vy)) / Determinant;
+    double a2der = ((-J21 * vx) + (J11 * vy)) / Determinant;
     
     //pc.printf("q1d = %.2f, q2d = %.2f \r\n", q1der, q2der);
     
-    //Calculate new angles
-    double q1new = q1 + (q1der * M1_Ts);    //nog fixen met die tijdstappen?
-    double q2new = q2 + (q2der * M1_Ts);    //hier ook
+    //Calculate new reference angles
+    double q1new = q1 + (a1der * M1_Ts);    //nog fixen met die tijdstappen?
+    double q2new = q2 + (a2der * M1_Ts);    //hier ook
     //pc.printf ("q1=%f, q2=%f\n", q1 * c, q2 * c);
     double qtnew = q1new + q2new;
     
@@ -163,7 +163,7 @@
     
     //pc.printf("a1 = %.2f, a2 = %.2f \r\n", a1, a2);
     //pc.printf("q1 = %.2f, q2 = %.2f \r\n", ref_q1, ref_q2);
-    //pc.printf("e1 = %.2f, e2 = %.2f \r\n", ref_q1-a1, ref_q2-a2);
+    pc.printf("e1 = %.2f, e2 = %.2f \r\n", ref_q1-a1, ref_q2-a2);
         
     MotorValue1 = PID( ref_q1 - a1 , M1_Kp, M1_Ki, M1_Kd, M1_Ts, M1_N, M1_f_v1, M1_f_v2); //Find the motorvalue by going through the PID
     MotorValue2 = PID( ref_q2 - a2 , M2_Kp, M2_Ki, M2_Kd, M2_Ts, M2_N, M2_f_v1, M2_f_v2);