Revision 0:57124be75a63, committed 2017-05-29

Comitter:

gustavaditya

Date:

Mon May 29 15:43:42 2017 +0000

Child:

1:7d3041941b1b

Commit message:

CLOSED LOOP BASE

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/JoystickPS3.h	Mon May 29 15:43:42 2017 +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;
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor.lib	Mon May 29 15:43:42 2017 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/aberk/code/Motor/#c75b234558af

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.lib	Mon May 29 15:43:42 2017 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/PID/#6e12a3e5af19

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PS3Arduino.txt	Mon May 29 15:43:42 2017 +0000
@@ -0,0 +1,299 @@
+/*********************************************************************************************/
+/**                                                                                         **/
+/** 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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/encoderKRAI.cpp	Mon May 29 15:43:42 2017 +0000
@@ -0,0 +1,126 @@
+/********************************************************/
+/*          Library untuk pembacaan Encoder             */
+/*                  Adapsi dari QEI                     */
+/*                                                      */
+/*  Encoder yang sudah dicoba :                         */
+/*  1. Autonics                                         */
+/*  2. Encoder bawaan Motor                             */
+/*                                                      */
+/*  ______________________                              */
+/*  |______Autonics______|                              */
+/*  | Out A = Input 1    |                              */
+/*  | Out B = Input 2    |                              */
+/*  | 5V                 |                              */
+/*  |_Gnd________________|                              */
+/*                                                      */
+/********************************************************/
+
+#include "mbed.h"
+
+#include "encoderKRAI.h"
+
+encoderKRAI::encoderKRAI(PinName channelA,
+                         PinName channelB,
+                         int pulsesPerRev,
+                         Encoding encoding) : channelA_(channelA), channelB_(channelB)
+{
+    pulses_       = 0;
+    revolutions_  = 0;
+    pulsesPerRev_ = pulsesPerRev;
+    encoding_     = encoding;
+
+    //Workout what the current state is.
+    int chanA = channelA_.read();
+    int chanB = channelB_.read();
+
+    //2-bit state.
+    currState_ = (chanA << 1) | (chanB);
+    prevState_ = currState_;
+
+    //X2 encoding uses interrupts on only channel A.
+    //X4 encoding uses interrupts on      channel A,
+    //and on channel B.
+    channelA_.rise(this, &encoderKRAI::encode);
+    channelA_.fall(this, &encoderKRAI::encode);
+
+    //If we're using X4 encoding, then attach interrupts to channel B too.
+    if (encoding == X4_ENCODING) {
+        channelB_.rise(this, &encoderKRAI::encode);
+        channelB_.fall(this, &encoderKRAI::encode);
+    }
+}
+
+void encoderKRAI::reset(void) {
+
+    pulses_      = 0;
+    revolutions_ = 0;
+
+}
+
+/*int encoderKRAI::getCurrentState(void) {
+
+    return currState_;
+
+}*/
+
+int encoderKRAI::getPulses(void) {
+
+    return pulses_;
+
+}
+
+int encoderKRAI::getRevolutions(void) {
+
+    revolutions_ = pulses_ / pulsesPerRev_;
+    return revolutions_;
+
+}
+
+////////////////////////////////////////////////////////
+
+void encoderKRAI::encode(void) {
+
+    int change = 0;
+    int chanA  = channelA_.read();
+    int chanB  = channelB_.read();
+
+    //2-bit state.
+    currState_ = (chanA << 1) | (chanB);
+
+    if (encoding_ == X2_ENCODING) {
+
+        //11->00->11->00 is counter clockwise rotation or "forward".
+        if ((prevState_ == 0x3 && currState_ == 0x0) ||
+                (prevState_ == 0x0 && currState_ == 0x3)) {
+
+            pulses_++;
+
+        }
+        //10->01->10->01 is clockwise rotation or "backward".
+        else if ((prevState_ == 0x2 && currState_ == 0x1) ||
+                 (prevState_ == 0x1 && currState_ == 0x2)) {
+
+            pulses_--;
+
+        }
+
+    } else if (encoding_ == X4_ENCODING) {
+
+        //Entered a new valid state.
+        if (((currState_ ^ prevState_) != INVALID) && (currState_ != prevState_)) {
+            //2 bit state. Right hand bit of prev XOR left hand bit of current
+            //gives 0 if clockwise rotation and 1 if counter clockwise rotation.
+            change = (prevState_ & PREV_MASK) ^ ((currState_ & CURR_MASK) >> 1);
+
+            if (change == 0) {
+                change = -1;
+            }
+
+            pulses_ -= change;
+        }
+
+    }
+
+    prevState_ = currState_;
+
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/encoderKRAI.h	Mon May 29 15:43:42 2017 +0000
@@ -0,0 +1,106 @@
+#ifndef ENCODERKRAI_H
+#define ENCODERKRAI_H
+
+/**
+ * Includes
+ */
+#include "mbed.h"
+
+/**
+ * Defines
+ */
+#define PREV_MASK 0x1 //Mask for the previous state in determining direction
+//of rotation.
+#define CURR_MASK 0x2 //Mask for the current state in determining direction
+//of rotation.
+#define INVALID   0x3 //XORing two states where both bits have changed.
+
+/**
+ * Quadrature Encoder Interface.
+ */
+class encoderKRAI {
+
+public:
+
+    typedef enum Encoding {
+
+        X2_ENCODING,
+        X4_ENCODING
+
+    } Encoding;
+    
+    /** Membuat interface dari encoder    
+     *
+     * @param inA DigitalIn, out A dari encoder
+     * @param inB DigitalIn, out B dari encoder
+     */
+    encoderKRAI(PinName channelA, PinName channelB, int pulsesPerRev, Encoding encoding = X2_ENCODING);
+    
+    /**
+     * Reset encoder.
+     *
+     * Menset pulse dan revolusi/putaran menjadi 0
+     */
+    void reset(void);
+    
+    /**
+     * Membaca pulse yang didapat oleh encoder
+     *
+     * @return Nilai pulse yang telah dilalui.
+     */
+    int getPulses(void);
+    
+    /**
+     * Membaca putaran yang didapat oleh encoder
+     *
+     * @return Nilai revolusi/putaran yang telah dilalui.
+     */
+    int getRevolutions(void);
+    
+    /**
+     * Membaca pulse yang didapat oleh encoder
+     *
+     * @return Nilai pulse yang telah dilalui.
+     */
+    //int readDeltaPulses(void);
+    
+    /**
+     * Membaca putaran yang didapat oleh encoder
+     *
+     * @return Nilai revolusi/putaran yang telah dilalui.
+     */
+    //int readDeltaRevolutions(void);
+
+private:
+
+    /**
+     * Menghitung pulse
+     *
+     * Digunakan setiap rising/falling edge baik channel A atau B
+     * Membaca putaran CW atau CCW => mengakibatkan pertambahan/pengurangan pulse
+     */
+    void encode(void);
+
+    /**
+     * Indeks setiap rising edge untuk menghitung putaran
+     * Nilai bertambah 1
+     */
+    //void index(void);
+
+    Encoding encoding_;
+
+    InterruptIn channelA_;
+    InterruptIn channelB_;
+    //InterruptIn index_;
+
+    int          pulsesPerRev_;
+    int          prevState_;
+    int          currState_;
+
+    volatile int pulses_;
+    volatile int revolutions_;
+
+
+};
+
+#endif /* ENCODERKRAI_H */
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon May 29 15:43:42 2017 +0000
@@ -0,0 +1,49 @@
+/****************************************************************************/
+/*                  PROGRAM UNTUK CLOSED LOOP BASE                          */
+/*                                                                          */
+/*                  Last Update : 29 Mei 2017                               */
+/*                                                                          */
+/*  - Digunakan encoder bawaan motor                                        */
+/*  - Konfigurasi Motor dan Encoder sbb :                                   */
+/*                      ______________________                              */
+/*                     /                      \    Rode Depan Belakang:     */
+/*                    /      1 (Depan)         \   Omniwheel                */
+/*                   |                          |                           */
+/*                   | 3 (kiri)       4 (kanan) |  Roda Kiri Kanan:         */
+/*                   |                          |  Omniwheel                */
+/*                    \       2 (Belakang)     /                            */
+/*                     \______________________/    Putaran CW tampak depan  */
+/*                                                 positif                  */
+/*   SETTINGS (WAJIB!) :                                                    */
+/*   1. Settings Pin Encoder, Resolusi, dan Tipe encoding                   */
+/*   2. Deklarasi penggunaan library pada bagian deklarasi encoder          */
+/*                                                                          */
+/****************************************************************************/
+
+#include "mbed.h"
+#include "JoystickPS3.h"
+#include "Motor.h"
+#include "encoderKRAI.h"
+#include "millis.h"
+
+#define PI 3.14159265
+#define D_ENCODER 10        // Diameter Roda Encoder
+#define D_ROBOT 80          // Diameter Roda Robot
+
+// Variable Bawah
+float tuneDpn           = 1.0;     // Tunning PWM motor Depan
+float tuneBlk           = 1.0;     // Tunning PWM motor belakang
+
+/* Inisialisasi  Pin TX-RX Joystik dan PC */
+joysticknucleo joystick(PA_0,PA_1);
+Serial pc(USBTX,USBRX);
+
+/* Deklarasi Encoder Launcher */
+encoderKRAI encKiri( PC_10, PC_11, 28, encoderKRAI::X4_ENCODING);
+encoderKRAI encKanan( PC_12, PD_2, 28, encoderKRAI::X4_ENCODING);
+
+/* Deklarasi Motor Base */
+Motor motorDpn(PB_7, PC_3, PC_0); //(PB_9, PA_12, PC_5);
+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);
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon May 29 15:43:42 2017 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/e1686b8d5b90
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/millis.lib	Mon May 29 15:43:42 2017 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/teams/DFRobot/code/millis/#736e6cc31bcd