Diff: main.cpp

Revision:

19:ed6043782809

Parent:

18:05ff73b29654

Child:

20:71c39665276e

--- a/main.cpp	Fri Nov 03 11:18:09 2017 +0000
+++ b/main.cpp	Fri Nov 03 11:25:54 2017 +0000
@@ -128,31 +128,31 @@
     float kp = 0.0015;                               // kind of scaled.
     float Proportional= kp*error;
     
-    /*float kd = 0.00008;                           // kind of scaled. 
+    float kd = 0.0008;                           // kind of scaled. 
     float VelocityError = (error - e_prev)/Ts; 
     float Derivative = kd*VelocityError;
-    */e_prev = error;
+    e_prev = error;
     
     float ki = 0.0001;                           // kind of scaled.
     e_int = e_int+Ts*error;
     float Integrator = ki*e_int;
     
     
-    float motorValue = Proportional + Integrator; //+ Derivative;
+    float motorValue = Proportional + Integrator + Derivative;
     return motorValue;
 }
 
 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.002;                             // kind of scaled.
+    float kp2 = 0.02;                             // kind of scaled.
     float Proportional2= kp2*error2;
     
-    float kd2 = 0.000008;                           // kind of scaled. 
+    float kd2 = 0.00008;                           // kind of scaled. 
     float VelocityError2 = (error2 - e_prev2)/Ts; 
     float Derivative2 = kd2*VelocityError2;
     e_prev2 = error2;
     
-    float ki2 = 0.00005;                           // kind of scaled.
+    float ki2 = 0.0005;                           // kind of scaled.
     e_int2 = e_int2+Ts*error2;
     float Integrator2 = ki2*e_int2;