Revision 0:dddc43348c25, committed 2019-02-24

Comitter:

SalbiFaza

Date:

Sun Feb 24 09:47:02 2019 +0000

Commit message:

bismillah

Changed in this revision

diff -r 000000000000 -r dddc43348c25 JoystickPS3.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/JoystickPS3.h	Sun Feb 24 09:47:02 2019 +0000
@@ -0,0 +1,214 @@
+#ifndef MBED_H
+#include "mbed.h"
+#endif
+ 
+#ifndef JoystickPS3__serialDEFAULT_BAUD
+#define JoystickPS3__serialDEFAULT_BAUD       115200
+#endif
+ 
+//Serial debug(USBTX,USBRX);
+ 
+namespace JoystickPS3 {
+ 
+class joysticknucleo {
+public:
+    joysticknucleo(PinName tx, PinName rx) : _serial(tx, rx)
+    {
+        
+    }
+    
+// Deklarasi variabel tombol analog
+    unsigned char LX, LY, RX, RY, R2, L2;
+    
+    unsigned char button;
+    unsigned char RL;
+    unsigned char button_click;
+    unsigned char RL_click;
+    
+    void setup(){
+        _serial.baud(JoystickPS3__serialDEFAULT_BAUD);
+  //      debug.baud(9600);
+        }
+ 
+    /*********************************************************************************************/
+    /**                                                                                         **/
+    /** FUNGSI PEMBACAAN DATA                                                                   **/
+    /** -   Data yang diterima dari Serial Arduino berbentuk 8-bit                              **/
+    /** -   Data yang diterima diolah menjadi boolean / 1-bit untuk data tombol button dan RL   **/
+    /**     karena data yang digunakan adalah 1-bit (true/false)                                **/
+    /** -   Untuk analog data yang diterima tidak diolah karena rentang data yang dikirimkan    **/
+    /**     memiliki rentang 0-255 / 8-bit, dan data yang akan digunakan adalah data 8-bit      **/
+    /**                                                                                         **/
+    /**         |------|-------|-------|------|-------|--------|-----------|----------|         **/
+    /** 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 olah_data()
+    {
+        // Pengolahan data dari data "button" 
+        segitiga = (bool)((button >> 0) & 0x1);
+        lingkaran = (bool)((button >> 1) & 0x1);
+        silang = (bool)((button >> 2) & 0x1);
+        kotak = (bool)((button >> 3) & 0x1);
+        atas = (bool)((button >> 4) & 0x1);
+        kanan = (bool)((button >> 5) & 0x1);
+        bawah = (bool)((button >> 6) & 0x1);
+        kiri = (bool)((button >> 7) & 0x1);
+        
+        // Pengolahan data dari data "RL" 
+        R1 = (bool)((RL >> 0) & 0x1);
+        R3 = (bool)((RL >> 1) & 0x1);
+        L1 = (bool)((RL >> 2) & 0x1);
+        L3 = (bool)((RL >> 3) & 0x1);
+        START = (bool)((RL >> 4) & 0x1);
+        SELECT = (bool)((RL >> 5) & 0x1);
+        PS = (bool)((RL >> 6) & 0x1);
+    
+        // R2 click dan L2 click
+        if (R2 > 100) {
+            if ( R2sebelum) { R2_click = false;
+                } else { R2_click = true;}   
+            R2sebelum = true; 
+        }else { 
+            R2sebelum = false;
+            R2_click = false;
+        }            
+        if (L2 > 100) {
+            if ( L2sebelum) { L2_click = false;
+                } else { L2_click = true;}   
+            L2sebelum = true; 
+        }else { L2sebelum = false;
+                L2_click = false;
+            }            
+    
+        segitiga_click = (bool)((button_click >> 0) & 0x1);
+        lingkaran_click = (bool)((button_click >> 1) & 0x1);
+        silang_click = (bool)((button_click >> 2) & 0x1);
+        kotak_click = (bool)((button_click >> 3) & 0x1);
+        atas_click = (bool)((button_click >> 4) & 0x1);
+        kanan_click = (bool)((button_click >> 5) & 0x1);
+        bawah_click = (bool)((button_click >> 6) & 0x1);
+        kiri_click = (bool)((button_click >> 7) & 0x1);
+        
+        // Pengolahan data dari data "RL" 
+        R1_click = (bool)((RL_click >> 0) & 0x1);
+        R3_click = (bool)((RL_click >> 1) & 0x1);
+        L1_click = (bool)((RL_click >> 2) & 0x1);
+        L3_click = (bool)((RL_click >> 3) & 0x1);
+        START_click = (bool)((RL_click >> 4) & 0x1);
+        SELECT_click = (bool)((RL_click >> 5) & 0x1);
+        PS_click = (bool)((RL_click >> 6) & 0x1);
+    }
+    
+    /*********************************************************************************************/
+    /**                                                                                         **/
+    /** FUNGSI IDLE                                                                             **/
+    /** -   Fungsi dijalankan saat Arduino mengirimkan data yang merupakan                      **/
+    /**     kondisi PS3 Disconnected                                                            **/
+    /** -   Fungsi membuat semua data joystik bernilai 0                                        **/
+    /**                                                                                         **/
+    /*********************************************************************************************/
+    
+    void idle(){
+        // Set 0    
+        button = 0;
+        RL = 0;
+        button_click = 0;
+        RL_click = 0;
+        R2_click =0;
+        L2_click =0;
+        R2 = 0;
+        L2 = 0;
+        RX = 0;
+        RY = 0;
+        LX = 0;
+        LY = 0;
+    
+    }
+    
+    /*********************************************************************************************/
+    /**                                                                                         **/
+    /** FUNGSI PEMBACAAN DATA                                                                   **/
+    /** -   Fungsi pembacaan data yang dikirim dari arduino                                     **/
+    /** -   Data yang dikirim dari arduino merupakan paket data dengan format pengiriman        **/
+    /**                                                                                         **/
+    /** |------|------|--------|----|--------------|----------|----|----|----|----|----|----|   **/
+    /** | 0x88 | 0x08 | button | RL | button_click | RL_click | R2 | L2 | RX | RY | LX | LY |   **/
+    /** |------|------|--------|----|--------------|----------|----|----|----|----|----|----|   **/
+    /**                                                                                         **/
+    /** |------|------|                                                                         **/
+    /** | 0x88 | 0x09 |                                                                         **/
+    /** |------|------|                                                                         **/
+    /**                                                                                         **/
+    /** -   Jika urutan data yang diterima seperti tabel diatas, maka data tersebut akan        **/
+    /**     diolah untuk input ke aktuator                                                      **/
+    /**                                                                                         **/
+    /*********************************************************************************************/
+    
+    void baca_data()
+    {
+        // Interrupt Serial
+        if(_serial.readable()&&(_serial.getc()==0x88)) {
+            // Pembacaan data dilakukan jika data awal yang diterima adalah 0x88 kemudian 0x08
+            if(_serial.getc()==0x08){
+                // Proses Pembacaan Data
+                button = _serial.getc();
+                RL = _serial.getc();
+                button_click = _serial.getc();
+                RL_click = _serial.getc();
+                R2 = _serial.getc();
+                L2 = _serial.getc();
+                RX = _serial.getc();
+                RY = _serial.getc();
+                LX = _serial.getc();
+                LY = _serial.getc();
+            } else if(_serial.getc()==0x09) {
+                // PS3 Disconnected
+                idle();
+            } else {
+                idle(); }
+            // Indikator - Print data pada monitor PC
+        // debug.printf("%2x %2x %2x %2x %3d %3d %3d %3d %3d %3d\n\r",button, RL, button_click, RL_click, R2, L2, RX, RY, LX, LY);
+        }   
+    }
+ 
+   
+    int readable(){
+        return _serial.readable();
+    }
+    
+public:
+    // Deklarasi variabel tombol joystik
+    bool kiri, kanan, atas, bawah;
+    bool segitiga, lingkaran, kotak, silang;
+    bool L1, R1, L3, R3, START, SELECT, PS;
+    
+    bool kiri_click, kanan_click, atas_click, bawah_click;
+    bool segitiga_click, lingkaran_click, kotak_click, silang_click;
+    bool L1_click, R1_click, L3_click, R3_click, R2_click, L2_click;
+    bool R2sebelum,L2sebelum;
+    bool START_click, SELECT_click, PS_click;
+  
+protected:  
+    virtual int _getc(){return _serial.getc();}
+    Serial _serial;
+};
+ 
+};
+ 
+using namespace JoystickPS3;

diff -r 000000000000 -r dddc43348c25 Motor.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor.lib	Sun Feb 24 09:47:02 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/KRAI-2018/code/Motornew/#1d0887244f8b

diff -r 000000000000 -r dddc43348c25 encoderKRTMI.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/encoderKRTMI.lib	Sun Feb 24 09:47:02 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/KRTMI-2019/code/encoderKRTMI/#fd29d4db8c40

diff -r 000000000000 -r dddc43348c25 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Feb 24 09:47:02 2019 +0000
@@ -0,0 +1,504 @@
+
+////////////////////////////////////////////////////////////////////////////////
+// Robo Taker (Semi-Otomatis) 2018                                            //
+// -------------------------------------------------------------------------- //
+// Gerakan Robot:                                                             //
+// - Arah   -> Gerak                                                          //
+// - Silang -> Buka atau Tutup Gripper A                                      //
+// - Lingkaran -> Buka atau Tutup Gripper B                                   //
+// - Kotak -> Extend atau Shrink Slider A                                     //
+// - Segitigas -> Extend atau Shrink Slider B                                 //
+// - Start  -> Ubah Mode                                                      //
+////////////////////////////////////////////////////////////////////////////////
+#include "mbed.h"
+#include "Motor.h"
+#include "encoderKRTMI.h"
+#include "JoystickPS3.h"
+#include "pinList.h"
+#include "millis.h"
+
+////////////////////////////////////////////////////////////////////////////////
+// Konstanta Program                                                          //
+////////////////////////////////////////////////////////////////////////////////
+#define PI 3.141592653593
+#define RAD_TO_DEG  57.2957795131
+#define MAX_W_SPEED 15000           //max angular speed of robot
+#define TOLERANCET 0.8              //theta tolerance
+#define PULSE_TO_JARAK  0.7416027  //0.7656027 //0.581776     //kll roda / pulses
+#define L 298.0                     //roda to center of robot
+#define TS 2.0                      //time sampling
+#define LIMITPWM 0.4                //limit pwm motor
+
+//Konstanta PID Theta
+#define KP_W 1.8
+#define KI_W 0
+#define KD_W 140
+
+#define MOTOR_LIMIT_MAX 1
+#define MOTOR_LIMIT_MIN -1
+
+// Set 1 untuk aktifkan fitur pc.print
+#define DEBUG   1
+
+////////////////////////////////////////////////////////////////////////////////
+// Object Program                                                               //
+////////////////////////////////////////////////////////////////////////////////
+// Serial
+Serial pc(USBTX, USBRX, 115200);
+joysticknucleo stick(PIN_TX, PIN_RX);
+
+// Pneumatik
+DigitalOut pneumatik[5]{PIN_PNEUMATIK_1, PIN_PNEUMATIK_2, PIN_PNEUMATIK_5, PIN_PNEUMATIK_6, PIN_PNEUMATIK_9};
+
+// Encoder
+encoderKRTMI encoder_A(PIN_A_CHANNEL_A, PIN_A_CHANNEL_B, 540, encoderKRTMI::X4_ENCODING);
+encoderKRTMI encoder_B(PIN_B_CHANNEL_A, PIN_B_CHANNEL_B, 540, encoderKRTMI::X4_ENCODING);
+encoderKRTMI encoder_C(PIN_C_CHANNEL_A, PIN_C_CHANNEL_B, 540, encoderKRTMI::X4_ENCODING);
+
+// Motor
+Motor motor1(PIN_PWM_A, PIN_FWD_A, PIN_REV_A);
+Motor motor2(PIN_PWM_B, PIN_FWD_B, PIN_REV_B);
+Motor motor3(PIN_PWM_C, PIN_FWD_C, PIN_REV_C);
+
+////////////////////////////////////////////////////////////////////////////////
+// Deklarasi Prosedure dan Fungsi                                             //
+////////////////////////////////////////////////////////////////////////////////
+void gerakMotor();
+void hitungPID(double theta_s);
+void hitungParameter();
+void printPulse();
+void case_gerak();
+void motor_Stop();
+void motor_ForceStop();
+void gerak_Pneumatic();
+void case_pneumatic();
+
+////////////////////////////////////////////////////////////////////////////////
+// Variable-variable                                                          //
+////////////////////////////////////////////////////////////////////////////////
+int joystick;
+int pn = 0;
+int pn2 = 0;
+int pn3 = 0;
+int pn_state = 0;
+double pulse_A=0, pulse_B=0, pulse_C=0;
+double Vr = 0, Vr_max = 0;
+double Vw = 0;
+double a = 0;
+double w = 0;
+double x =0, x_s=0, y=0, y_s=0, x_prev=0, y_prev=0;
+double theta=0, theta_s=0;
+double theta_error = 0, theta_prev=0, theta_error_prev=0, sum_theta_error=0;
+unsigned long last_mt_print,last_mt_print2, last_mt_pid, last_mt_rotasi;
+unsigned long last_mt_aksel, last_mt_desel,last1;
+bool modeauto = 1;
+bool print_pulse = 0;
+int brake_state=0;
+double Vmax = 0.4;
+double Vw_max = 0.3;
+int force=0;
+int count = 0;
+int countX = 0;
+int countKotak = 0;
+int countCircle = 0;
+int countSegitiga=0;
+
+////////////////////////////////////////////////////////////////////////////////
+// Main Program                                                               //
+////////////////////////////////////////////////////////////////////////////////
+int main(){
+    stick.setup();
+    stick.idle();
+
+    while(1){
+        if(stick.readable() ) {
+            // Panggil fungsi pembacaan joystik
+            stick.baca_data();
+            // Panggil fungsi pengolahan data joystik
+            stick.olah_data();
+            if ( (!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)&&(!stick.L1)&&(!stick.R1) && (!stick.R2) && (!stick.L2) ){
+
+                pc.printf("tidak ada input\n");
+            } else {
+               pc.printf("ada input\n");
+            }
+            
+            
+        }
+        
+   }/*
+    while(1){
+        //doing nothing
+        
+        pulse_A = encoder_C.getPulses();
+        pc.printf("%.2f \n", pulse_A);
+        if (pulse_A>10000)
+            motor3.speed(0.0);
+        else
+            motor3.speed(-0.9);
+        Thread::wait(2);
+    }*/
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Prosedure dan Fungsi                                             //
+////////////////////////////////////////////////////////////////////////////////
+void hitungParameter(){
+    pulse_A = encoder_A.getPulses()*PULSE_TO_JARAK;
+    pulse_B = encoder_B.getPulses()*PULSE_TO_JARAK;
+    pulse_C = encoder_C.getPulses()*PULSE_TO_JARAK;
+
+    //Compute value
+    x = x_prev + (2*pulse_A - pulse_C - pulse_B)/3*cos(theta_prev) - (-pulse_C+pulse_B)*0.5773*sin(theta_prev);
+    y = y_prev + (2*pulse_A - pulse_C - pulse_B)/3*sin(theta_prev) + (-pulse_C+pulse_B)*0.5773*cos(theta_prev);
+    theta = theta_prev + (pulse_A + pulse_C + pulse_B)/(3.0*L);
+
+    //Update value
+    x_prev = x;
+    y_prev = y;
+    theta_prev = theta;
+
+    encoder_A.reset();
+    encoder_B.reset();
+    encoder_C.reset();
+}
+
+void hitungPID(double theta_s){
+    //menghitung error theta
+    theta_error = theta_s - (theta*RAD_TO_DEG);
+    sum_theta_error += theta_error;
+
+    //kalkulasi PID Theta
+    w = KP_W*theta_error + KI_W*TS*sum_theta_error + KD_W*(theta_error - theta_error_prev)/TS;
+    Vw += (w*L/MAX_W_SPEED)*LIMITPWM;
+
+    //update
+    theta_error_prev = theta_error;
+
+    //Limit Kecepatan Vw (kec rotasi)
+    if (Vw > Vw_max){
+        Vw = Vw_max;
+    }
+    else if ( Vw < -1*Vw_max){
+        Vw = -1*Vw_max;
+    }
+}
+
+void gerakMotor(){
+    // Berhenti jika tidak maju, mundur, kiri, kanan, atau pivot
+    if (brake_state == 1){
+        motor_ForceStop();
+        //motor_Stop();
+    } else {  
+        if (count>50){
+            if ((millis() - last_mt_aksel > 100) && Vr < Vr_max){
+                if (Vr < 0.39){
+                    Vr = 0.39;
+                }
+                else if ( (Vr >= 0.39)&& (Vr<=0.45)){
+                    Vr+= 0.05;
+                } else if ((Vr>0.45) && (Vr<1.00) ){
+                    Vr+=0.12;
+                } else { 
+                        Vr = 1.2;
+                }
+                last_mt_aksel = millis();
+            }
+        } else {
+            Vr=0.42;
+        }
+        
+        // Limit
+        if (Vr > Vr_max && Vr_max >= 0.000){
+            Vr = Vr_max;
+        }
+        
+        // Control Motor
+        motor1.speed((Vr*cos(a) - Vw));
+        motor2.speed((Vr*(-0.5*cos(a) - 0.866*sin(a)) - Vw));
+        motor3.speed((Vr*(-0.5*cos(a) + 0.866*sin(a)) - Vw));
+        print_pulse = 1;
+    }
+    
+}
+
+void printPulse(){
+    pc.printf("%d \n", pn);
+    //pc.printf("%.2f\t%.2f\t%.2f\t%.2f\t%.2f\n", pulse_A, pulse_B, pulse_C, x, y);
+    //pc.printf("%.2f\t%.2f\t%.2f\t%.2f\t%d\n",theta*RAD_TO_DEG, x, y, Vmax, modeauto);
+}
+
+void motor_Stop(){
+//brake biasa
+    motor1.speed(0);
+    motor2.speed(0);
+    motor2.speed(0);
+}
+
+void motor_ForceStop(){
+//FORCEBRAKE
+    motor1.brake(BRAKE_HIGH);
+    motor2.brake(BRAKE_HIGH);
+    motor3.brake(BRAKE_HIGH);
+}
+
+void case_gerak(){
+    if(stick.SELECT_click){
+    //reset semua variable
+        pneumatik[0]=1; //gripA
+        pneumatik[1]=1; //gripB
+        pneumatik[2]=1; //sliderA
+        pneumatik[3]=1; //sliderB
+        pn=0;
+        pn2=0;
+        pn3=0;
+        modeauto = 0;
+        theta = 0.0;
+        theta_prev = 0.0;
+        theta_s = 0;
+        theta_error_prev = 0;
+        sum_theta_error = 0;
+        theta_error = 0;
+    }
+
+    // Rotasi
+    if (!stick.L1 && stick.R1){        // Pivot Kanan
+        //theta = 0.0;
+        //theta_prev = 0.0;
+        theta_s = theta*RAD_TO_DEG;
+        theta_error_prev = 0;
+        sum_theta_error = 0;
+        theta_error = 0;
+        if (Vr_max==0)
+            Vw = 0.25;
+        else
+            Vw = 0.17;
+    }
+    else if (!stick.R1 && stick.L1){   // Pivot Kiri
+        //theta = 0.0;
+        //theta_prev = 0.0;
+        theta_s = theta*RAD_TO_DEG;
+        theta_error_prev = 0;
+        sum_theta_error = 0;
+        theta_error = 0;
+        if (Vr_max==0)
+            Vw = -0.25;
+        else
+            Vw = -0.17;
+    }
+    
+
+    if (stick.R2){
+        // Mode Sprint
+        Vmax=1.25;
+    } else 
+        Vmax=0.83;
+        
+    
+    if ( (!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)&&(!stick.L1)&&(!stick.R1) && (!stick.R2) && (!stick.L2) ){
+        count=0;
+        brake_state=1;
+        theta = 0.0;
+        theta_prev = 0.0;
+        theta_s = 0.0;
+        theta_error_prev = 0;
+        sum_theta_error = 0;
+        theta_error = 0; 
+    }else{
+        if (count<200)
+            count++;
+        else 
+            count=500;
+        brake_state=0;
+    }
+    // Linier
+    
+        if ((!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)&&(stick.L2)){
+            //L2 : serong kiri bawah + pivot kiri (mundur saat kasih SC
+            a = (-22/RAD_TO_DEG); // mundur saat setelah pengambilan
+            Vr_max = 0.84;
+            Vw=-0.19;
+        } 
+        else if ((stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)&&(stick.L2)){
+            a = (5/RAD_TO_DEG); // L2+atas : mundur saat setelah pengambilan
+            Vr_max = 0.87;
+            Vw=-0.05;
+        } 
+        else if ((stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)){
+            a = (90/RAD_TO_DEG); // Maju
+            Vr_max = Vmax;
+        }
+        else if ((!stick.atas)&&(stick.bawah)&&(!stick.kanan)&&(!stick.kiri)){
+            a = (-96/RAD_TO_DEG); // Mundur
+            Vr_max = Vmax;
+        }
+        else if ((stick.atas)&&(!stick.bawah)&&(!stick.kiri)&&(stick.kanan)){
+            a = (135/RAD_TO_DEG); // Serong Atas Kanan
+            Vr_max = Vmax;
+        }
+        else if ((!stick.atas)&&(stick.bawah)&&(!stick.kiri)&&(stick.kanan)){
+            a = (-135/RAD_TO_DEG); // Serong Bawah Kanan
+            Vr_max = Vmax;
+        }
+        else if ((stick.atas)&&(!stick.bawah)&&(stick.kiri)&&(!stick.kanan)){
+            a = (45/RAD_TO_DEG); // Serong Atas Kiri
+            Vr_max = Vmax;
+        }
+        else if ((!stick.atas)&&(stick.bawah)&&(stick.kiri)&&(!stick.kanan)){
+            a = (-45/RAD_TO_DEG); // Serong Bawah Kiri
+            Vr_max = Vmax;
+        }
+        else if ((!stick.atas)&&(!stick.bawah)&&(stick.kanan)&&(!stick.kiri)){
+            a = (180/RAD_TO_DEG); // Kanan
+            Vr_max = 0.8;
+        }
+        else if ((!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(stick.kiri)){
+            a = (0/RAD_TO_DEG); // Kiri
+            Vr_max = 0.8;
+        }
+        else {
+            Vr_max = 0;
+        }
+    //}
+}
+void case_pneumatic(){
+ // Control Pneumatic
+ /**
+ *  pneumatik[0] = Gripper A
+ *  pneumatik[1] = Gripper B
+ *  pneumatik[3] = Slider A
+ *  pneumatik[4] = Slider B
+ **/
+    if((!stick.silang) && (!stick.kotak) && (!stick.lingkaran) && (!stick.segitiga)) {
+    // reset state pneumatik
+        countX=0;
+        countCircle=0;
+        countSegitiga=0;
+        countKotak=0;
+    } 
+    if ((stick.silang && countX<50)) {
+    //SILANG : sekuensial tiap tangan gripper
+        countX++;
+    }
+    
+    if (countX>0 && countX<100) {
+    //state button X, lengan Kiri - lengakn Kanan
+        countX=150;
+        if (pn<4 && pn>=0)
+            pn++;
+        else    
+            pn=0;
+
+        if(pn==0 || pn==22){
+                pneumatik[0]=1;//gripA
+                pneumatik[1]=1;//gripB
+                pneumatik[2]=1;//sliderA
+                pneumatik[3]=1;//sliderB
+        } else if (pn==1){
+            pneumatik[2] = 0;
+        } else  if (pn==2){
+            pneumatik[0] = 0; 
+            wait_ms(50);
+            pneumatik[2] = 1;
+        } else if (pn==3){
+            pneumatik[3]=0;
+        } else if (pn==4){
+            pneumatik[1] = 0; 
+            wait_ms(50);
+            pneumatik[3] = 1;
+        }
+        pn2=0;
+        pn3=0;
+    }
+    
+    if ((stick.kotak) && countKotak<50){
+    //KOTAK : maju 2 glider 
+        countKotak++;
+    }
+    
+    if (countKotak>5 && countKotak<100){
+    //state button KOTAK
+        countKotak=250;
+        pn=0;
+        pn3=0;
+        if (pn2<2 && pn2>=0)
+            pn2++;
+        else
+            pn2=0;
+        
+            if(pn2==0){
+                pneumatik[0]=1;
+                pneumatik[1]=1;
+                pneumatik[2]=1;
+                pneumatik[3]=1;
+            } else if(pn2==1){
+                pneumatik[2]=0;
+                pneumatik[3]=0;
+            } else if(pn2==2){
+                pneumatik[0]=0;
+                pneumatik[1]=0;
+                wait_ms(50);
+                pneumatik[2]=1;
+                pneumatik[3]=1;
+            } 
+        
+    } 
+
+    if ((stick.segitiga) && countSegitiga<50)
+    //SEGITIGA :  buka tutup kedua gripper
+        countSegitiga++;
+    if (countSegitiga>4 && countSegitiga<100) {
+    //state button SEGITIGA
+        countSegitiga=250;
+        if ( (pneumatik[0]==0 && pneumatik[1]==1) || (pneumatik[0]==1 && pneumatik[1]==0) ){
+            pneumatik[0]=1;
+            pneumatik[1]=1;
+            pneumatik[2]=1;
+            pneumatik[3]=1;
+        } else { 
+            pneumatik[0]=!pneumatik[0];
+            pneumatik[1]=!pneumatik[1];
+            pneumatik[2]=1;
+            pneumatik[3]=1;
+        }
+        pn=0;
+        pn2=0;
+        pn3=0;
+    }
+        
+    if ((stick.lingkaran) && countCircle<50){
+    //KOTAK : maju 2 glider , 2 kali kotak 1 kali lingkaran
+        countCircle++;
+    }
+    
+    if (countCircle>4 && countCircle<100){
+    //LINGKARAN : lengan kanan - lengan kiri 
+        countCircle=250;
+        pn=0;
+        pn2=0;
+        if (pn3<4 && pn3>=0)
+            pn3++;
+        else
+            pn3=0;
+        if(pn3==0 || pn3==22){
+                pneumatik[0]=1;//gripA
+                pneumatik[1]=1;//gripB
+                pneumatik[2]=1;//sliderA
+                pneumatik[3]=1;//sliderB
+        } else if (pn3==1){
+            pneumatik[3] = 0;
+        } else  if (pn3==2){
+            pneumatik[1] = 0; 
+            wait_ms(50);
+            pneumatik[3] = 1;
+        } else if (pn3==3){
+            pneumatik[2]=0;
+        } else if (pn3==4){
+            pneumatik[0] = 0; 
+            wait_ms(50);
+            pneumatik[2] = 1;
+        }
+        pn2=0;
+    }  
+        
+}

diff -r 000000000000 -r dddc43348c25 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Sun Feb 24 09:47:02 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file

diff -r 000000000000 -r dddc43348c25 millis.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/millis.lib	Sun Feb 24 09:47:02 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/DFRobot/code/millis/#736e6cc31bcd

diff -r 000000000000 -r dddc43348c25 pinList.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pinList.h	Sun Feb 24 09:47:02 2019 +0000
@@ -0,0 +1,38 @@
+// Pin Motor
+// Motor A
+#define PIN_PWM_A           PB_6
+#define PIN_FWD_A           PB_13
+#define PIN_REV_A           PA_10
+// Motor B
+#define PIN_PWM_B           PB_8
+#define PIN_FWD_B           PA_5
+#define PIN_REV_B           PA_9
+// Motor C
+#define PIN_PWM_C           PB_9
+#define PIN_FWD_C           PA_12
+#define PIN_REV_C           PA_11
+
+// Pin UART
+// Arduino
+#define PIN_TX              PA_0
+#define PIN_RX              PA_1
+
+// Pin Encoder
+#define PIN_A_CHANNEL_A     PC_0
+#define PIN_A_CHANNEL_B     PC_1
+#define PIN_B_CHANNEL_A     PC_6
+#define PIN_B_CHANNEL_B     PC_7
+#define PIN_C_CHANNEL_A     PC_5
+#define PIN_C_CHANNEL_B     PC_4
+
+// Pin pneumatik
+#define PIN_PNEUMATIK_1 PC_11
+#define PIN_PNEUMATIK_2 PA_15
+#define PIN_PNEUMATIK_3 PC_14
+#define PIN_PNEUMATIK_4 PH_0
+#define PIN_PNEUMATIK_5 PH_1       
+#define PIN_PNEUMATIK_6 PA_4
+#define PIN_PNEUMATIK_7 PC_15      
+#define PIN_PNEUMATIK_8 PC_13   
+#define PIN_PNEUMATIK_9 PA_14        
+#define PIN_PNEUMATIK_10 PC_10