Revision 52:876ff6bdff3c, committed 2017-05-17

Comitter:

Najib_irvani

Date:

Wed May 17 08:19:36 2017 +0000

Parent:

51:292c5807a93f

Commit message:

dear all

Changed in this revision

diff -r 292c5807a93f -r 876ff6bdff3c main.cpp
--- a/main.cpp	Mon May 15 12:51:35 2017 +0000
+++ b/main.cpp	Wed May 17 08:19:36 2017 +0000
@@ -57,8 +57,8 @@
 // indek 2 untuk motor depan, 1 untuk motor belakang
 double speed, speed2;
 const double maxSpeed = 0.95, minSpeed = -0.95, Ts = 12.5;
-const double kpA1=0.19982, kdA1=0.91824, kiA1=0.00072609;
-const double kpA2=0.20481, kdA2=0.92191, kiA2=0.00076326;
+const double kpA1=0.1478, kdA1=0.9295, kiA1=0.0004226;
+const double kpA2=0.1293, kdA2=1.0070, kiA2=0.0002986;
 double a1,b1,c1;
 double a2,b2,c2;
 double current_error1, previous_error1_1 = 0, previous_error1_2 = 0;
@@ -74,9 +74,10 @@
 const float pwmPowerDown      = -1.0;
 
 double jarak_ping=0;
-double ping_target = 17;
+//double ping_target = 15; // ping lama
+double ping_target = 14; // ping baru
 double ping_current_error, ping_previous_error1 = 0, ping_sum_error=0;
-double ping_Kp = -0.3747, ping_Kd = -0.535, ping_Ts=10;
+double ping_Kp = -0.3747, ping_Kd = -0.049, ping_Ts=10;
 double ping_pwm, ping_previous_pwm = 0; 
 
 // Variable Bawah
@@ -99,14 +100,14 @@
 float tunning_Blk;
 
 /* Deklarasi Variable Millis */
-unsigned long int previousMillis = 0;   // PID launcher
-unsigned long int currentMillis;
-unsigned long int previousMillis2 = 0;  // PID launcher
-unsigned long int currentMillis2;
-unsigned long int previousMillis3 = 0;  // Pneumatik
-unsigned long int previousMillis4 = 0;  // Ping
-unsigned long int previousMillis5 = 0;  // Display
-unsigned long int previousMillis6 = 0;  // Display
+static volatile uint32_t previousMillis = 0;   // PID launcher
+static volatile uint32_t currentMillis;
+static volatile uint32_t previousMillis2 = 0;  // PID launcher
+static volatile uint32_t currentMillis2;
+static volatile uint32_t previousMillis3 = 0;  // Pneumatik
+static volatile uint32_t previousMillis4 = 0;  // Ping
+static volatile uint32_t previousMillis5 = 0;  // Display
+static volatile uint32_t previousMillis6 = 0;  // pneu
 
 /* Variabel Stick */
 //Logic untuk masuk aktuator
@@ -116,6 +117,7 @@
 bool ReloadOn = false;
 bool flag_Pneu = false;
 bool flag_paku = false;
+
 bool ready = false;
 
 /*****************************************************/
@@ -592,8 +594,7 @@
                 previousMillis3 = millis();
                 flag_Pneu = true;
                 ready = false;
-                //ReloadOn = !ReloadOn;
-                //previousMillis6 = millis();
+                previousMillis6 = millis();
                 
             }
             break;
@@ -673,19 +674,23 @@
         }
         else if(!flag_Pneu){
             //pc.printf("%.2f\n", ping_pwm);
-            ping_current_error =  (double) (ping_target-jarak_ping);
+            if (millis()-previousMillis6>700)
+            {
+                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_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;   
+                
+                if (ping_pwm>1) ping_pwm=1;
+                if (ping_pwm>0.049 && ping_pwm<0.5) ping_pwm = 0.5;
+                if (ping_pwm<-0.049 && ping_pwm>-0.3) ping_pwm = -0.3;
+                if (ping_pwm<-1) ping_pwm=-1;
             
-            if ((ping_pwm<0.7) && (ping_pwm>0.0)) ping_pwm = 0.7;
-            if ((ping_pwm>-0.5) && (ping_pwm<0.0)) ping_pwm = -0.5;
-            if (ping_pwm>1) ping_pwm=1;
-            if (ping_pwm<-1) ping_pwm=-1;
-            pc.printf("%.2f\n", jarak_ping);
-            powerScrew.speed(ping_pwm);
+                powerScrew.speed(ping_pwm);
+                
+                ping_previous_error1 = ping_current_error;
+            }
             
-            ping_previous_error1 = ping_current_error;
         }
         if ((jarak_ping>(ping_target-2))&&(jarak_ping<(ping_target+2))){
             ready = true;
@@ -821,6 +826,7 @@
             if ((millis()-previousMillis3 >= 230)&&(flag_Pneu)){
                 pneumatik = 1;
                 flag_Pneu = false;
+                //wait_ms(1000);
             }
         }
         else