Diff: main.cpp

Revision:

20:71c39665276e

Parent:

19:ed6043782809

Child:

21:60102b9dd590

--- a/main.cpp	Fri Nov 03 11:25:54 2017 +0000
+++ b/main.cpp	Fri Nov 03 11:30:23 2017 +0000
@@ -49,7 +49,7 @@
 double Rsx = 0.38;       //Reference x-component of the setpoint radius
 double Rsy = 0.30;       //Reference y-component of the setpoint radius
 double refP = 0;      //Reference position motor 1
-double refP2 = 0.5*pi; //Reference position motor 2
+double refP2 = 0; //Reference position motor 2
 double Rex = cos(q1)*L1 - sin(q2)*L2;   //The x-component of the end-effector radius 
 double Rey = sin(q1)*L1 + cos(q2)*L2;   //The y-component of the end-effector radius
 double R1x = 0;                         //The x-component of the joint 1 radius
@@ -80,7 +80,7 @@
     
     int maxwaarde = 4096;                   // = 64x64
     refP = (((0.5*pi) - q1)/(2*pi))*maxwaarde;
-    refP2 = (((-pi) + q1 - q2)/(2*pi))*maxwaarde;    //Get reference positions was eerst 0.5*pi
+    refP2 = ((- q1 - q2)/(2*pi))*maxwaarde;    //Get reference positions was eerst 0.5*pi
 }
 
 void SetpointRobot()
@@ -144,15 +144,15 @@
 
 float FeedBackControl2(float error2, float &e_prev2, float &e_int2)   // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
 {
-    float kp2 = 0.02;                             // kind of scaled.
+    float kp2 = 0.002;                             // kind of scaled.
     float Proportional2= kp2*error2;
     
-    float kd2 = 0.00008;                           // kind of scaled. 
+    float kd2 = 0.000008;                           // kind of scaled. 
     float VelocityError2 = (error2 - e_prev2)/Ts; 
     float Derivative2 = kd2*VelocityError2;
     e_prev2 = error2;
     
-    float ki2 = 0.0005;                           // kind of scaled.
+    float ki2 = 0.00005;                           // kind of scaled.
     e_int2 = e_int2+Ts*error2;
     float Integrator2 = ki2*e_int2;