Fix pisan inimah plis jangan revisi ultimate mantep

Dependencies:   DigitDisplay Motor PID mbed millis

Fork of DagonFly__RoadToJapan_15Mei by KRAI 2017

Revision:
46:85169ae8659b
Parent:
45:964ae71a30e3
Child:
47:6cac4f1a3c8e
--- a/main.cpp	Wed Apr 05 16:04:08 2017 +0000
+++ b/main.cpp	Mon Apr 24 14:12:08 2017 +0000
@@ -2,7 +2,7 @@
 /****************************************************************************/
 /*                  PROGRAM UNTUK PID CLOSED LOOP                           */
 /*                                                                          */
-/*                  Last Update : 11 Maret 2017                          */
+/*                  Last Update : 16 April 2017                          */
 /*                                                                          */
 /*  - Digunakan encoder autonics                                            */
 /*  - Konfigurasi Motor dan Encoder sbb :                                   */
@@ -53,38 +53,48 @@
 #define D_ENCODER 10        // Diameter Roda Encoder
 #define D_ROBOT 80          // Diameter Roda Robot
 
-// Variable Atas
+// Variable Atas 
+// indek 2 untuk motor depan, 1 untuk motor belakang
 double speed, speed2;
-const double maxSpeed = 0.95, minSpeed = 0.0;
-const double kpA=0.6757, kdA=0.7757, kiA=0.00003757;
-double p,i,d;
-double p2,i2,d2;
-double last_error = 0, current_error, sum_error = 0;
-double last_error2 = 0, current_error2, sum_error2 = 0;
+const double maxSpeed = 0.95, minSpeed = 0.0, Ts = 12.5;
+const double kpA1=0.1478, kdA1=0.9295, kiA1=0.0004226;
+const double kpA2=0.1293, kdA2=1.007, kiA2=0.0002986;
+double a1,b1,c1;
+double a2,b2,c2;
+double current_error1, previous_error1_1 = 0, previous_error1_2 = 0;
+double current_error2, previous_error2_1 = 0, previous_error2_2 = 0;
+double previous_speed1 = 0;
+double previous_speed2 = 0;
+
 float rpm, rpm2;
-float target_rpm = 17.0, target_rpm2 = 17.0; // selisih 4 bagus, sama bagus
-const float maxRPM = 28, minRPM = 0; // Limit 25 atau 27
+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;
 
-float jarak_ping=0;
- 
+double jarak_ping=0;
+double ping_target = 12;
+double ping_current_error, ping_previous_error1 = 0, ping_sum_error=0;
+double ping_Kp = -0.13, ping_Kd =-0.049, ping_Ts=10;
+double ping_pwm, ping_previous_pwm = 0; 
+
 // Variable Bawah
 float Vt;
 float keliling_enc      = PI*D_ENCODER;
 float keliling_robot    = PI*D_ROBOT;
 float speedT            = 0.2;
 float PIVOT             = 0.17;      // PWM Pivot Kanan, Pivot Kiri
-float tuneDpn           = 0.80;     // Tunning PWM motor Depan
-float tuneBlk           = 0.80;     // Tunning PWM motor belakang
+float tuneDpn           = 1.0;     // Tunning PWM motor Depan
+float tuneBlk           = 1.0;     // Tunning PWM motor belakang
 float tuneAksel         = 0.6;
 int aksel               = 0;
 float tuneAkselBlk      = 0.4;
-float tuneR             = 0.78;
-float tuneL             = 0.72;
+float tuneR             = 1.0;
+float tuneL             = 1.0;
 float serong            = 0.4;
-float rasio             = 1.4545;
+float rasio             = 3.0;
+float t_new             = 0.25;
 
 /* variable tunning */
 float tunning_L;
@@ -141,7 +151,7 @@
 //Motor motorBlk(PB_6, PC_14, PC_13);  //(PB_6, PB_1, PB_12); (PC_7, PC_14, PC_13); 
 Motor motorBlk(PB_2, PB_15, PB_1);
 Motor motorL  (PB_9, PA_12, PA_6);  
-Motor motorR  (PB_8, PC_5, PC_6);   //(PC_6, PB_4, PB_5);
+Motor motorR  (PB_8, PC_6, PC_5);   //(PC_6, PB_4, PB_5);
 
 /* Deklarasi Motor Launcher */
 Motor launcherDpn(PA_5,PB_12,PA_11);    // pwm ,fwd, rev
@@ -494,36 +504,36 @@
         {
             // Serong kanan maju
             motorDpn.speed(-serong);
-            motorL.speed(-serong);
+            motorL.speed(-serong-t_new);
             motorBlk.speed(serong);
-            motorR.speed(serong);
+            motorR.speed(serong+t_new);
             break;
         }
         case (14) : 
         {
             // Serong kiri maju
             motorDpn.speed(serong);
-            motorR.speed(serong);
+            motorR.speed(serong+t_new);
             motorBlk.speed(-serong);
-            motorL.speed(-serong);
+            motorL.speed(-serong-t_new);
             break;
         }
         case (15) : 
         {
             // Serong kanan mundur 
             motorDpn.speed(-serong);
-            motorR.speed(-serong);
+            motorR.speed(-serong-t_new);
             motorBlk.speed(serong);
-            motorL.speed(serong);
+            motorL.speed(serong+t_new);
             break;
         }
         case (16) : 
         {
             // Serong kiri mundur
             motorDpn.speed(serong);
-            motorL.speed(serong);
+            motorL.speed(serong+t_new);
             motorBlk.speed(-serong);
-            motorR.speed(-serong);
+            motorR.speed(-serong-t_new);
             break;
         }
         case (3) : 
@@ -559,8 +569,8 @@
             if (aksel>300)
                 tuneAksel = 0.9;
             
-            motorR.speed(tuneAksel);
-            motorL.speed(-tuneAksel);
+            motorR.speed(tuneAksel+t_new);
+            motorL.speed(-tuneAksel-t_new);
             motorDpn.brake(1);
             motorBlk.brake(1);
             break;
@@ -572,8 +582,8 @@
             if (aksel>300)
                 tuneAksel = 0.9;
             
-            motorR.speed(-tuneAksel);
-            motorL.speed(tuneAksel);
+            motorR.speed(-tuneAksel-t_new);
+            motorL.speed(tuneAksel+t_new);
             motorDpn.brake(1);
             motorBlk.brake(1);
             break;
@@ -631,8 +641,8 @@
         case (32) : 
         {
             // select
-            target_rpm2 = 10;
-            target_rpm = 10;
+            target_rpm2 = 11;
+            target_rpm = 11;
             init_rpm();
             break;
         }
@@ -676,18 +686,27 @@
         else if(!limitTengah){
             isReload = true;
         }
-        else if((jarak_ping > 6.5) && !flag_Pneu){
-            powerScrew.speed(pwmPowerUp);
-            ready = false;
+        else if(!flag_Pneu){
+            //pc.printf("%.2f\n", ping_pwm);
+            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;   
+            
+            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.5) ping_pwm = -0.5;
+            if (ping_pwm<-1) ping_pwm=-1;
+            
+            powerScrew.speed(ping_pwm);
+            
+            ping_previous_error1 = ping_current_error;
         }
-        else if((jarak_ping < 6.0) && !flag_Pneu) {
-            powerScrew.speed(-0.85);
+        if ((jarak_ping>(ping_target-3))&&(jarak_ping<(ping_target+3))){
+            ready = true;
+        }else{
             ready = false;
         }
-        else{
-            powerScrew.brake(1);
-            ready = true;
-        }
     }
     else{
         powerScrew.brake(1);
@@ -702,16 +721,15 @@
         currentMillis  = millis();
         currentMillis2 = millis();
         
-        // PID Launcher Depan
-        if (currentMillis-previousMillis>=12.5)
+        // PID Launcher Belakang
+        if (currentMillis-previousMillis>=Ts)
         {
             rpm = (float)encLauncherBlk.getPulses();    
-            current_error = target_rpm - rpm;
-            sum_error = sum_error + current_error*12.5;
-            p = current_error*kpA;
-            d = (current_error-last_error)*kdA/12.5;
-            i = sum_error*kiA;
-            speed = p + d + i;
+            current_error1 = target_rpm - rpm;
+            a1 = kpA1 + kiA1*Ts/2 + kdA1/Ts;
+            b1 = -kpA1 + kiA1*Ts/2 - 2*kdA1/Ts;
+            c1 = kdA1/Ts;
+            speed = previous_speed1 + a1*current_error1 + b1*previous_error1_1 + c1*previous_error1_2;
             //init_speed();
             if(speed > maxSpeed){
                 launcherBlk.speed(maxSpeed);
@@ -722,31 +740,35 @@
             else {
                 launcherBlk.speed(speed);
             }
-            last_error = current_error;
+            previous_speed1 = speed;
+            previous_error1_2 = previous_error1_1;
+            previous_error1_1 = current_error1;
             encLauncherBlk.reset();
-          //pc.printf("%.04lf\n",rpm);
             previousMillis = currentMillis;
+            
         }
-        if (currentMillis2-previousMillis2>=12.5)
+        // PID Launcher Depan
+        if (currentMillis2-previousMillis2>=Ts)
         {
             rpm2 = (float)encLauncherDpn.getPulses();    
             current_error2 = target_rpm2 - rpm2;
-            sum_error2 = sum_error2 + current_error2*12.5;
-            p2 = current_error2*kpA;
-            d2 = (current_error2-last_error2)*kdA/12.5;
-            i2 = sum_error2*kiA;
-            speed2 = p2 + d2 + i2;
+            a2 = kpA2 + kiA2*Ts/2 + kdA2/Ts;
+            b2 = -kpA2 + kiA2*Ts/2 - 2*kdA2/Ts;
+            c2 = kdA2/Ts;
+            speed2 = previous_speed2 + a2*current_error2 + b2*previous_error2_1 + c2*previous_error2_2;
             //init_speed();
             if (speed2 > maxSpeed){
                 launcherDpn.speed(maxSpeed);
             }
-            else if ( speed < minSpeed){
+            else if ( speed2 < minSpeed){
                 launcherDpn.speed(minSpeed);
             }
             else{
                 launcherDpn.speed(speed2);
             }
-            last_error2 = current_error2;
+            previous_speed2 = speed2;
+            previous_error2_2 = previous_error2_1;
+            previous_error2_1 = current_error2;
             encLauncherDpn.reset();
             previousMillis2 = currentMillis2;
         }
@@ -756,12 +778,13 @@
     {
         launcherDpn.brake(1);
         launcherBlk.brake(1);
-        sum_error = 0;
-        sum_error2 = 0;
-        current_error = 0;
-        current_error2 = 0;
-        last_error = 0;
-        last_error2 = 0;
+        
+        previous_error1_1 = 0;
+        previous_error1_2 = 0;
+        previous_error2_1 = 0;
+        previous_error2_2 = 0;
+        previous_speed1 = 0;
+        previous_speed2 = 0;
     }     
 }
   
@@ -791,8 +814,8 @@
     while(1)
     {   
         // interupsi pembacaan PING setiap 30 ms
-        if(millis() - previousMillis4 >= 5){    //30
-            jarak_ping = (float)pingAtas.Read_cm()/2;
+        if(millis() - previousMillis4 >= 10){    //30
+            jarak_ping = (float)pingAtas.Read_cm();
             
             pingAtas.Send();
             previousMillis4 = millis();