dear all
Dependencies: DigitDisplay Motor PID Ping mbed millis
Fork of DagonFly__RoadToJapan_15Mei_A by
Diff: main.cpp
- Revision:
- 52:876ff6bdff3c
- Parent:
- 51:292c5807a93f
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