Revision 43:3807a95aa284, committed 2017-03-12

Comitter:

Najib_irvani

Date:

Sun Mar 12 06:56:53 2017 +0000

Parent:

42:6caf8bd5abbc

Commit message:

Update 12 Maret 2017. Tambahan kombinasi tombol

Changed in this revision

--- a/main.cpp	Mon Feb 20 13:29:52 2017 +0000
+++ b/main.cpp	Sun Mar 12 06:56:53 2017 +0000
@@ -1,7 +1,8 @@
+
 /****************************************************************************/
 /*                  PROGRAM UNTUK PID CLOSED LOOP                           */
 /*                                                                          */
-/*                  Last Update : 18 Februari 2017                          */
+/*                  Last Update : 11 Maret 2017                          */
 /*                                                                          */
 /*  - Digunakan encoder autonics                                            */
 /*  - Konfigurasi Motor dan Encoder sbb :                                   */
@@ -9,8 +10,8 @@
 /*                     /                      \    Rode Depan Belakang:     */
 /*                    /      2 (Belakang)      \   Omniwheel                */
 /*                   |                          |                           */
-/*                   | 3 (Encoder)            4 |  Roda Kiri Kanan:         */
-/*                   |                          |  Fixed Wheel              */
+/*                   | 3 (kiri)       4 (kanan) |  Roda Kiri Kanan:         */
+/*                   |                          |  Omniwheel                */
 /*                    \       1 (Depan)        /                            */
 /*                     \______________________/    Putaran berlawanan arah  */
 /*                                                 jarum jam positif        */
@@ -57,13 +58,13 @@
 // 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;
 double last_error2 = 0, current_error2, sum_error2 = 0;
 float rpm, rpm2;
-float target_rpm = 15.0, target_rpm2 = 17.0; 
+float target_rpm = 8.0, target_rpm2 = 10.0; 
 const float maxRPM = 28, minRPM = 0; // Limit 25 atau 27
 
 const float pwmPowerUp        = 0.8;
@@ -77,9 +78,19 @@
 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.62;     // Tunning PWM motor Depan
+float tuneBlk           = 0.62;     // Tunning PWM motor belakang
+float tuneR             = 0.72;
+float tuneL             = 0.72;
+float serong            = 0.65;
+float rasio             = 1.4545;
+
+/* variable tunning */
+float tunning_L;
+float tunning_R;
+float tunning_Dpn;
+float tunning_Blk;
 
 /* Variabel Encoder Bawah */
 float errTetha, Tetha;    // Variabel yang didapatkan encoder
@@ -125,13 +136,15 @@
 encoderKRAI encLauncherDpn( PD_2, PC_12, 28, encoderKRAI::X4_ENCODING);
 
 /* Deklarasi Motor Base */
-Motor motorDpn(PB_9, PA_12, PC_5);    // pwm, fwd, rev
-Motor motorBlk(PB_6, PB_1, PB_12);    // pwm, fwd, rev
+Motor motorDpn(PB_9, PA_12, PC_5);  // pwm, fwd, rev
+Motor motorBlk(PB_6, PB_1, PB_12);    
+Motor motorL  (PA_11, PA_6, PA_7);
+Motor motorR  (PB_7, PA_14, PA_15);
 
 /* Deklarasi Motor Launcher */
 Motor launcherDpn(PA_8,PC_2,PC_1);    // pwm ,fwd, rev
 Motor launcherBlk(PA_10, PC_3, PC_0); // pwm, fwd, rev 
-Motor powerScrew(PB_7, PA_14, PA_15); // pwm, fwd, rev                
+Motor powerScrew(PA_9, PA_4, PC_15); // pwm, fwd, rev                
 
 /* Deklarasi Penumatik Launcher */
 DigitalOut pneumatik(PB_3, PullUp);
@@ -162,13 +175,89 @@
 //---------------------------------------------------//
     
     int caseJoystick;
-    if (!joystick.L1 && joystick.R1) {
+    if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
         // Pivot Kanan
         caseJoystick = 1;
     } 
-    else if (!joystick.R1 && joystick.L1) {
+    else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
         // Pivot Kiri
         caseJoystick = 2;
+    }
+    else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri))  {  
+        // Kanan + Rotasi kanan
+        caseJoystick = 17;
+    } 
+    else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri))  {  
+        // Kanan + Rotasi kiri
+        caseJoystick = 18;
+    }
+    else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri))  {  
+        // Kiri + Rotasi kanan
+        caseJoystick = 19;
+    } 
+    else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri))  {  
+        // Kanan + Rotasi kiri
+        caseJoystick = 20;
+    }
+    else if ((joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
+        // Maju + Rotasi kanan
+        caseJoystick = 21;
+    } 
+    else if ((!joystick.R1)&&(joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
+        // Maju + Rotasi kiri
+        caseJoystick = 22;
+    }
+    else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
+        // Mundur + Rotasi kanan
+        caseJoystick = 23;
+    } 
+    else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
+        // Mundur + Rotasi kiri
+        caseJoystick = 24;
+    }
+    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.segitiga_click))  {  
+        // Kanan + segitiga
+        caseJoystick = 25;
+    }
+    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.segitiga_click))  {  
+        // Kiri + segitiga
+        caseJoystick = 26;
+    }
+    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.lingkaran_click))  {  
+        // Kanan + lingkaran
+        caseJoystick = 27;
+    }
+    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.lingkaran_click))  {  
+        // Kiri + lingkaran
+        caseJoystick = 28;
+    }
+    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.kotak_click))  {  
+        // Kanan + kotak
+        caseJoystick = 29;
+    }
+    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.kotak_click))  {  
+        // Kiri + kotak
+        caseJoystick = 30;
+    }
+    else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {   
+        // Serong kanan maju
+        caseJoystick = 13;
+         //pc.printf("bawah");       
+    }
+    else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {   
+        // Serong kiri maju
+        caseJoystick = 14;
+         //pc.printf("bawah");       
+    }
+    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {   
+        // Serong kanan mundur
+        caseJoystick = 15;
+         //pc.printf("bawah");       
+    }
+    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {   
+        // Serong kiri mundur
+        caseJoystick = 16;
+         //pc.printf("bawah");       
     } 
     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri))  {   
         // Kanan
@@ -179,6 +268,16 @@
         // Kiri
         caseJoystick = 4;
          //pc.printf("kiri");       
+    }
+    else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {   
+        // Atas -- Maju
+        caseJoystick = 8;
+         //pc.printf("atas");       
+    }
+    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {   
+        // Bawah -- Mundur
+        caseJoystick = 9;
+         //pc.printf("bawah");       
     } 
     else if (joystick.segitiga_click){
         // Motor Launcher
@@ -189,33 +288,21 @@
          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.L3_click){
-        // Target Pulse PID -- Motor Belakang
-         caseJoystick = 9;
-    }*/
     else if (joystick.silang_click){
         // Pnemuatik ON
         caseJoystick = 10;
     }
-    else if ((joystick.atas)&&(!joystick.bawah))  {   
+    else if ((joystick.lingkaran_click)&&(!joystick.kotak_click))  {   
         // Power Screw Up
         caseJoystick = 11;
     } 
-    else if ((!joystick.atas)&&(joystick.bawah)) {   
+    else if ((joystick.kotak_click)&&(!joystick.lingkaran_click)) {   
         // Power Screw Down
         caseJoystick = 12;       
     } 
-    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {  
-        // Break
-        caseJoystick = 13; 
-    }
     
     return(caseJoystick);
 }
@@ -266,7 +353,8 @@
             // Pivot Kanan
             motorDpn.speed(-PIVOT);
             motorBlk.speed(-PIVOT);
-            setCenter();
+            motorR.speed(-rasio*PIVOT);
+            motorL.speed(-rasio*PIVOT);
             break;
         }
         case (2): 
@@ -274,27 +362,193 @@
             // Pivot Kiri
             motorDpn.speed(PIVOT);
             motorBlk.speed(PIVOT);
-            setCenter();
+            motorR.speed(rasio*PIVOT);
+            motorL.speed(rasio*PIVOT);
+            break;
+        }
+        case (17): 
+        {       
+            // Kanan + Rotasi Kanan
+            motorDpn.speed(-PIVOT);
+            motorBlk.speed(-PIVOT);
+            motorR.speed(-rasio*PIVOT);
+            motorL.speed(-rasio*PIVOT);
+            break;
+        }
+        case (18): 
+        {
+            // Kanan + Rotasi Kiri
+            motorDpn.speed(PIVOT);
+            motorBlk.speed(PIVOT);
+            motorR.speed(rasio*PIVOT);
+            motorL.speed(rasio*PIVOT);
+            break;
+        }
+        case (19): 
+        {       
+            // Kiri + Rotasi Kanan
+            motorDpn.speed(-PIVOT);
+            motorBlk.speed(-PIVOT);
+            motorR.speed(-rasio*PIVOT);
+            motorL.speed(-rasio*PIVOT);
+            break;
+        }
+        case (20): 
+        {
+            // Kiri + Rotasi Kiri
+            motorDpn.speed(PIVOT);
+            motorBlk.speed(PIVOT);
+            motorR.speed(rasio*PIVOT);
+            motorL.speed(rasio*PIVOT);
+            break;
+        }
+        case (21): 
+        {       
+            // Maju + Rotasi Kanan
+            motorDpn.speed(-PIVOT);
+            motorBlk.speed(-PIVOT);
+            motorR.speed(-rasio*PIVOT);
+            motorL.speed(-rasio*PIVOT);
+            break;
+        }
+        case (22): 
+        {
+            // Maju + Rotasi Kiri
+            motorDpn.speed(PIVOT);
+            motorBlk.speed(PIVOT);
+            motorR.speed(rasio*PIVOT);
+            motorL.speed(rasio*PIVOT);
+            break;
+        }
+        case (23): 
+        {       
+            // Mundur + Rotasi Kanan
+            motorDpn.speed(-PIVOT);
+            motorBlk.speed(-PIVOT);
+            motorR.speed(-rasio*PIVOT);
+            motorL.speed(-rasio*PIVOT);
+            break;
+        }
+        case (24): 
+        {
+            // Mundur + Rotasi Kiri
+            motorDpn.speed(PIVOT);
+            motorBlk.speed(PIVOT);
+            motorR.speed(rasio*PIVOT);
+            motorL.speed(rasio*PIVOT);
+            break;
+        }
+        case (25): 
+        {
+            // Kanan + segitiga
+            isLauncher = !isLauncher;
+            break;
+        }
+        case (26): 
+        {
+            // Kiri + segitiga
+            isLauncher = !isLauncher;
+            break;
+        }
+        case (27): 
+        {
+            // Kanan + lingkaran
+            ReloadOn = !ReloadOn;
+            isReload = false;
+            break;
+        }
+        case (28): 
+        {
+            // Kiri + lingkaran
+            ReloadOn = !ReloadOn;
+            isReload = false;
+            break;
+        }
+        case (29): 
+        {
+            // Kanan + kotak
+            ReloadOn = !ReloadOn;
+            isReload = true;
+            break;
+        }
+        case (30): 
+        {
+            // Kiri + kotak
+            ReloadOn = !ReloadOn;
+            isReload = true;
+            break;
+        }
+        case (13) : 
+        {
+            // Serong kanan maju
+            motorDpn.speed(-serong);
+            motorL.brake(-rasio*serong);
+            motorBlk.speed(serong);
+            motorR.brake(rasio*serong);
+            break;
+        }
+        case (14) : 
+        {
+            // Serong kiri maju
+            motorDpn.speed(serong);
+            motorR.brake(rasio*serong);
+            motorBlk.speed(-serong);
+            motorL.brake(-rasio*serong);
+            break;
+        }
+        case (15) : 
+        {
+            // Serong kanan mundur 
+            motorR.brake(-rasio*serong);
+            motorBlk.speed(serong);
+            motorL.brake(rasio*serong);
+            break;
+        }
+        case (16) : 
+        {
+            // Serong kiri mundur
+            motorDpn.speed(serong);
+            motorL.brake(rasio*serong);
+            motorBlk.speed(-serong);
+            motorR.brake(-rasio*serong);
             break;
         }
         case (3) : 
         {
             // Kanan
-            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);
+            motorR.brake(1);
+            motorL.brake(1);
             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);
+            motorBlk.speed(-tuneBlk);
+            motorR.brake(1);
+            motorL.brake(1);
+            break;
+        }
+        case (8) : 
+        {
+            // Maju
+            //init_rpm();
+            motorR.speed(tuneR);
+            motorL.speed(-tuneL);
+            motorDpn.brake(1);
+            motorBlk.brake(1);
+            break;
+        }
+        case (9) : 
+        {
+            // Mundur
+            //init_rpm();
+            motorR.speed(-tuneR);
+            motorL.speed(tuneL);
+            motorDpn.brake(1);
+            motorBlk.brake(1);
             break;
         }
         case (5) : 
@@ -319,18 +573,6 @@
             init_rpm();
             break;
         }
-        /*case (8) : 
-        {
-            // Target Pulse PID ++ Motor Belakang=
-            //init_rpm();
-            break;
-        }
-        case (9) : 
-        {
-            // Target Pulse PID -- Motor Belakang
-            //init_rpm();
-            break;
-        }*/
         case (10) : 
         {
             // Pneumatik
@@ -342,34 +584,23 @@
         case (11) : 
         {
             // Power Screw Up
-            //powerScrew.speed(pwmPowerUp);
             ReloadOn = !ReloadOn;
-            //powerScrew.speed(pwmPowerUp);
+            isReload = false;
             break;
         }
         case (12) : 
         {
             // Power Screw Down
-            //powerScrew.speed(pwmPowerDown);
+            ReloadOn = !ReloadOn;
+            isReload = true;
             break;
         }
         default : 
         {
             motorDpn.brake(1);
             motorBlk.brake(1);
-           /* if(isReload){
-                powerScrew.speed(pwmPowerDown);
-                if(!limitBawah){
-                    isReload = false;
-                    ReloadOn = false;
-                }
-            }
-            else if(!limitTengah){
-                isReload = true;
-            }
-            else{
-                powerScrew.brake(1);
-            }*/
+            motorR.brake(1);
+            motorL.brake(1);
         }   
     } // End Switch
  }
@@ -387,10 +618,10 @@
         else if(!limitTengah){
             isReload = true;
         }
-        else if((jarak_ping > 4) && !flag_Pneu){
+        else if((jarak_ping > 6.5) && !flag_Pneu){
             powerScrew.speed(pwmPowerUp);
         }
-        else if((jarak_ping < 3.5 ) && !flag_Pneu) {
+        else if((jarak_ping < 6 ) && !flag_Pneu) {
             powerScrew.speed(-0.1);
         }
         else{
@@ -509,10 +740,11 @@
             joystick.olah_data();
             // Masuk ke case joystick
             case_joy = case_joystick();
+            pc.printf("%d\n",case_joy);
             aktuator();
             launcher();
             reloader();
-            if ((millis()-previousMillis3 >= 370)&&(flag_Pneu)){
+            if ((millis()-previousMillis3 >= 320)&&(flag_Pneu)){
                 pneumatik = 1;
                 flag_Pneu = false;
             }
@@ -523,8 +755,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);

--- a/mbed.bld	Mon Feb 20 13:29:52 2017 +0000
+++ b/mbed.bld	Sun Mar 12 06:56:53 2017 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed/builds/25aea2a3f4e3
\ No newline at end of file
+http://mbed.org/users/mbed_official/code/mbed/builds/e1686b8d5b90
\ No newline at end of file