Diff: main.cpp

Revision:

43:5e347df94a26

Parent:

42:6caf8bd5abbc

--- a/main.cpp	Mon Feb 20 13:29:52 2017 +0000
+++ b/main.cpp	Thu Mar 09 13:06:30 2017 +0000
@@ -1,7 +1,7 @@
 /****************************************************************************/
 /*                  PROGRAM UNTUK PID CLOSED LOOP                           */
 /*                                                                          */
-/*                  Last Update : 18 Februari 2017                          */
+/*                  Last Update : 8 Maret 2017                          */
 /*                                                                          */
 /*  - Digunakan encoder autonics                                            */
 /*  - Konfigurasi Motor dan Encoder sbb :                                   */
@@ -57,7 +57,7 @@
 // Variable Atas
 double speed, speed2;
 const double maxSpeed = 0.95, minSpeed = 0.0;
-const double kpA=0.6757, kdA=0.6757, kiA=0.00006757;
+const double kpA=0.6757, kdA=0.06757, kiA=0.00006757;
 double p,i,d;
 double p2,i2,d2;
 double last_error = 0, current_error, sum_error = 0;
@@ -77,9 +77,9 @@
 float keliling_robot    = PI*D_ROBOT;
 float speedT            = 0.2;
 float vpid              = 0;
-float PIVOT             = 0.27;      // PWM Pivot Kanan, Pivot Kiri
-float tuneDpn           = 0.35;    // Tunning PWM motor Depan
-float tuneBlk           = 0.3;      // Tunning PWM motor belakang
+float PIVOT             = 0.2;      // PWM Pivot Kanan, Pivot Kiri
+float tuneDpn           = 0.45;    // Tunning PWM motor Depan
+float tuneBlk           = 0.37;      // Tunning PWM motor belakang
 
 /* Variabel Encoder Bawah */
 float errTetha, Tetha;    // Variabel yang didapatkan encoder
@@ -189,17 +189,25 @@
          caseJoystick = 6;
     }  
     else if (joystick.L2_click){
-        // Target Pulse PID -- Motor Depan
+        // Target Pulse PID -- Motor 
          caseJoystick = 7;
     }
-    /*else if (joystick.R3_click){
-        // Target Pulse PID ++ Motor Belakang
-         caseJoystick = 8;
+    else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
+        // kiri + pivot kiri
+         caseJoystick = 14;
+    }
+    else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
+        // kiri + pivot kanan
+         caseJoystick = 15;
     }
-    else if (joystick.L3_click){
-        // Target Pulse PID -- Motor Belakang
-         caseJoystick = 9;
-    }*/
+    else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
+        // kanan + pivot kiri
+         caseJoystick = 16;
+    }
+    else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
+        // kanan + pivot kanan
+         caseJoystick = 17;
+    }
     else if (joystick.silang_click){
         // Pnemuatik ON
         caseJoystick = 10;
@@ -280,21 +288,21 @@
         case (3) : 
         {
             // Kanan
-            motorDpn.speed(-tuneDpn + pidBase(0.009,0,0));
-            motorBlk.speed(tuneBlk + pidBase(0.009,0,0));
+            //motorDpn.speed(-tuneDpn + pidBase(0.009,0,0));
+            //motorBlk.speed(tuneBlk + pidBase(0.009,0,0));
             //speedDpn = tuneDpn + pidBase(0.009,0,0)
             //speedBlk = tuneBlk + pidBase(0.009,0,0)
-            //motorDpn.speed(-tuneDpn);
-            //motorBlk.speed(tuneBlk);
+            motorDpn.speed(-tuneDpn);
+            motorBlk.speed(tuneBlk);
             break;
         }
         case (4) : 
         {
             // Kiri
-            motorDpn.speed(tuneDpn + pidBase(0.009,0,0));
-            motorBlk.speed(-tuneBlk + pidBase(0.009,0,0));
-            //motorDpn.speed(tuneDpn);
-            //motorBlk.speed(-tuneBlk);
+            //motorDpn.speed(tuneDpn + pidBase(0.009,0,0));
+            //motorBlk.speed(-tuneBlk + pidBase(0.009,0,0));
+            motorDpn.speed(tuneDpn+0.02);
+            motorBlk.speed(-tuneBlk);
             break;
         }
         case (5) : 
@@ -353,6 +361,34 @@
             //powerScrew.speed(pwmPowerDown);
             break;
         }
+        case (14) : 
+        {
+            // kiri + piv kiri
+            motorDpn.speed(tuneDpn+0.02);
+            motorBlk.brake(1);
+            break;
+        }
+        case (15) : 
+        {
+            // kiri + pivkanan
+            motorDpn.brake(1);
+            motorBlk.speed(-tuneBlk);
+            break;
+        }
+        case (16) : 
+        {
+            // kanan + pivkiri
+            motorDpn.brake(1);
+            motorBlk.speed(tuneBlk);
+            break;
+        }
+        case (17) : 
+        {
+            // kanan + pivkanan
+            motorDpn.speed(-tuneDpn);
+            motorBlk.brake(1);
+            break;
+        }
         default : 
         {
             motorDpn.brake(1);
@@ -387,10 +423,10 @@
         else if(!limitTengah){
             isReload = true;
         }
-        else if((jarak_ping > 4) && !flag_Pneu){
+        else if((jarak_ping > 3.8) && !flag_Pneu){
             powerScrew.speed(pwmPowerUp);
         }
-        else if((jarak_ping < 3.5 ) && !flag_Pneu) {
+        else if((jarak_ping < 3.3 ) && !flag_Pneu) {
             powerScrew.speed(-0.1);
         }
         else{
@@ -512,7 +548,7 @@
             aktuator();
             launcher();
             reloader();
-            if ((millis()-previousMillis3 >= 370)&&(flag_Pneu)){
+            if ((millis()-previousMillis3 >= 320)&&(flag_Pneu)){
                 pneumatik = 1;
                 flag_Pneu = false;
             }
@@ -523,8 +559,14 @@
         }
         
         if(millis() - previousMillis5 >= 400){
-            display.write(0,((int) target_rpm2-2) / 10);
-            display.write(1,((int)target_rpm2-2) % 10);
+            //display.write(0,((int) target_rpm2-2) / 10);
+            //display.write(1,((int)target_rpm2-2) % 10);
+            //display.write(2, (int)target_rpm2 / 10);
+            //display.write(3, (int)target_rpm2 % 10);
+            //display.setColon(true);
+            
+            display.write(0,((int) rpm2) / 10);
+            display.write(1,((int)rpm2) % 10);
             display.write(2, (int)target_rpm2 / 10);
             display.write(3, (int)target_rpm2 % 10);
             display.setColon(true);