Diff: main.cpp

Revision:

7:5d9907aa6dbc

Parent:

6:f40e26afc9aa

diff -r f40e26afc9aa -r 5d9907aa6dbc main.cpp
--- a/main.cpp	Thu Nov 02 18:56:04 2017 +0000
+++ b/main.cpp	Thu Nov 02 19:50:07 2017 +0000
@@ -21,10 +21,10 @@
 double refrad1;
 double refrad2;
 double ref2 = 0;
-double Kp1 = 0.002;    //kp motor 2 = 0.0001
+double Kp1 = 0.002;   // 1 moet nog getweakt worden
 double Ki1 = 0;
-double Kp2 = 0.0005;
-double Ki2 = 0.0005;
+double Kp2 = 0.00021;
+double Ki2 = 0.00005;
 int q1_puls;
 int q2_puls;
 double rad2pulses=(2100/pi);
@@ -68,8 +68,8 @@
     q2_puls = q2_enc.getPulses();
     q2_pos =  q2_puls*pulses2rad;       // berekent positie q2 in radialen  
     
-    vx= 0.012f*sin(t.read());
-    vy=0;
+    vy= 0;
+    vx=0.012f*sin(t.read());
     
     q1 = ((-(L3 + L2*cos(q1_pos - q2_pos))/(L1*L2*cos(2*q1_pos - q2_pos) + L1*L3*cos(q1_pos)))*vx + ((L2*sin(q1_pos - q2_pos))/(L1*L2*cos(2*q1_pos - q2_pos) + L1*L3*cos(q1_pos)))*vy) * 5 *ts + q1;
     q2 = (((L3 - L1*sin(q1_pos) + L2*cos(q1_pos - q2_pos))/(L1*L2*cos(2*q1_pos - q2_pos) + L1*L3*cos(q1_pos))*vx) +  ((L1*cos(q1_pos) - L2*sin(q1_pos - q2_pos))/(L1*L2*cos(2*q1_pos - q2_pos) + L1*L3*cos(q1_pos)))*vy) * 5 * ts + q2;
@@ -77,6 +77,9 @@
     ref1 = -q1*rad2pulses;
     ref2 = q2*rad2pulses;
     
+    
+    
+    
   
     
 }
@@ -88,6 +91,8 @@
     //PID 1
 error1_1 = ref1 - q1_puls;
 error_I_1 = error_I2_1 + ts*((error1_1 + error2_1)/2);
+error_I2_1 = error_I_1;
+error2_1 = error1_1;
 
 PI1 = Kp1*error1_1 + Ki1*error_I_1;
 
@@ -125,7 +130,7 @@
     motor2MagnitudePin = fabs(PI2);               
     }
     
-    if(n==500){         
+    if(n==100){         
     printf("q1_puls = %d   q2_puls = %d    ref1 = %f   ref2 = %f\n\r", q1_puls, q2_puls, ref1, ref2);
     n=0;
     }