Diff: main.cpp

Revision:

32:054900bfb0a5

Parent:

31:98a1155f5edb

Child:

33:5477deb3803e

--- a/main.cpp	Tue Oct 20 13:16:40 2015 +0000
+++ b/main.cpp	Tue Oct 20 13:31:29 2015 +0000
@@ -65,10 +65,10 @@
 //PID variables
 double u1; double u2;                                               //Output of PID controller (PWM value for motor 1 and 2)
 double m1_error=0; double m1_e_int=0; double m1_e_prev=0;           //Error, integrated error, previous error
-const double m1_kp=0.01; const double m1_ki=0.00125; const double m1_kd=0.5;   //Proportional, integral and derivative gains.
+const double m1_kp=0.01; const double m1_ki=0.00125; const double m1_kd=0.00;   //Proportional, integral and derivative gains.
 
 double m2_error=0; double m2_e_int=0; double m2_e_prev=0;           //Error, integrated error, previous error
-const double m2_kp=0.01; const double m2_ki=0.00125; const double m2_kd=0.5;   //Proportional, integral and derivative gains.
+const double m2_kp=0.01; const double m2_ki=0.00125; const double m2_kd=0.00;   //Proportional, integral and derivative gains.
 
 
 //lowpass filter 7 Hz  - envelope
@@ -198,7 +198,7 @@
         goal2 = (potmeter2.read()-0.5)*4200;
         
         double error2=(goal2 - Encoder2.getPulses());
-        pwm_to_motor2 = pid2(error2,m2_kp,m2_ki,m2_kd,m2_e_int,m2_e_prev);
+        pwm_to_motor2 = pid(error2,m2_kp,m2_ki,m2_kd,m2_e_int,m2_e_prev);
     
         if(pwm_to_motor2 > 0)
             dir_motor2 = 0;         //=counterclockwise
@@ -286,7 +286,7 @@
     // Integral
     e_int = e_int + CONTROL_RATE * error;
     // PID
-    return kp*error + ki*e_int; //+ kd * e_der;
+    return kp*error + ki*e_int + kd * e_der;
     
 }