Diff: main.cpp

Revision:

50:8bc9dbca2ffa

Parent:

49:0c9148ab8585

Child:

51:df6391c3fa68

--- a/main.cpp	Sat May 13 15:17:23 2017 +0000
+++ b/main.cpp	Mon May 15 07:08:30 2017 +0000
@@ -67,16 +67,16 @@
 double previous_speed2 = 0;
 
 float rpm, rpm2;
-double target_rpm = 18.0, target_rpm2 = 18.0; // selisih 4 bagus, sama bagus
+double target_rpm = 17.0, target_rpm2 = 17.0; // selisih 4 bagus, sama bagus
 const float maxRPM = 35, minRPM = 0; // Limit 25 atau 27
 
 const float pwmPowerUp        = 1.0;
 const float pwmPowerDown      = -1.0;
 
 double jarak_ping=0;
-double ping_target = 16;
+double ping_target = 15;
 double ping_current_error, ping_previous_error1 = 0, ping_sum_error=0;
-double ping_Kp = -0.3747, ping_Ki =0, ping_Ts=10;
+double ping_Kp = -0.2747, ping_Kd = -0.535, ping_Ts=10;
 double ping_pwm, ping_previous_pwm = 0; 
 
 // Variable Bawah
@@ -106,6 +106,7 @@
 unsigned long int previousMillis3 = 0;  // Pneumatik
 unsigned long int previousMillis4 = 0;  // Ping
 unsigned long int previousMillis5 = 0;  // Display
+unsigned long int previousMillis6 = 0;  // Display
 
 /* Variabel Stick */
 //Logic untuk masuk aktuator
@@ -591,6 +592,8 @@
                 previousMillis3 = millis();
                 flag_Pneu = true;
                 ready = false;
+                //ReloadOn = !ReloadOn;
+                //previousMillis6 = millis();
                 
             }
             break;
@@ -605,8 +608,8 @@
         case (31) : 
         {
             // start
-            target_rpm2 = 23;
-            target_rpm = 23;
+            target_rpm2 = 24;
+            target_rpm = 24;
             init_rpm();
             break;
         }
@@ -621,8 +624,8 @@
         case (33) : 
         {
             // R3
-            target_rpm2 = 18;
-            target_rpm = 18;
+            target_rpm2 = 17;
+            target_rpm = 17;
             init_rpm();
             break;
         }
@@ -673,8 +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_Ki*(ping_sum_error+ping_current_error)*ping_Ts;   
-            ping_sum_error = ping_sum_error+ping_current_error;
+            ping_pwm = (double) ping_Kp*ping_current_error + ping_Kd*(ping_current_error-ping_previous_error1)/ping_Ts;   
+            //ping_sum_error = ping_sum_error+ping_current_error;
             
             pc.printf("%.2f\n", jarak_ping);
             powerScrew.speed(ping_pwm);
@@ -812,9 +815,12 @@
             aktuator();
             launcher();
             reloader();
-            if ((millis()-previousMillis3 >= 320)&&(flag_Pneu)){
+            if ((millis()-previousMillis3 >= 230)&&(flag_Pneu)){
                 pneumatik = 1;
                 flag_Pneu = false;
+                //if (millis()-previousMillis6 >= 100){
+                //    ReloadOn = !ReloadOn;
+                //}
             }
         }
         else