File content as of revision 2:9cf609b74de9:

/****************************************************************************/
/*                  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          */
/*   3. Searah jarum jam positif 517 pulse 1 putaran enc Atas, 524 kiri     */
/*                                                                          */
/****************************************************************************/

#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 55          // Diameter Roda Robot

float K_enc = PI*D_ENCODER;
float K_robot = PI*D_ROBOT;

float speed1=0.6;
float speed2=0.6;
float speed3=0.6;
float speed4=0.6;

float errX, errY, errT, Vt, Vx, Vy, KpX=1.5, KpY=1.5, Kp_tetha=0.15;
float V1, V2, V3, V4;

// 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( PB_5, PB_4, 28, encoderKRAI::X4_ENCODING);
encoderKRAI encAtas( PA_15, PA_14, 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);

void setCenter(){
/* Fungsi untuk menentukan center dari robot */
    encKiri.reset();
    encAtas.reset();
}

int main(void){
    pc.baud(115200);
    setCenter();
    while(1){
        pc.printf("kiri %d, atas %d \n",(int)encKiri.getPulses(),(int)encAtas.getPulses());
        //wait_ms(500); 
    }
}