Diff: main.cpp

Revision:

48:3006386dc8df

Parent:

47:6cac4f1a3c8e

Child:

49:0c9148ab8585

--- a/main.cpp	Sat May 13 05:47:10 2017 +0000
+++ b/main.cpp	Sat May 13 07:27:51 2017 +0000
@@ -74,9 +74,9 @@
 const float pwmPowerDown      = -1.0;
 
 double jarak_ping=0;
-double ping_target = 14;
+double ping_target = 16;
 double ping_current_error, ping_previous_error1 = 0, ping_sum_error=0;
-double ping_Kp = -0.2747, ping_Kd =0, ping_Ts=10;
+double ping_Kp = -0.3747, ping_Ki =0, ping_Ts=10;
 double ping_pwm, ping_previous_pwm = 0; 
 
 // Variable Bawah
@@ -676,7 +676,8 @@
             ping_current_error =  (double) (ping_target-jarak_ping);
                
             ping_sum_error += ping_current_error*ping_Ts;
-            ping_pwm = (double) ping_Kp*ping_current_error + ping_Kd*(ping_current_error-ping_previous_error1)/ping_Ts;   
+            ping_pwm = (double) ping_Kp*ping_current_error + ping_Ki*(ping_sum_error+ping_current_error)*ping_Ts;   
+            ping_sum_error = ping_sum_error+ping_current_error;
             
             pc.printf("%.2f\n", jarak_ping);
             powerScrew.speed(ping_pwm);