Kodingan 5 April setting kecepatan power screw

Dependencies:   DigitDisplay Motor PID Ping mbed millis

Fork of MainProgram_BaseBaru_fix_omni12Feb by KRAI 2017

Files at this revision

API Documentation at this revision

Comitter:
Najib_irvani
Date:
Wed Apr 05 12:44:30 2017 +0000
Parent:
43:3807a95aa284
Commit message:
Kodingan 5 April setting kecepatan power screw

Changed in this revision

PS3Arduino.txt Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
--- a/PS3Arduino.txt	Sun Mar 12 06:56:53 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,299 +0,0 @@
-/*********************************************************************************************/
-/**                                                                                         **/
-/** PROGRAM KOMUNIKASI DATA JOYSTIK PS3                                                     **/
-/**                                                                                         **/
-/** Joystik PS3 -> Arduino -> STM32Nucleo                                                   **/
-/**                                                                                         **/
-/** Fanny Achmad Hindrarta                                                                  **/
-/** EL'12 - 13212076                                                                        **/
-/**                                                                                         **/
-/** Last Update : 01 Februar1 2015, 20.30                                                   **/
-/*********************************************************************************************/
- 
-#include <PS3BT.h>
-#include <usbhub.h>
-//#include <PS3USB.h>
-#include "Arduino.h"
-// Satisfy IDE, which only needs to see the include statment in the ino.
-#ifdef dobogusinclude
-#include <spi4teensy3.h>
-#endif
- 
- 
-USB Usb;
- 
-// Bluetooth
-// You can create the instance of the class in two ways 
-BTD Btd(&Usb); // You have to create the Bluetooth Dongle instance like so
-//You can create the instance of the class in two ways 
-PS3BT PS3(&Btd); // This will just create the instance
-//PS3BT PS3(&Btd, 0x00, 0x15, 0x83, 0x3D, 0x0A, 0x57); // This will also store the bluetooth address - this can be obtained from the dongle when running the sketch
-/*
-// USB
-// You can create the instance of the class in two ways
-PS3USB PS3(&Usb); // This will just create the instance
-//PS3USB PS3(&Btd, 0x00, 0x15, 0x83, 0x3D, 0x0A, 0x57); // This will also store the bluetooth address - this can be obtained from the dongle when running the sketch
-*/
- 
-boolean printAngle;
-uint8_t state = 0;
- 
-// Deklarasi tombol
-boolean kiri_click=0, kanan_click=0, atas_click=0, bawah_click=0;
-boolean segitiga_click=0, lingkaran_click=0, kotak_click=0, silang_click=0;
-boolean L1_click=0, R1_click=0, L3_click=0, R3_click=0;
-boolean START_click=0, SELECT_click=0, PS_click=0;
- 
-// Deklarasi variabel tombol analog
-unsigned char LX, LY, RX, RY, aL2, aR2;
- 
-// Deklarasi varibel data yang dikirim
-unsigned char button;
-unsigned char RL;
-unsigned int button_click;
-unsigned int RL_click;
- 
-void setup_joystik()
-{
-    while (!Serial) // Wait for serial port to connect - used on Leonardo, Teensy and other boards with built-in USB CDC serial connection
-        {
-                Serial.write(0x88);
-        Serial.write(0x09);
-        }
-    if (Usb.Init() == -1) {
-        //Serial.print(F("\r\nOSC did not start"));
-        while (1) //halt
-                {
-                         Serial.write(0x88);
-                 Serial.write(0x09);
-                }
-    }
-    //Serial.print(F("\r\nPS3 USB Library Started"));
-}
- 
-/*********************************************************************************************/
-/**                                                                                         **/
-/** FUNGSI PENGIRIMAN DATA                                                                  **/
-/** -   Data yang akan dikirim adalah paket data 8-bit dengan urutan sebagai berikut        **/
-/** |------|------|--------|----|--------------|----------|----|----|----|----|----|----|   **/
-/** | 0x88 | 0x08 | button | RL | button_click | RL_click | R2 | L2 | RX | RY | LX | LY |   **/
-/** |------|------|--------|----|--------------|----------|----|----|----|----|----|----|   **/
-/**                                                                                         **/
-/*********************************************************************************************/
- 
-void kirimdatajoystik()
-{
-    
-    Serial.write(0x88);
-    Serial.write(0x08);
-    Serial.write(button);
-    Serial.write(RL);
-    Serial.write(button_click);
-    Serial.write(RL_click);
-    Serial.write(aR2);
-    Serial.write(aL2);
-    Serial.write(RX);
-    Serial.write(RY);
-    Serial.write(LX);
-    Serial.write(LY);
- 
-// Debug    
- 
-/*
-    Serial.print(millis());
-    Serial.print("\t");
-    Serial.print(button);
-    Serial.print("\t");
-    Serial.print(RL);
-    Serial.print("\t");
-    Serial.print(button_click);
-    Serial.print("\t");
-    Serial.print(RL_click);
-    Serial.print("\t");
-    Serial.print(aR2);
-    Serial.print("\t");
-    Serial.print(aL2);
-    Serial.print("\t");
-    Serial.print(RX);
-    Serial.print("\t");
-    Serial.print(RY);
-    Serial.print("\t");
-    Serial.print(LX);
-    Serial.print("\t");
-    Serial.println(LY);
-*/  
-}
- 
-/*****************************************************************************************/
-/** SETUP REGISTER dan INISIALISASI                                                     **/
-/** -   Setup Joystik                                                                   **/
-/** -   Baud Rate Serial 115200, 8-bit, 1 stop, 0 parity                                **/
-/*****************************************************************************************/
- 
-void setup() {
-    Serial.begin(115200);
- 
-    setup_joystik();
-}
- 
- 
-/*****************************************************************************************/
-/**                                                                                     **/
-/** FUNGSI PEMBACAAN DATA JOYSTIK                                                       **/
-/** -   Data dari Joystik dikirim melalui Bluetooth.                                    **/
-/**     Lihat Library "PS3BT.h"                                                         **/
-/** -   Data tombol dari joystik adalah data 1-bit. Pengiriman data melalui USART       **/
-/**     adalah 8-bit. Agar pengiriman data efisien, maka data 1-bit digabungkan         **/
-/**     agar terbentuk data 8-bit                                                       **/
-/** -   Penggabungan data segitiga, longkaran, silang, kotak, arah atas, arah kanan,    **/
-/**     arah bawah, arah kiri disimpan dalam variabel "button"                          **/
-/** -   Urutan data pada variabel "button" dan "button_click"                           **/
-/**     adalah sebagai berikut                                                          **/
-/**                                                                                     **/
-/**         |------|-------|-------|------|-------|--------|-----------|----------|     **/
-/** Bit Ke  |   7  |   6   |   5   |   4  |   3   |    2   |      1    |     0    |     **/
-/**         |------|-------|-------|------|-------|--------|-----------|----------|     **/
-/** Data    | kiri | bawah | kanan | atas | kotak | silang | lingkaran | segitiga |     **/
-/**         |------|-------|-------|------|-------|--------|-----------|----------|     **/
-/**                                                                                     **/
-/** -   Penggabungan data R1, R2, L1, L2, R3, L3, START, dan SELECT disimpan dalam      **/
-/**     variabel "RL"                                                                   **/
-/** -   Urutan data pada variabel "RL" dan "RL_click" adalah                            **/
-/**     sebagai berikut                                                                 **/
-/**                                                                                     **/
-/**         |----|--------|-------|----|----|----|----|                                 **/
-/** Bit Ke  |  6 |    5   |   4   |  3 |  2 |  1 |  0 |                                 **/
-/**         |----|--------|-------|----|----|----|----|                                 **/
-/** Data    | PS | SELECT | START | L3 | L1 | R3 | R1 |                                 **/
-/**         |----|--------|-------|----|----|----|----|                                 **/
-/**                                                                                     **/
-/*****************************************************************************************/
- 
-void loop() {
-    button = 0;
-    RL = 0;
-        button_click=0;
-        RL_click=0;
-    Usb.Task();
- 
-    // Pembacaan data joystik dilakukan jika PS3 tersambung ke Arduino
-    if (PS3.PS3Connected || PS3.PS3NavigationConnected) {
-        // Pembacaan dan penggabungan data segitiga, lingkaran, silang, kotak, atas, kanan, bawah, kiri
-        // Data bernilai '1' jika tombol ditekan
-        if(PS3.getButtonPress(TRIANGLE)){
-            button = button + (0x1 << 0);
-        }
-        if(PS3.getButtonPress(CIRCLE)){
-            button = button + (0x1 << 1);
-        }
-        if(PS3.getButtonPress(CROSS)){
-            button = button + (0x1 << 2);
-        }
-        if(PS3.getButtonPress(SQUARE)){
-            button = button + (0x1 << 3);
-        }
-        if(PS3.getButtonPress(UP)){
-            button = button + (0x1 << 4);
-        }
-        if(PS3.getButtonPress(RIGHT)){
-            button = button + (0x1 << 5);
-        }
-        if(PS3.getButtonPress(DOWN)){
-            button = button + (0x1 << 6);
-        }
-        if(PS3.getButtonPress(LEFT)){
-            button = button + (0x1 << 7);
-        }
- 
-        // Pembacaan dan penggabungan data R1, R3, L1, L3, START, SELECT, dan PS
-        // Data bernilai '1' jika tombol ditekan
-        if(PS3.getButtonPress(R1)){
-            RL = RL + (0x1 << 0);
-        }
-        if(PS3.getButtonPress(R3)){
-            RL = RL + (0x1 << 1);
-        }
-        if(PS3.getButtonPress(L1)){
-            RL = RL + (0x1 << 2);
-        }
-        if(PS3.getButtonPress(L3)){
-            RL = RL + (0x1 << 3);
-        }
-        if(PS3.getButtonPress(START)){
-            RL = RL + (0x1 << 4);
-        }
-        if(PS3.getButtonPress(SELECT)){
-            RL = RL + (0x1 << 5);
-        }
-        if(PS3.getButtonPress(PS)){
-            RL = RL + (0x1 << 6);
-        }
- 
-        // Pembacaan dan penggabungan data segitiga, lingkaran, silang, kotak, atas, kanan, bawah, kiri
-        // Data bernilai '1' hanya saat tombol pertama kali ditekan
-        if(PS3.getButtonClick(TRIANGLE)){
-            button_click = button_click + (0x1 << 0);
-        }
-        if(PS3.getButtonClick(CIRCLE)){
-            button_click = button_click + (0x1 << 1);
-        }
-        if(PS3.getButtonClick(CROSS)){
-            button_click = button_click + (0x1 << 2);
-        }
-        if(PS3.getButtonClick(SQUARE)){
-            button_click = button_click + (0x1 << 3);
-        }
-        if(PS3.getButtonClick(UP)){
-            button_click = button_click + (0x1 << 4);
-        }
-        if(PS3.getButtonClick(RIGHT)){
-            button_click = button_click + (0x1 << 5);
-        }
-        if(PS3.getButtonClick(DOWN)){
-            button_click = button_click + (0x1 << 6);
-        }
-        if(PS3.getButtonClick(LEFT)){
-            button_click = button_click + (0x1 << 7);
-        }
- 
-        if(PS3.getButtonClick(R1)){
-            RL_click = RL_click + (0x1 << 0);
-        }
-        if(PS3.getButtonClick(R3)){
-            RL_click = RL_click + (0x1 << 1);
-        }
-        if(PS3.getButtonClick(L1)){
-            RL_click = RL_click + (0x1 << 2);
-        }
-        if(PS3.getButtonClick(L3)){
-            RL_click = RL_click + (0x1 << 3);
-        }
-        if(PS3.getButtonClick(START)){
-            RL_click = RL_click + (0x1 << 4);
-        }
-        if(PS3.getButtonClick(SELECT)){
-            RL_click = RL_click + (0x1 << 5);
-        }
-        if(PS3.getButtonClick(PS)){
-            RL_click = RL_click + (0x1 << 6);
-        }
- 
-        // Tombol Analog
-        LX = PS3.getAnalogHat(LeftHatX);
-        LY = PS3.getAnalogHat(LeftHatY);
-        RX = PS3.getAnalogHat(RightHatX);
-        RY = PS3.getAnalogHat(RightHatY);
-        
-        aL2 = PS3.getAnalogButton(L2);
-        aR2 = PS3.getAnalogButton(R2);
-            
-                kirimdatajoystik();
-    } else {
-        // PS3 Disconnected
-        Serial.write(0x88);
-        Serial.write(0x09);
-    }
-    delay(3);
-}
- 
-      
\ No newline at end of file
--- a/main.cpp	Sun Mar 12 06:56:53 2017 +0000
+++ b/main.cpp	Wed Apr 05 12:44:30 2017 +0000
@@ -25,15 +25,14 @@
 /*  Kanan =>                                                                */
 /*  Kiri  =>                                                                */
 /*                                                                          */
-/*  Tombol silang    => Power Screw Aktif                                   */
+/*  Tombol silang    => Pneumatik aktif                                     */
 /*  Tombol segitiga  => Aktif motor Launcher                                */
-/*  Tombol lingkaran => Aktif Pneumatik Launcher                            */
+/*  Tombol lingkaran => Reloader naik                                       */
+/*  Tombol kotak     => Reloader turun                                      */
 /*  Tombol L1        => Pivot Kiri                                          */
 /*  Tombol R1        => Pivot Kanan                                         */
-/*  Tombol L3        => PWM Motor Belakang Dikurangi                        */
-/*  Tombol R3        => PWM Motor Belakang Ditambah                         */
-/*  Tombol L2        => PWM Motor Depan Dikurangi                           */
-/*  Tombol R2        => PWM Motor Depan Ditambah                            */
+/*  Tombol L2        => Kurang PWM Motor Launcher                           */
+/*  Tombol R2        => Tambah PWM Motor Launcher                           */
 /*                                                                          */
 /*  Bismillahirahmanirrahim                                                 */
 /*  Jagalah Kebersihan Kodingan                                             */
@@ -53,22 +52,21 @@
 #define PI 3.14159265
 #define D_ENCODER 10        // Diameter Roda Encoder
 #define D_ROBOT 80          // Diameter Roda Robot
-#define PERPINDAHAN 1       // Perpindahan ke kanan dan kiri
 
 // Variable Atas
 double speed, speed2;
 const double maxSpeed = 0.95, minSpeed = 0.0;
-const double kpA=0.6757, kdA=0.06757, kiA=0.00006757;
+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;
 float rpm, rpm2;
-float target_rpm = 8.0, target_rpm2 = 10.0; 
+float target_rpm = 17.0, target_rpm2 = 17.0; // selisih 4 bagus, sama bagus
 const float maxRPM = 28, minRPM = 0; // Limit 25 atau 27
 
-const float pwmPowerUp        = 0.8;
-const float pwmPowerDown      = -0.9;
+const float pwmPowerUp        = 1.0;
+const float pwmPowerDown      = -1.0;
 
 float jarak_ping=0;
  
@@ -77,13 +75,12 @@
 float keliling_enc      = PI*D_ENCODER;
 float keliling_robot    = PI*D_ROBOT;
 float speedT            = 0.2;
-float vpid              = 0;
-float PIVOT             = 0.2;      // PWM Pivot Kanan, Pivot Kiri
+float PIVOT             = 0.17;      // 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 tuneR             = 0.78;
 float tuneL             = 0.72;
-float serong            = 0.65;
+float serong            = 0.4;
 float rasio             = 1.4545;
 
 /* variable tunning */
@@ -111,6 +108,7 @@
 bool isReload = false;
 bool ReloadOn = false;
 bool flag_Pneu = false;
+bool ready = false;
 
 /*****************************************************/
 /*   Definisi Prosedur, Fungsi dan Setting Pinout    */
@@ -132,33 +130,35 @@
 encoderKRAI encoderBase(PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING);   //inA, inB, pin Indeks (NC = tak ada), 2xresolusi, mode pembacaan
 
 /* Deklarasi Encoder Launcher */
-encoderKRAI encLauncherBlk( PC_10, PC_11, 28, encoderKRAI::X4_ENCODING);
-encoderKRAI encLauncherDpn( PD_2, PC_12, 28, encoderKRAI::X4_ENCODING);
+encoderKRAI encLauncherDpn( PC_10, PC_11, 28, encoderKRAI::X4_ENCODING);
+encoderKRAI encLauncherBlk( PC_12, PD_2, 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);    
-Motor motorL  (PA_11, PA_6, PA_7);
-Motor motorR  (PB_7, PA_14, PA_15);
+Motor motorDpn(PB_7, PC_3, PC_0); //(PB_9, PA_12, PC_5);
+//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);
 
 /* 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(PA_9, PA_4, PC_15); // pwm, fwd, rev                
+Motor launcherDpn(PA_5,PB_12,PA_11);    // pwm ,fwd, rev
+Motor launcherBlk(PB_3, PC_4, PA_10); // pwm, fwd, rev 
+Motor powerScrew(PB_10, PB_14, PB_13); // pwm, fwd, rev                
 
 /* Deklarasi Penumatik Launcher */
-DigitalOut pneumatik(PB_3, PullUp);
+DigitalOut pneumatik(PA_4, PullUp);
 
 /*Dekalrasi Limit Switch */
 //DigitalIn infraAtas(PC_9, PullUp);
-DigitalIn limitTengah(PB_10, PullUp);
-DigitalIn limitBawah(PC_8, PullUp);
+DigitalIn limitTengah(PA_9, PullUp);
+DigitalIn limitBawah(PC_7, PullUp);
+DigitalIn limitBawah1(PA_7, PullUp);
 
 /*deklarasi PING ultrasonic*/
-Ping pingAtas(PC_9);
+Ping pingAtas(PC_15);
 
 /*Deklarasi Display*/
-DigitDisplay display (D15, D4);
+DigitDisplay display (PA_8, PC_8);
 
 /****************************************************/
 /*         Deklarasi Fungsi dan Procedure           */
@@ -183,6 +183,14 @@
         // Pivot Kiri
         caseJoystick = 2;
     }
+    else if ((joystick.START_click)&&(!joystick.SELECT_click)) {   
+        // tambah rpm dengan nilai tertentu
+        caseJoystick = 31;     
+    }
+    else if ((!joystick.START_click)&&(joystick.SELECT_click)) {   
+        // kurangi rpm dengan nilai tertentu
+        caseJoystick = 32;     
+    }
     else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri))  {  
         // Kanan + Rotasi kanan
         caseJoystick = 17;
@@ -241,43 +249,35 @@
     }
     else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {   
         // Serong kanan maju
-        caseJoystick = 13;
-         //pc.printf("bawah");       
+        caseJoystick = 13;     
     }
     else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {   
         // Serong kiri maju
-        caseJoystick = 14;
-         //pc.printf("bawah");       
+        caseJoystick = 14;      
     }
     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {   
         // Serong kanan mundur
-        caseJoystick = 15;
-         //pc.printf("bawah");       
+        caseJoystick = 15;       
     }
     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {   
         // Serong kiri mundur
-        caseJoystick = 16;
-         //pc.printf("bawah");       
+        caseJoystick = 16;       
     } 
     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri))  {   
         // Kanan
         caseJoystick = 3;
-         //pc.printf("kanan");
     } 
     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {   
         // 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");       
+        caseJoystick = 8;       
     }
     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {   
         // Bawah -- Mundur
-        caseJoystick = 9;
-         //pc.printf("bawah");       
+        caseJoystick = 9;       
     } 
     else if (joystick.segitiga_click){
         // Motor Launcher
@@ -482,35 +482,36 @@
         {
             // Serong kanan maju
             motorDpn.speed(-serong);
-            motorL.brake(-rasio*serong);
+            motorL.speed(-serong);
             motorBlk.speed(serong);
-            motorR.brake(rasio*serong);
+            motorR.speed(serong);
             break;
         }
         case (14) : 
         {
             // Serong kiri maju
             motorDpn.speed(serong);
-            motorR.brake(rasio*serong);
+            motorR.speed(serong);
             motorBlk.speed(-serong);
-            motorL.brake(-rasio*serong);
+            motorL.speed(-serong);
             break;
         }
         case (15) : 
         {
             // Serong kanan mundur 
-            motorR.brake(-rasio*serong);
+            motorDpn.speed(-serong);
+            motorR.speed(-serong);
             motorBlk.speed(serong);
-            motorL.brake(rasio*serong);
+            motorL.speed(serong);
             break;
         }
         case (16) : 
         {
             // Serong kiri mundur
             motorDpn.speed(serong);
-            motorL.brake(rasio*serong);
+            motorL.speed(serong);
             motorBlk.speed(-serong);
-            motorR.brake(-rasio*serong);
+            motorR.speed(-serong);
             break;
         }
         case (3) : 
@@ -534,7 +535,6 @@
         case (8) : 
         {
             // Maju
-            //init_rpm();
             motorR.speed(tuneR);
             motorL.speed(-tuneL);
             motorDpn.brake(1);
@@ -544,7 +544,6 @@
         case (9) : 
         {
             // Mundur
-            //init_rpm();
             motorR.speed(-tuneR);
             motorL.speed(tuneL);
             motorDpn.brake(1);
@@ -576,9 +575,13 @@
         case (10) : 
         {
             // Pneumatik
-            pneumatik = 0;
-            previousMillis3 = millis();
-            flag_Pneu = true;
+            if (ready)
+            {
+                pneumatik = 0;
+                previousMillis3 = millis();
+                flag_Pneu = true;
+                ready = false;
+            }
             break;
         }
         case (11) : 
@@ -588,6 +591,22 @@
             isReload = false;
             break;
         }
+        case (31) : 
+        {
+            // start
+            target_rpm2 = 22;
+            target_rpm = 22;
+            init_rpm();
+            break;
+        }
+        case (32) : 
+        {
+            // select
+            target_rpm2 = 10;
+            target_rpm = 10;
+            init_rpm();
+            break;
+        }
         case (12) : 
         {
             // Power Screw Down
@@ -610,7 +629,7 @@
     if(ReloadOn){
         if(isReload){
             powerScrew.speed(pwmPowerDown);
-            if(!limitBawah){
+            if((!limitBawah)||(!limitBawah1)){
                 isReload = false;
                 ReloadOn = false;
             }
@@ -620,12 +639,15 @@
         }
         else if((jarak_ping > 6.5) && !flag_Pneu){
             powerScrew.speed(pwmPowerUp);
+            ready = false;
         }
-        else if((jarak_ping < 6 ) && !flag_Pneu) {
-            powerScrew.speed(-0.1);
+        else if((jarak_ping < 6.0) && !flag_Pneu) {
+            powerScrew.speed(-0.7);
+            ready = false;
         }
         else{
             powerScrew.brake(1);
+            ready = true;
         }
     }
     else{
@@ -646,10 +668,10 @@
         {
             rpm = (float)encLauncherBlk.getPulses();    
             current_error = target_rpm - rpm;
-            sum_error = sum_error + current_error;
+            sum_error = sum_error + current_error*12.5;
             p = current_error*kpA;
             d = (current_error-last_error)*kdA/12.5;
-            i = sum_error*kiA*12.5;
+            i = sum_error*kiA;
             speed = p + d + i;
             //init_speed();
             if(speed > maxSpeed){
@@ -670,10 +692,10 @@
         {
             rpm2 = (float)encLauncherDpn.getPulses();    
             current_error2 = target_rpm2 - rpm2;
-            sum_error2 = sum_error2 + current_error2;
+            sum_error2 = sum_error2 + current_error2*12.5;
             p2 = current_error2*kpA;
             d2 = (current_error2-last_error2)*kdA/12.5;
-            i2 = sum_error2*kiA*12.5;
+            i2 = sum_error2*kiA;
             speed2 = p2 + d2 + i2;
             //init_speed();
             if (speed2 > maxSpeed){
@@ -689,11 +711,18 @@
             encLauncherDpn.reset();
             previousMillis2 = currentMillis2;
         }
+        pc.printf("%.2f\t%.2f\n",rpm,rpm2);
     }
     else
     {
         launcherDpn.brake(1);
         launcherBlk.brake(1);
+        sum_error = 0;
+        sum_error2 = 0;
+        current_error = 0;
+        current_error2 = 0;
+        last_error = 0;
+        last_error2 = 0;
     }     
 }
   
@@ -740,7 +769,7 @@
             joystick.olah_data();
             // Masuk ke case joystick
             case_joy = case_joystick();
-            pc.printf("%d\n",case_joy);
+            //pc.printf("%d\n",case_joy);
             aktuator();
             launcher();
             reloader();
@@ -754,13 +783,8 @@
             joystick.idle();
         }
         
-        if(millis() - previousMillis5 >= 400){
-            //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);
-            
+        if(millis() - previousMillis5 >= 400)
+        {    
             display.write(0,((int) rpm2) / 10);
             display.write(1,((int)rpm2) % 10);
             display.write(2, (int)target_rpm2 / 10);