convert_KeilToMbed

Dependencies:   DigitDisplay Motor PID mbed millis

Fork of DagonFly__RoadToJapan_15Mei_Ultimate by KRAI 2017

Revision:
52:a39e26b935a9
Parent:
51:df6391c3fa68
--- a/main.cpp	Mon May 15 07:49:13 2017 +0000
+++ b/main.cpp	Sun Jul 02 01:37:31 2017 +0000
@@ -43,7 +43,6 @@
 #include "Motor.h"
 #include "encoderKRAI.h"
 #include "millis.h"
-#include "Ping.h"
 #include "DigitDisplay.h"
 
 /***********************************************/
@@ -51,242 +50,338 @@
 /***********************************************/
 #define PI 3.14159265
 #define D_ENCODER 10        // Diameter Roda Encoder
-#define D_ROBOT 80          // Diameter Roda Robot
+#define D_ROBOT1 54         // Diameter Roda Robot dari kiri ke kanan
+#define D_ROBOT2 79         // Diameter Roda Robot dari depan ke belakang
 
-// Variable Atas 
-// 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;
-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;
+/**********************************************************/
+/* LAUNCHER */
+bool isLauncher = false;
+double speed;
+const double maxSpeed = 1.0, minSpeed = -1.0, Ts = 20;
+const double kpA=0.0004667, kdA=0, kiA=6.2645e-07;
+double a,b,c;
+double current_error, previous_error_1 = 0, previous_error_2 = 0;
+double previous_speed = 0;
 
-float rpm, rpm2;
-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;
-
-double jarak_ping=0;
-double ping_target = 15;
-double ping_current_error, ping_previous_error1 = 0, ping_sum_error=0;
-double ping_Kp = -0.2747, ping_Kd = -0.535, ping_Ts=10;
-double ping_pwm, ping_previous_pwm = 0; 
+float rpm;
+double target_rpm = 2600;
+const float maxRPM = 4000, minRPM = 0.0;
+/**********************************************************/
 
-// Variable Bawah
-float PIVOT             = 0.17;      // PWM Pivot Kanan, Pivot Kiri
-float tuneDpn           = 1.0;     // Tunning PWM motor Depan
-float tuneBlk           = 1.0;     // Tunning PWM motor belakang
+/**********************************************************/
+/* BASE */
+float PIVOT             = 0.3;      // PWM Pivot Kanan, Pivot Kiri
 float tuneAksel         = 0.6;
-int aksel               = 0;
-float tuneAkselBlk      = 0.4;
-float tuneR             = 1.0;
-float tuneL             = 1.0;
-float serong            = 0.4;
-float rasio             = 1.4545;
-float t_new             = 0.1;
+float serong            = 0.45;
+float rasio= (D_ROBOT2/D_ROBOT1);
+//float jariR = 15, jariL = 64, jariDpn = 27, jariBlk = 27;
+float K_enc = PI*D_ENCODER;
+float K_robot1 = PI*D_ROBOT1;
+float K_robot2 = PI*D_ROBOT2;
+float speedDpn, speedBlk, speedR, speedL;
+//unsigned long int previousMillis=0;
+float jarakXnow, jarakXbefore, jarakYnow, jarakYbefore; //Untuk aksel
+/* PID BASE */
+float setpointX=0.0, setpointY=0.0, setpointT=0.0;
+const float maxPIDSpeedX = 0.6, minPIDSpeedX = -0.6;
+const float maxPIDSpeedY = 0.6, minPIDSpeedY = -0.6;
+const float maxPIDSpeedT = 0.4, minPIDSpeedT = -0.4;
+const float TsBase = 50;
+double current_efrror, previous_error1 = 0, previous_error2 = 0;
+float PIDSpeedX, PIDSpeedY, PIDSpeedT;
+float errorX, previous_errorX=0, derivativeX, integralX=0;
+float errorY, previous_errorY=0, derivativeY, integralY=0;
+float errorT, previous_errorT=0, derivativeT, integralT=0;
+float KpX=0.06, KiX=0, KdX=0.4;
+float KpY=0.06, KiY=0, KdY=0.4;
+float KpT=0.08, KiT=0, KdT=3.33;
+//float KpT=0.08, KiT=0, KdT=0.0;
+// Otomatis sb X
+bool autoX = false, auto_rotate = false;
+bool reset_encoder, inAutoRotate;
+int mode=0;
+/**********************************************************/
 
-/* variable tunning */
-float tunning_L;
-float tunning_R;
-float tunning_Dpn;
-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
-
-/* Variabel Stick */
-//Logic untuk masuk aktuator
-int case_joy;
-bool isLauncher = false;
+/**********************************************************/
+/* RELOADER */
 bool isReload = false;
 bool ReloadOn = false;
 bool flag_Pneu = false;
-bool flag_paku = false;
-bool ready = false;
-
-/*****************************************************/
-/*   Definisi Prosedur, Fungsi dan Setting Pinout    */
-/*****************************************************/
+bool readySlideFromLeft = false;
+bool readySlideFromMiddle = false;
+bool getBack = false;
+bool isDown = false, sliderOn = false;
+bool ready = true;
+bool init_slider = true;
+bool init_lifter = true;
+bool sliderReady = false;
+bool flag_tengah = true;
+bool delay = false;
+float lempar1 = -0.8, lempar2 = -0.85, balik = 0.7;
+float lempar_naik1 = -0.85, lempar2_naik = -0.8;
+float swipper_dorong1, swipper_dorong2;
+bool slideNowLeft = false, slideNowMid = false;
+bool naik = false;
+/**********************************************************/
 
-/* Fungsi dan Procedur Encoder */
-void init_speed();                  // 
-void aktuator();                    // Pergerakan aktuator berdasarkan case joystick
-int case_joystick();                // Mendapatkan case dari joystick
-//void speedKalibrasiMotor();       // Pertambahan target RPM motor atas melalui joystick
+/**********************************************************/
+/* MILLIS */
+static volatile uint32_t previousMillis = 0;
+static volatile uint32_t previousMillis3 = 0;  // Pneumatik
+static volatile uint32_t previousMillis5 = 0;  // Display
+static volatile uint32_t previousMillis6 = 0;  // pneu
+static volatile uint32_t previousMillis7 = 0;  // delay
+static volatile uint32_t currentMillis;
+static volatile uint32_t previousMillisAutoX=0; // Otomatis X
+/**********************************************************/
 
-/* Inisialisasi  Pin TX-RX Joystik dan PC */
-joysticknucleo joystick(PA_0,PA_1);
-Serial pc(USBTX,USBRX);
-
-/* Deklarasi Encoder Launcher */
-encoderKRAI encLauncherDpn( PC_10, PC_11, 28, encoderKRAI::X4_ENCODING);
-encoderKRAI encLauncherBlk( PC_12, PD_2, 28, encoderKRAI::X4_ENCODING);
+/**********************************************************/
+/* JOYSTICK */
+bool flag_LXmax = true, flag_LXmin = true, flag_LYmax = true, flag_LYmin = true;
+int case_joy;
+//int debug = 0;
+bool awal = true;
+/**********************************************************/
 
-/* Deklarasi Motor Base */
-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_6, PC_5);   //(PC_6, PB_4, PB_5);
+/**********************************************************/
+/* Motor */
+Motor motorDpn(PB_6, PA_10, PB_13);
+Motor motorBlk(PB_7, PA_8, PB_1);
+Motor motorL(PB_9, PA_11, PA_12);
+Motor motorR(PB_8, PA_9, PA_5);
+Motor Launcher(PA_6, PB_2, PB_12);
+Motor powerScrew(PA_7, PB_14, PB_15);
+Motor swipper(PB_0, PA_4, PA_13);
+/**********************************************************/
 
-/* Deklarasi Motor Launcher */
-Motor launcherDpn(PA_5,PA_11,PB_12);    // 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                
+/**********************************************************/
+/* Encoder */
+encoderKRAI Benc_R(PC_5, PC_4, 540, encoderKRAI::X4_ENCODING);
+encoderKRAI Benc_L(PC_7, PC_6, 540, encoderKRAI::X4_ENCODING);
+encoderKRAI Benc_Dpn(PC_1, PC_0, 540, encoderKRAI::X4_ENCODING);
+encoderKRAI Benc_Blk(PC_3, PC_2, 540, encoderKRAI::X4_ENCODING);
+encoderKRAI Lenc(PC_9, PC_8, 1000, encoderKRAI::X4_ENCODING);
+
+/**********************************************************/
 
-/* Deklarasi Penumatik Launcher */
-DigitalOut pneumatik(PA_4, PullUp);
-DigitalOut pneu_paku(PC_2, PullUp);
+/**********************************************************/
+/* Display 7 Segmen */
+DigitDisplay display (PC_12, PB_10);
+/**********************************************************/
 
-/*Dekalrasi Limit Switch */
-//DigitalIn infraAtas(PC_9, PullUp);
-DigitalIn limitTengah(PA_9, PullUp);
-DigitalIn limitBawah(PC_7, PullUp);
-DigitalIn limitBawah1(PA_7, PullUp);
+/**********************************************************/
+/* Limit Switch */
+DigitalIn limitKanan(PD_2, PullUp);
+DigitalIn limitTengah(PB_4, PullUp);
+DigitalIn limitKiri(PB_5, PullUp);
+DigitalIn limitBawah(PC_13, PullUp);
+DigitalIn limitAtas(PC_14, PullUp);
+DigitalIn limitFB(PC_15, PullUp);
+/**********************************************************/
 
-/*deklarasi PING ultrasonic*/
-Ping pingAtas(PC_15);
+/**********************************************************/
+/* Pneumatik */
+DigitalOut Pneumatik(PA_14, PullUp);
+DigitalOut Elevator(PA_15, PullUp);
+/****************************************************/
 
-/*Deklarasi Display*/
-DigitDisplay display (PA_8, PC_8);
+/**********************************************************/
+/* Serial */
+Serial pc(USBTX,USBRX);
+joysticknucleo joystick(PA_0,PA_1);
+/**********************************************************/
 
-/****************************************************/
 /*         Deklarasi Fungsi dan Procedure           */
 /****************************************************/
+
+void setCenter()
+ {
+    Benc_R.reset();
+    Benc_L.reset();
+    Benc_Dpn.reset();
+    Benc_Blk.reset();
+    Lenc.reset();    
+ }
+
+float getX()
+{
+    float  jarakEncDpn, jarakEncBlk;
+    jarakEncDpn = (Benc_Dpn.getPulses())/(float)(540.0)*K_enc;
+    jarakEncBlk = (Benc_Blk.getPulses())/(float)(540.0)*K_enc;
+    return (jarakEncDpn-jarakEncBlk)/2;
+}
+
+float getY()
+{
+    float  jarakEncKir, jarakEncKan;
+    jarakEncKir = (Benc_L.getPulses())/(float)(540.0)*K_enc;
+    jarakEncKan = (Benc_R.getPulses())/(float)(540.0)*K_enc;
+    return (jarakEncKir-jarakEncKan)/2;
+}
+
+float getTetha()
+{
+    float busurDpn, busurBlk, busurKir, busurKan, sudut;
+    busurKir = ((Benc_L.getPulses())/(float)(540.0)*K_enc)/K_robot2*360.0;
+    busurKan = ((Benc_R.getPulses())/(float)(540.0)*K_enc)/K_robot2*360.0;
+    busurDpn = ((Benc_Dpn.getPulses())/(float)(540.0)*K_enc)/K_robot1*360.0;
+    busurBlk = ((Benc_Blk.getPulses())/(float)(540.0)*K_enc)/K_robot1*360.0;
+    sudut = -(busurDpn+busurBlk+busurKir+busurKan)/4;
+    if (sudut>=360.0 || sudut<=-360.0)
+    { sudut = 0.0; }
+    return sudut; 
+}
+
+void PIDcompute_X()
+{
+    errorX = setpointX - getX();
+    integralX = integralX + errorX*TsBase;
+    derivativeX = (errorX - previous_errorX)/TsBase;
+    PIDSpeedX = KpX*errorY + KiX*integralY + KdX*derivativeX;
+    if(PIDSpeedX > maxPIDSpeedX)
+            { PIDSpeedX = maxPIDSpeedX; }
+    else if (PIDSpeedX < minPIDSpeedX)
+            { PIDSpeedX = minPIDSpeedX; }
+    previous_errorX = errorX;
+}
+
+void PIDcompute_Y()
+{
+    errorY = setpointY - getY();
+    integralY = integralY + errorY*TsBase;
+    derivativeY = (errorY - previous_errorY)/TsBase;
+    PIDSpeedY = KpY*errorY + KiY*integralY + KdY*derivativeY;
+    if(PIDSpeedY > maxPIDSpeedY)
+            { PIDSpeedY = maxPIDSpeedY; }
+    else if (PIDSpeedY < minPIDSpeedY)
+            { PIDSpeedY = minPIDSpeedY; }
+    previous_errorY = errorY;
+}
+
+void PIDcompute_T()
+{
+    errorT = setpointT - getTetha();
+    integralT = integralT + errorT*TsBase;
+    derivativeT = (errorT - previous_errorT)/TsBase;
+    PIDSpeedT = KpT*errorT + KiT*integralT + KdT*derivativeT;
+    if(PIDSpeedT > maxPIDSpeedT)
+            { PIDSpeedT = maxPIDSpeedT; }
+    else if (PIDSpeedT < minPIDSpeedT)
+            { PIDSpeedT = minPIDSpeedT; }
+    previous_errorT = errorT;
+}
+
 int case_joystick()
 {
-//---------------------------------------------------//
-//  Gerak Motor Base                                 //
-//   Case 1  : Pivot kanan                           //
-//   Case 2  : Pivot Kiri                            //
-//   Case 3  : Kanan                                 //
-//   Case 4  : Kiri                                  //
-//   Case 5  : Break                                 //
-//---------------------------------------------------//
-    
     int caseJoystick;
-    if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
+    if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {
         // Pivot Kanan
         caseJoystick = 1;
-    } 
-    else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
+    }
+    else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {
         // Pivot Kiri
         caseJoystick = 2;
     }
-    else if ((joystick.START_click)&&(!joystick.SELECT_click)&&(!joystick.R3_click)) {   
+    else if ((joystick.START_click)&&(!joystick.SELECT_click)&&(!joystick.R3_click)) {
         // tambah rpm dengan nilai tertentu
-        caseJoystick = 31;     
+        caseJoystick = 31;
     }
-    else if ((!joystick.START_click)&&(joystick.SELECT_click)&&(!joystick.R3_click)) {   
+    else if ((!joystick.START_click)&&(joystick.SELECT_click)&&(!joystick.R3_click)) {
         // kurangi rpm dengan nilai tertentu
-        caseJoystick = 32;     
+        caseJoystick = 32;
     }
-    else if ((!joystick.START_click)&&(!joystick.SELECT_click)&&(joystick.R3_click)) {   
+    else if ((!joystick.START_click)&&(!joystick.SELECT_click)&&(joystick.R3_click)) {
         // kurangi rpm dengan nilai tertentu
-        caseJoystick = 33;     
+        caseJoystick = 33;
     }
-    else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri))  {  
+    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))  {  
+    }
+    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))  {  
+    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))  {  
+    }
+    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))  {  
+    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))  {  
+    }
+    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))  {  
+    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))  {  
+    }
+    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))  {  
+    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))  {  
+    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))  {  
+    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))  {  
+    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))  {  
+    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))  {  
+    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)) {   
+    else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
         // Serong kanan maju
-        caseJoystick = 13;     
+        caseJoystick = 13;
     }
-    else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {   
+    else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
         // Serong kiri maju
-        caseJoystick = 14;      
+        caseJoystick = 14;
     }
-    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {   
+    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
         // Serong kanan mundur
-        caseJoystick = 15;       
+        caseJoystick = 15; 
     }
-    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {   
+    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
         // Serong kiri mundur
-        caseJoystick = 16;       
-    } 
-    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri))  {   
+        caseJoystick = 16; 
+    }
+    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri))  {
         // Kanan
         caseJoystick = 3;
-    } 
-    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {   
+    }
+    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
         // Kiri
         caseJoystick = 4;
     }
-    else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {   
+    else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
         // Atas -- Maju
-        caseJoystick = 8;       
+        caseJoystick = 8; 
     }
-    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {   
+    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
         // Bawah -- Mundur
-        caseJoystick = 9;       
-    } 
+        caseJoystick = 9; 
+    }
     else if (joystick.segitiga_click){
         // Motor Launcher
         caseJoystick = 5;
@@ -294,495 +389,631 @@
     else if (joystick.R2_click){
         // Target Pulse PID ++ Motor Depan
          caseJoystick = 6;
-    }  
+    }
     else if (joystick.L2_click){
-        // Target Pulse PID -- Motor 
+        // Target Pulse PID -- Motor
          caseJoystick = 7;
     }
     else if (joystick.silang_click){
         // Pnemuatik ON
         caseJoystick = 10;
     }
-    else if ((joystick.lingkaran_click)&&(!joystick.kotak_click))  {   
+    else if ((joystick.lingkaran_click)&&(!joystick.kotak_click))  {
         // Power Screw Up
         caseJoystick = 11;
-    } 
-    else if ((joystick.kotak_click)&&(!joystick.lingkaran_click)) {   
+    }
+    else if ((joystick.kotak_click)&&(!joystick.lingkaran_click)) {
         // Power Screw Down
-        caseJoystick = 12;       
-    } 
-    else if (joystick.LYp_click) {
-        // Paku Bumi Naik
+        caseJoystick = 12; 
+    }
+    else if (joystick.L3_click){
+        // elevasi ON/OFF
         caseJoystick = 34;
     }
-    else if (joystick.LYn_click) {
-        // Paku Bumi Turun
-        caseJoystick = 35;
-    }
     else
     {
-        tuneAksel = 0.6;
-        aksel = 0;
+        //tuneAksel = 0.6;    
     }
-    
     return(caseJoystick);
 }
 
+void aturSetpoint()
+{
+    switch (mode)
+    {
+        case (-2):       
+            setpointT = -45.0;
+            break;
+        case (-1):
+            setpointT = -20.0;
+            break;
+        case (0):       
+            setpointT = 0.0;
+            break;
+        case (1):
+            setpointT = 20.0;
+            break;
+        case (2):
+            setpointT = 45.0;
+            break;
+    }
+}
+void cekMode()
+{
+    if (mode<-2){mode = -2;}
+    else if (mode>2){mode = 2;}  
+}
 
-void init_rpm (){
-    if (target_rpm>maxRPM-2){
-        target_rpm = maxRPM-2;
+void case_autoRotate()
+{
+    if ((joystick.LY>175)&&flag_LYmax) // Bawah, Keluar
+    {
+        flag_LYmax = false;
+        auto_rotate = false;
+        setpointT = 0.0;
+        inAutoRotate = false;
+        mode = 0;
+    }
+    else if ((15<joystick.LY&&joystick.LY<175)&&!flag_LYmax){ flag_LYmax = true; }
+   
+    if ((joystick.LY<15)&&flag_LYmin) // Atas, Masuk (aktif)
+    {
+        flag_LYmin = false;
+        auto_rotate = true;
+        reset_encoder = true;
+        inAutoRotate =true;
+        setCenter();
+        mode = 0;
+    }
+    else if ((15<joystick.LY && joystick.LY<175)&&!flag_LYmin){ flag_LYmin = true;}
+        
+    if ((joystick.LX>200)&&flag_LXmax) 
+    { 
+        flag_LXmax = false;
+        mode--;
+        cekMode();
+    }
+    else if ((55<joystick.LX&&joystick.LX<200)&&!flag_LXmax){ flag_LXmax = true; }
+   
+    if ((joystick.LX<55)&&flag_LXmin)
+    { 
+        flag_LXmin = false;
+        mode++;
+        cekMode();
+    }
+    else if ((55<joystick.LX&&joystick.LX<200)&&!flag_LXmin){ flag_LXmin = true; }
+}
+
+void init_rpm ()
+{
+    if (target_rpm>maxRPM){
+        target_rpm = maxRPM;
     }
     if (target_rpm<minRPM){
         target_rpm = minRPM;
     }
-    if (target_rpm2>maxRPM){
-        target_rpm2 = maxRPM;
+}
+
+void sliderMove()
+{
+    if (readySlideFromLeft && slideNowLeft && !delay)
+    {
+        swipper.speed(swipper_dorong2);
+        if(!(limitTengah) && flag_tengah)
+        {
+            swipper.brake(1);
+            slideNowLeft = false;
+            slideNowMid = false;
+            readySlideFromLeft = false;
+            readySlideFromMiddle = true;
+            getBack = false;
+            flag_tengah = false;
+        }
+        else {flag_tengah = true;}
+    }
+    else if (readySlideFromMiddle && slideNowMid && !delay)
+    {
+        swipper.speed(swipper_dorong1);
+        if(!(limitKanan))
+        {
+            readySlideFromMiddle = false;
+            slideNowLeft = false;
+            slideNowMid = false;
+            readySlideFromLeft = false;
+            getBack = true;
+        }
     }
-    if (target_rpm2<minRPM+2){
-        target_rpm2 = minRPM+2;
+    else if (getBack)
+    {
+        swipper.speed(balik);
+        if(!(limitKiri))
+        {
+            swipper.brake(1);
+            slideNowLeft = false;
+            slideNowMid = false;
+            readySlideFromLeft = false;
+            readySlideFromMiddle = false;
+            getBack = false;
+            sliderReady = false;
+            ReloadOn = true;
+            isDown = false;
+        }
+    }
+    else
+    {
+        swipper.brake(1);
+    }
+}
+
+void lifterMove()
+{
+    if(ReloadOn)
+    {
+        if(isDown)
+        {
+            powerScrew.speed(-1.0);
+            if(!(limitBawah))
+            {
+                ReloadOn = false;
+                isDown = false;
+            }
+        }
+        else if (!(limitAtas))
+        {
+            isDown = true;
+            readySlideFromMiddle = false;
+            getBack = true;
+        }
+        else if (sliderReady)
+        {
+            powerScrew.brake(1);
+            sliderMove();
+        }
+        else if(!(limitFB))
+        {
+            sliderReady = true;
+            readySlideFromLeft = true;
+        }
+        else
+        {
+            powerScrew.speed(1.0);
+        }
+    }
+    else
+    {
+        powerScrew.brake(1);
     }
 }
 
 void aktuator()
 {
     switch (case_joy) {
-        case (1): 
-        {       
+        case (1):
+        {
             // Pivot Kanan
             motorDpn.speed(-PIVOT);
             motorBlk.speed(-PIVOT);
-            motorR.speed(-rasio*PIVOT-t_new);
-            motorL.speed(-rasio*PIVOT-t_new);
+            motorR.speed(-rasio*PIVOT);
+            motorL.speed(-rasio*PIVOT);
             break;
         }
-        case (2): 
+        case (2):
         {
             // Pivot Kiri
             motorDpn.speed(PIVOT);
             motorBlk.speed(PIVOT);
-            motorR.speed(rasio*PIVOT+t_new);
-            motorL.speed(rasio*PIVOT+t_new);
+            motorR.speed(rasio*PIVOT);
+            motorL.speed(rasio*PIVOT);
             break;
         }
-        case (17): 
-        {       
+        case (17):
+        {
             // Kanan + Rotasi Kanan
             motorDpn.speed(-PIVOT);
             motorBlk.speed(-PIVOT);
-            motorR.speed(-rasio*PIVOT-t_new);
-            motorL.speed(-rasio*PIVOT-t_new);
+            motorR.speed(-rasio*PIVOT);
+            motorL.speed(-rasio*PIVOT);
             break;
         }
-        case (18): 
+        case (18):
         {
             // Kanan + Rotasi Kiri
             motorDpn.speed(PIVOT);
             motorBlk.speed(PIVOT);
-            motorR.speed(rasio*PIVOT+t_new);
-            motorL.speed(rasio*PIVOT+t_new);
+            motorR.speed(rasio*PIVOT);
+            motorL.speed(rasio*PIVOT);
             break;
         }
-        case (19): 
-        {       
+        case (19):
+        { 
             // Kiri + Rotasi Kanan
             motorDpn.speed(-PIVOT);
             motorBlk.speed(-PIVOT);
-            motorR.speed(-rasio*PIVOT-t_new);
-            motorL.speed(-rasio*PIVOT-t_new);
+            motorR.speed(-rasio*PIVOT);
+            motorL.speed(-rasio*PIVOT);
             break;
         }
-        case (20): 
+        case (20):
         {
             // Kiri + Rotasi Kiri
             motorDpn.speed(PIVOT);
             motorBlk.speed(PIVOT);
-            motorR.speed(rasio*PIVOT+t_new);
-            motorL.speed(rasio*PIVOT+t_new);
+            motorR.speed(rasio*PIVOT);
+            motorL.speed(rasio*PIVOT);
             break;
         }
-        case (21): 
-        {       
+        case (21):
+        { 
             // Maju + Rotasi Kanan
             motorDpn.speed(-PIVOT);
             motorBlk.speed(-PIVOT);
-            motorR.speed(-rasio*PIVOT-t_new);
-            motorL.speed(-rasio*PIVOT-t_new);
+            motorR.speed(-rasio*PIVOT);
+            motorL.speed(-rasio*PIVOT);
             break;
         }
-        case (22): 
+        case (22):
         {
             // Maju + Rotasi Kiri
             motorDpn.speed(PIVOT);
             motorBlk.speed(PIVOT);
-            motorR.speed(rasio*PIVOT+t_new);
-            motorL.speed(rasio*PIVOT+t_new);
+            motorR.speed(rasio*PIVOT);
+            motorL.speed(rasio*PIVOT);
             break;
         }
-        case (23): 
-        {       
+        case (23):
+        { 
             // Mundur + Rotasi Kanan
             motorDpn.speed(-PIVOT);
             motorBlk.speed(-PIVOT);
-            motorR.speed(-rasio*PIVOT-t_new);
-            motorL.speed(-rasio*PIVOT-t_new);
+            motorR.speed(-rasio*PIVOT);
+            motorL.speed(-rasio*PIVOT);
             break;
         }
-        case (24): 
+        case (24):
         {
             // Mundur + Rotasi Kiri
             motorDpn.speed(PIVOT);
             motorBlk.speed(PIVOT);
-            motorR.speed(rasio*PIVOT+t_new);
-            motorL.speed(rasio*PIVOT+t_new);
+            motorR.speed(rasio*PIVOT);
+            motorL.speed(rasio*PIVOT);
             break;
         }
-        case (25): 
+        case (25):
         {
             // Kanan + segitiga
             isLauncher = !isLauncher;
             break;
         }
-        case (26): 
+        case (26):
         {
             // Kiri + segitiga
             isLauncher = !isLauncher;
             break;
         }
-        case (27): 
+        case (27):
         {
-            // Kanan + lingkaran
-            ReloadOn = !ReloadOn;
-            isReload = false;
+            // Kanan + lingkaran ----- Lifter Up
+                        ReloadOn = !ReloadOn;
+                        isDown = false;
             break;
         }
-        case (28): 
+        case (28):
         {
-            // Kiri + lingkaran
-            ReloadOn = !ReloadOn;
-            isReload = false;
+                        // Kiri + lingkaran ----- Lifter Up
+                        ReloadOn = !ReloadOn;
+                        isDown = false;
             break;
         }
-        case (29): 
+        case (29):
         {
-            // Kanan + kotak
-            ReloadOn = !ReloadOn;
-            isReload = true;
+                        // Kanan + kotak ----- Lifter Down
+                        ReloadOn = !ReloadOn;
+                        isDown = true;
             break;
         }
-        case (30): 
+        case (30):
         {
-            // Kiri + kotak
-            ReloadOn = !ReloadOn;
-            isReload = true;
+            // Kiri + kotak ----- Lifter Down
+                        ReloadOn = !ReloadOn;
+                        isDown = true;
             break;
         }
-        case (13) : 
+        case (13) :
         {
             // Serong kanan maju
+            awal = true;
             motorDpn.speed(-serong);
-            motorL.speed(-serong-t_new);
             motorBlk.speed(serong);
-            motorR.speed(serong+t_new);
+            motorR.speed(serong);
+            motorL.speed(-serong);
             break;
         }
-        case (14) : 
+        case (14) :
         {
             // Serong kiri maju
+            awal = true;
             motorDpn.speed(serong);
-            motorR.speed(serong+t_new);
             motorBlk.speed(-serong);
-            motorL.speed(-serong-t_new);
+            motorR.speed(serong);
+            motorL.speed(-serong);
             break;
         }
-        case (15) : 
+        case (15) :
         {
-            // Serong kanan mundur 
+            // Serong kanan mundur
+            awal = true;
             motorDpn.speed(-serong);
-            motorR.speed(-serong-t_new);
             motorBlk.speed(serong);
-            motorL.speed(serong+t_new);
+            motorR.speed(-serong);
+            motorL.speed(serong);
             break;
         }
-        case (16) : 
+        case (16) :
         {
             // Serong kiri mundur
+            awal = true;
             motorDpn.speed(serong);
-            motorL.speed(serong+t_new);
             motorBlk.speed(-serong);
-            motorR.speed(-serong-t_new);
+            motorR.speed(-serong);
+            motorL.speed(serong);
             break;
         }
-        case (3) : 
+        case (3) :
         {
             // Kanan
-            aksel++;
-            if (aksel>300)
-                tuneAksel = 0.9;
+            if (awal)
+            {
+                tuneAksel = 0.6;
+                awal = false;
+            }
+            jarakXnow = getX();
+            if(jarakXnow-jarakXbefore>5.0)
+            {
+                tuneAksel = tuneAksel+0.05;
+                jarakXbefore = jarakXnow;
+            }
+            if (tuneAksel>1.0) tuneAksel = 1.0;
             
             motorDpn.speed(-tuneAksel);
             motorBlk.speed(tuneAksel);
             motorR.brake(1);
             motorL.brake(1);
             break;
-        }
-        case (4) : 
+                }
+        case (4) :
         {
             // Kiri
-            aksel++;
-            if (aksel>300)
-                tuneAksel = 0.9;
-                
+            if (awal)
+            {
+                tuneAksel = 0.6;
+                awal = false;
+            }
+            jarakXnow = getX();
+            if(jarakXnow-jarakXbefore<-5.0)
+            {
+                tuneAksel = tuneAksel+0.05;
+                jarakXbefore = jarakXnow;
+            }
+            if (tuneAksel>1.0) tuneAksel = 1.0;
+          
             motorDpn.speed(tuneAksel);
             motorBlk.speed(-tuneAksel);
             motorR.brake(1);
             motorL.brake(1);
             break;
-        }
-        case (8) : 
+                }
+        case (8) :
         {
             // Maju
-            aksel++;
-            if (aksel>300)
-                tuneAksel = 0.9;
-            
+            if (awal)
+            {
+                tuneAksel = 0.525;
+                awal = false;
+            }
+            jarakYnow = getY();
+            if(jarakYnow-jarakYbefore>5.0)
+            {
+                tuneAksel = tuneAksel+0.025;
+                jarakYbefore = jarakYnow;
+            }
+            if (tuneAksel>0.675) tuneAksel = 0.675;
+      
+            motorDpn.brake(1);
+            motorBlk.brake(1);
             motorR.speed(tuneAksel);
             motorL.speed(-tuneAksel);
-            motorDpn.brake(1);
-            motorBlk.brake(1);
             break;
         }
-        case (9) : 
+        case (9) :
         {
             // Mundur
-            aksel++;
-            if (aksel>300)
-                tuneAksel = 0.9;
-            
+            if (awal)
+            {
+                tuneAksel = 0.525;
+                awal = false;
+            }
+            jarakYnow = getY();
+            if(jarakYnow-jarakYbefore<-5.0)
+            {
+                tuneAksel = tuneAksel+0.025;
+                jarakYbefore = jarakYnow;
+            }
+            if (tuneAksel>0.675) tuneAksel = 0.675;
+      
+            motorDpn.brake(1);
+            motorBlk.brake(1);
             motorR.speed(-tuneAksel);
             motorL.speed(tuneAksel);
-            motorDpn.brake(1);
-            motorBlk.brake(1);
             break;
         }
-        case (5) : 
+        case (5) :
         {
             // Aktifkan motor atas
             isLauncher = !isLauncher;
             break;
         }
-        case (6) : 
+        case (6) :
         {
-            // Target Pulse PID ++ Motor Depan
-            target_rpm2 = target_rpm2+1.0;
-            target_rpm = target_rpm+1.0;
+            // Target Pulse PID ++
+            target_rpm = target_rpm+100.0;
             init_rpm();
             break;
         }
-        case (7) : 
+        case (7) :
         {
-            // Target Pulse PID -- Motor Depan
-            target_rpm2 = target_rpm2-1.0;
-            target_rpm = target_rpm-1.0;
+            // Target Pulse PID --
+            target_rpm = target_rpm-100.0;
             init_rpm();
             break;
         }
-        case (10) : 
+        case (10) :
         {
-            // Pneumatik
+        // Pneumatik pendorong
             if (ready)
             {
-                pneumatik = 0;
+                Pneumatik = 0;
                 previousMillis3 = millis();
                 flag_Pneu = true;
                 ready = false;
-                //ReloadOn = !ReloadOn;
-                //previousMillis6 = millis();
-                
+                previousMillis6 = millis();
             }
             break;
         }
-        case (11) : 
+        case (11) :
         {
-            // Power Screw Up
+            // Lifter Up
             ReloadOn = !ReloadOn;
-            isReload = false;
+            isDown = false;
             break;
         }
-        case (31) : 
+        case (31) :
         {
             // start
-            target_rpm2 = 24;
-            target_rpm = 24;
+            Elevator = 1;
+            target_rpm = 3200;
             init_rpm();
             break;
         }
-        case (32) : 
+        case (32) :
         {
             // select
-            target_rpm2 = 11;
-            target_rpm = 11;
+            target_rpm = 1600;
             init_rpm();
             break;
         }
-        case (33) : 
+        case (33) :
         {
             // R3
-            target_rpm2 = 17;
-            target_rpm = 17;
+            target_rpm = 2600;
             init_rpm();
             break;
         }
-        case (12) : 
+        case (12) :
         {
-            // Power Screw Down
+            // Lifter Down
             ReloadOn = !ReloadOn;
-            isReload = true;
+            if (readySlideFromLeft)
+            {
+                sliderReady = false;
+                readySlideFromLeft = false;
+            }
+            isDown = true;
             break;
         }
         case (34) :
         {
-            pneu_paku = 1;
-            // Paku Bumi Naik
-            wait_ms(50);
-            PIVOT = 0.17;
+            // Pneumatik naikkan sudut
+            naik = !naik;
+            Elevator = 0;
+            break;
         }
-        case (35) :
+        default :
         {
-            // Paku Bumi Turun
-            pneu_paku = 0;
-            wait_ms(50);
-            PIVOT = 0.8;
+            if (tuneAksel>0.4)
+            {
+                if (getX()>0.0)
+                {
+                    jarakXnow = getX();
+                    if(jarakXnow-jarakXbefore>4.5)
+                    {
+                        tuneAksel = tuneAksel-0.05;
+                        jarakXbefore = jarakXnow;
+                    }
+                }
+                if (getX()<0.0)
+                {
+                    jarakXnow = getX();
+                    if(jarakXnow-jarakXbefore<-4.5)
+                    {
+                        tuneAksel = tuneAksel-0.05;
+                        jarakXbefore = jarakXnow;
+                    }
+                }
+            }
+            else
+            {
+                motorDpn.brake(1);
+                motorBlk.brake(1);
+                motorR.brake(1);
+                motorL.brake(1);
+                setCenter();
+                jarakXbefore = 0;
+                jarakYbefore = 0;
+            }
+            if(PIVOT>0.2 || serong>0.2)
+            {
+                motorDpn.brake(1);
+                motorBlk.brake(1);
+                motorR.brake(1);
+                motorL.brake(1);
+            }
+            awal = true;
         }
-        default : 
-        {
-            tuneAksel = 0.6;
-            aksel = 0;
-            motorDpn.brake(1);
-            motorBlk.brake(1);
-            motorR.brake(1);
-            motorL.brake(1);
-        }   
     } // End Switch
  }
  
-void reloader()
-{
-    if(ReloadOn){
-        if(isReload){
-            powerScrew.speed(pwmPowerDown);
-            pc.printf("%.2f\n", jarak_ping);
-            if((!limitBawah)||(!limitBawah1)){
-                isReload = false;
-                ReloadOn = false;
-            }
-        }
-        else if(!limitTengah){
-            isReload = true;
-        }
-        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;   
-            //ping_sum_error = ping_sum_error+ping_current_error;
-            
-            pc.printf("%.2f\n", jarak_ping);
-            powerScrew.speed(ping_pwm);
-            
-            ping_previous_error1 = ping_current_error;
-        }
-        if ((jarak_ping>(ping_target-2))&&(jarak_ping<(ping_target+2))){
-            ready = true;
-        }else{
-            ready = false;
-        }
-    }
-    else{
-        powerScrew.brake(1);
-    }
-}
- 
- 
-void launcher()
+ void launcher()
 {
     if (isLauncher)
     {
         currentMillis  = millis();
-        currentMillis2 = millis();
-        
+  
         // PID Launcher Belakang
         if (currentMillis-previousMillis>=Ts)
         {
-            rpm = (float)encLauncherBlk.getPulses();    
-            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();
+            rpm = (float)Lenc.getPulses();;
+            current_error = target_rpm - rpm;
+            a = kpA + kiA*Ts/2 + kdA/Ts;
+            b = -kpA + kiA*Ts/2 - 2*kdA/Ts;
+            c = kdA/Ts;
+            speed = previous_speed + a*current_error + b*previous_error_1 + c*previous_error_2;
             if(speed > maxSpeed){
-                launcherBlk.speed(maxSpeed);
+               Launcher.speed(maxSpeed);
             }
             else if ( speed < minSpeed){
-                launcherBlk.speed(minSpeed);
+                Launcher.speed(minSpeed);
             }
             else {
-                launcherBlk.speed(speed);
+                Launcher.speed(speed);
             }
-            previous_speed1 = speed;
-            previous_error1_2 = previous_error1_1;
-            previous_error1_1 = current_error1;
-            encLauncherBlk.reset();
+            previous_speed = speed;
+            previous_error_2 = previous_error_1;
+            previous_error_1 = current_error;
             previousMillis = currentMillis;
-            
+            Lenc.reset();  
         }
-        // PID Launcher Depan
-        if (currentMillis2-previousMillis2>=Ts)
-        {
-            rpm2 = (float)encLauncherDpn.getPulses();    
-            current_error2 = target_rpm2 - rpm2;
-            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 ( speed2 < minSpeed){
-                launcherDpn.speed(minSpeed);
-            }
-            else{
-                launcherDpn.speed(speed2);
-            }
-            previous_speed2 = speed2;
-            previous_error2_2 = previous_error2_1;
-            previous_error2_1 = current_error2;
-            encLauncherDpn.reset();
-            previousMillis2 = currentMillis2;
-        }
-        //pc.printf("%.2f\t%.2f\n",rpm,rpm2);
     }
     else
-    {
-        launcherDpn.brake(1);
-        launcherBlk.brake(1);
-        
-        previous_error1_1 = 0;
-        previous_error1_2 = 0;
-        previous_error2_1 = 0;
-        previous_error2_2 = 0;
-        previous_speed1 = 0;
-        previous_speed2 = 0;
-    }     
+    {  
+        Launcher.brake(1);
+        current_error = 0;
+        previous_error_1 = 0;
+        previous_error_2 = 0;
+        previous_speed = 0;
+    }
 }
-  
-/*********************************************************/
-/*                  Main Function                        */
-/*********************************************************/
 
+// Main Program
 int main (void)
 {
     // Set baud rate - 115200
@@ -790,61 +1021,86 @@
     pc.baud(115200);
     wait_ms(1000);
     
-    // initializing encoder
-    pneumatik =1;
-    
+    // initializing pneumatik
+    Pneumatik = 1;
+    Elevator = 1;
     wait_ms(500);
     
-    //initializing PING
-    pingAtas.Send();
-    
-    pc.printf("ready....");
+   while(init_slider)
+    {
+        swipper.speed(balik);
+        if (!(limitKiri))
+        {
+            init_slider = false;
+            swipper.brake(1);
+        }
+    }
+    while(init_lifter)
+    {
+        powerScrew.speed(-1.0);
+        if(!(limitBawah))
+        {
+            init_lifter = false;
+            powerScrew.brake(1);
+        }
+    }
+ 
     startMillis();
-    while(1)
-    {   
-        // interupsi pembacaan PING setiap 30 ms
-        if(millis() - previousMillis4 >= 10){    //30
-            jarak_ping = (float)pingAtas.Read_cm();
-            
-            pingAtas.Send();
-            previousMillis4 = millis();
-        }
-        
-        // Interrupt Serial
-        joystick.idle();        
+    /* USER CODE END 2 */
+
+    /* Infinite loop */
+    /* USER CODE BEGIN WHILE */
+    //idle();
+    setCenter();
+    joystick.idle(); 
+    while (1)
+    {
         if(joystick.readable()) 
         {
-            // Panggil fungsi pembacaan joystik
             joystick.baca_data();
-            // Panggil fungsi pengolahan data joystik
-            joystick.olah_data();
-            // Masuk ke case joystick
+            joystick.olah_data();  
             case_joy = case_joystick();
-            //pc.printf("%d\n",case_joy);
+            if (naik)
+            {     
+                swipper_dorong1 = lempar_naik1;
+                swipper_dorong2 = lempar2_naik;
+            }
+            else
+            {
+                swipper_dorong1 = lempar1;
+                swipper_dorong2 = lempar2;
+            }
             aktuator();
             launcher();
-            reloader();
-            if ((millis()-previousMillis3 >= 230)&&(flag_Pneu)){
-                pneumatik = 1;
+            lifterMove();
+            
+            if ((millis()-previousMillis3 >= 500)&&(flag_Pneu))
+            {
+                Pneumatik = 1;
                 flag_Pneu = false;
-                //if (millis()-previousMillis6 >= 100){
-                //    ReloadOn = !ReloadOn;
-                //}
+                slideNowLeft = true;
+                slideNowMid = true;
+                ready = true;
+                delay = true;
+                previousMillis7 = millis(); //delay
+            }         
+            if((millis()-previousMillis7>=200) && delay)
+            {
+                delay = false;
             }
         }
         else
         {
             joystick.idle();
         }
-        
         if(millis() - previousMillis5 >= 400)
-        {    
-            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.write(0,((int)rpm) / 1000);
+            display.write(1,((((int)rpm)%1000)/ 100));
+            display.write(2,((int)target_rpm) / 1000);
+            display.write(3,((((int)target_rpm)%1000)/ 100));
             display.setColon(true);
-            
+
             previousMillis5 = millis();
         }
     }