File content as of revision 3:db2dc06c0686:

/****************************************************************************/
/*                  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. !arah 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 encBawah( PB_4, PB_5, 28, encoderKRAI::X4_ENCODING);
encoderKRAI encDepan( PA_14, PA_15, 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 */
    encBawah.reset();
    encDepan.reset();
}

int main(void){
    double p_samp=0,error=0,kp=0.082;
    pc.baud(115200);
    setCenter();
    
    while(encDepan.getPulses()<5740){
        pc.printf("error %d, p_samp %lf \n",(int)encDepan.getPulses()+(int)encBawah.getPulses(),p_samp);
        error = encDepan.getPulses()+encBawah.getPulses();
        p_samp = kp * error;
        motorDpn.speed(0.7-p_samp);
        motorBlk.speed(-0.7);
        wait_ms(12.5); 
        if(p_samp>1.5)p_samp = 1.5;
        if(p_samp<-0.3)p_samp = -0.3;
    }
    motorDpn.speed(0);
    motorBlk.speed(0);
}