File content as of revision 0:20c5e7e5c862:

#include "mbed.h"
#include "Motor.h"
#include "QEI.h"
#include "PID.h"

Serial pc(USBTX, USBRX);
Serial device(PB_6, PA_10);

Motor DepanKanan(PB_9, PC_5, PA_12);
QEI DepanKananQEI(PB_8, PC_9, NC, 624);
PID DepanKananPID(1.2, 0.1, 0.00000001, 0.02);

char nilai, huruf;
int DepanKananPulses      = 0; //How far the left wheel has travelled.
int DepanKananPrevPulses  = 0; //The previous reading of how far the left wheel
    //has travelled.
int DepanKananCount, d_pulse_dk;
float DepanKananVelocity  = 0.0; //The velocity of the left wheel in pulses per
float Koordinat;
int Resolusi = 624;
float pi = 3.14;
float diameter = 5.8;
Timer t_jalan;

void kosongkan()
{
    DepanKananQEI.reset();
    DepanKananPulses      = 0;
    DepanKananPrevPulses  = 0;
    DepanKananVelocity  = 0.0;
}

void proses_kecepatan()
{
    DepanKananPulses = DepanKananQEI.getPulses();
    d_pulse_dk = DepanKananPulses - DepanKananPrevPulses;
    DepanKananVelocity = (DepanKananPulses - DepanKananPrevPulses) / 0.02;
    DepanKananPrevPulses = DepanKananPulses;             
    DepanKananPID.setProcessValue(fabs(DepanKananVelocity));
}

void cari_koordinat()
{
    Koordinat = (DepanKananCount/Resolusi)*pi*diameter;
}

void fungsiBluetooth(void)
{
    if(device.readable())
    {
        nilai = device.getc();
        if (nilai == 'S')
        {
            huruf = 'S';    
        }
        else if (nilai == 'F')
        {
            huruf = 'F';
        }
        else if(nilai =='B')
        {
            huruf = 'B';
        }
        else if(nilai =='L')
        {
            huruf = 'L';
        }
        else if(nilai =='R')
        {
            huruf = 'R';
        }
        else
        {
            huruf = 'M';
        }
    }
        
}

void Get_Count()
{
    DepanKananCount = DepanKananCount + DepanKananPulses;
}

int main()
{
    pc.baud(38400);
    device.baud(9600);
    device.attach(&fungsiBluetooth, Serial::RxIrq);
    DepanKanan.period(0.01f);    

    DepanKananPID.setInputLimits(0, 2220);//Input  units: counts per second.
    DepanKananPID.setOutputLimits(0.0, 0.9);//Output units: PwmOut duty cycle as %.
    DepanKananPID.setMode(AUTO_MODE);
    
    DepanKananPID.setSetPoint(1000);
    wait(3);
    
    while(1)
    {
        if(huruf == 'F')
        {
            kosongkan();
            while(huruf == 'F')
            {
                t_jalan.reset();
                t_jalan.start();
                proses_kecepatan();
                DepanKanan.speed(DepanKananPID.compute());
                while(t_jalan.read_ms()<=20)
                {
                }
                t_jalan.reset();
                DepanKananCount = DepanKananCount + d_pulse_dk;
                cari_koordinat();
                pc.printf("Koordinat = %f\n", Koordinat);
            }
            DepanKanan.brake();
        }
        
        else if(huruf == 'B')
        {
            kosongkan();
            while(huruf == 'B')
            {
                t_jalan.reset();
                t_jalan.start();
                proses_kecepatan();
                DepanKanan.speed(DepanKananPID.compute()*-1);
                while(t_jalan.read_ms()<=20)
                {
                }
                t_jalan.reset();
                DepanKananCount = DepanKananCount + d_pulse_dk;
                cari_koordinat();
                pc.printf("\n\r Koordinat: %f", Koordinat);
            }
            DepanKanan.brake();
        }
        
        else
        {
            pc.printf("%c\n", huruf);
        }
    }
}