File content as of revision 0:9f9e6b69aa8a:

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

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

QEI DepanKananQEI(PB_8, PC_9, NC, 986); //roda depan kanan PB_1, PB_15, NC, 624
QEI BelakangKananQEI(PB_1, PB_15, NC, 986); //roda belakang kiri PB_14, PB_13, NC, 624
QEI DepanKiriQEI(PC_10, PC_12, NC, 986); //roda depan kiri 
QEI BelakangKiriQEI(PA_15, PB_7, NC, 986); //roda belakang kiri

PID DepanKananPID(0.62, 0.1, 0.00001, 0.02);  //0000001 Kc, Ti, Td MOTOR baru nomor 1
PID BelakangKananPID(0.6, 0.1, 0.000001, 0.02);  //Kc, Ti, Td MOTOR baru nomor 2
PID BelakangKiriPID(0.89, 0.1, 0.000000000001, 0.02);  //Kc, Ti, Td MOTOR baru nomor 3
PID DepanKiriPID(0.68, 0.1, 0.0000001, 0.02);  //Kc, Ti, Td MOTOR baru nomor 4

Motor DepanKanan(PA_11, PC_8, PC_6);//PB_9, PA_12, PC_5
Motor DepanKiri(PA_0, PB_0, PA_4);
Motor BelakangKanan(PB_9, PC_5, PA_12);//PA_11, PC_6, PC_8
Motor BelakangKiri(PA_1, PC_0, PC_1);

Timer t_jalan;

char nilai, huruf;
int Jalan;
int Jalan_Maju =1;
int Berhenti = 0;
int Jalan_Mundur = 2;
int Jalan_Kanan = 3;
int Jalan_Kiri = 4;
int ke_setpoint = 5;
int mode_inverse = 6;

int DepanKananPulses      = 0; 
int DepanKananPrevPulses  = 0; 
float DepanKananVelocity  = 0.0; 
    
int BelakangKananPulses     = 0; 
int BelakangKananPrevPulses = 0; 
float BelakangKananVelocity = 0.0;

int BelakangKiriPulses     = 0;
int BelakangKiriPrevPulses = 0; 
float BelakangKiriVelocity = 0.0;
    
int DepanKiriPulses     = 0;
int DepanKiriPrevPulses = 0;
float DepanKiriVelocity = 0.0;

int Depan_Kanan_Count;
int Depan_Kiri_Count;
int Belakang_Kanan_Count;
int Belakang_Kiri_Count;

int d_pulse_DepanKanan;
int d_pulse_DepanKiri;
int d_pulse_BelakangKanan;
int d_pulse_BelakangKiri;

int koordinat_x_DepanKiri;
int koordinat_x_DepanKanan;
int koordinat_x_BelakangKiri;
int koordinat_x_BelakangKanan;
int koordinat_y_DepanKiri;
int koordinat_y_DepanKanan;
int koordinat_y_BelakangKiri;
int koordinat_y_BelakangKanan;
int koordinat_x;
int koordinat_y;
int Resolusi = 986;
float pi = 3.141;
float diameter = 5.8f;
float rtd = pi/180.0f;
int setpoint_x;
int setpoint_y;
float radius = 0.029;
float orientasi;

float target_depan_kiri;
float target_depan_kanan;
float target_belakang_kiri;
float target_belakang_kanan;
int arah_depan_kiri;
int arah_depan_kanan;
int arah_belakang_kiri;
int arah_belakang_kanan;


void fungsi_bluetooth(void)
{
    if(bluetooth.readable())
    {
        setpoint_x = bluetooth.getc();
        setpoint_y = bluetooth.getc();
        nilai = bluetooth.getc();
        if (nilai == 'S')
        {
            huruf = 'S';
            Jalan = Berhenti;  
        }
        else if (nilai == 'F')
        {
            huruf = 'F';
            Jalan = Jalan_Maju;
        }
        else if(nilai =='B')
        {
            huruf = 'B';
            Jalan = Jalan_Mundur;
        }
        else if(nilai =='L')
        {
            huruf = 'L';
            Jalan = Jalan_Kiri;
        }
        else if(nilai =='R')
        {
            huruf = 'R';
            Jalan = Jalan_Kanan;
        }
        else if(nilai =='G')
        {
            huruf = 'G';
            Jalan = ke_setpoint;
        }
        else if(nilai == 'Y')
        {
            huruf = 'G';
            Jalan = mode_inverse;
        }
        else if(nilai == 'S')
        {
            Jalan = Berhenti;
        }         
    }
}

void kosongkan()
{
    BelakangKananQEI.reset();
    DepanKananQEI.reset();
    DepanKiriQEI.reset();
    BelakangKiriQEI.reset();
                
    DepanKananPulses      = 0;
    DepanKananPrevPulses  = 0; 
    DepanKananVelocity  = 0.0; 
                
    BelakangKananPulses     = 0; 
    BelakangKananPrevPulses = 0; 
    BelakangKananVelocity = 0.0;
    
    BelakangKiriPulses     = 0;
    BelakangKiriPrevPulses = 0; 
    BelakangKiriVelocity = 0.0;
                
    DepanKiriPulses     = 0;
    DepanKiriPrevPulses = 0;
    DepanKiriVelocity = 0.0; 
}

void proses_kecepatan()
{
    DepanKananPulses = DepanKananQEI.getPulses();
    d_pulse_DepanKanan = DepanKananPulses - DepanKananPrevPulses;
    DepanKananVelocity = (DepanKananPulses - DepanKananPrevPulses) / 0.02;
    DepanKananPrevPulses = DepanKananPulses;             
    DepanKananPID.setProcessValue(fabs(DepanKananVelocity));
    
    DepanKiriPulses = DepanKiriQEI.getPulses();
    d_pulse_DepanKiri = DepanKiriPulses - DepanKiriPrevPulses;
    DepanKiriVelocity = (DepanKiriPulses - DepanKiriPrevPulses) / 0.02;
    DepanKiriPrevPulses = DepanKiriPulses;             
    DepanKiriPID.setProcessValue(fabs(DepanKiriVelocity));
    
    BelakangKananPulses = BelakangKananQEI.getPulses();
    d_pulse_BelakangKanan = BelakangKananPulses - BelakangKananPrevPulses;
    BelakangKananVelocity = (BelakangKananPulses - BelakangKananPrevPulses) / 0.02;
    BelakangKananPrevPulses = BelakangKananPulses;             
    BelakangKananPID.setProcessValue(fabs(BelakangKananVelocity));
    
    BelakangKiriPulses = BelakangKiriQEI.getPulses();
    d_pulse_BelakangKiri = BelakangKiriPulses - BelakangKiriPrevPulses;
    BelakangKiriVelocity = (BelakangKiriPulses - BelakangKiriPrevPulses) / 0.02;
    BelakangKiriPrevPulses = BelakangKiriPulses;             
    BelakangKiriPID.setProcessValue(fabs(BelakangKiriVelocity));
}

void Get_Count()
{
    Depan_Kanan_Count = Depan_Kanan_Count + d_pulse_DepanKanan;
    Depan_Kiri_Count = Depan_Kiri_Count + d_pulse_DepanKiri;
    Belakang_Kanan_Count = Belakang_Kanan_Count + d_pulse_BelakangKanan;
    Belakang_Kiri_Count = Belakang_Kiri_Count + d_pulse_BelakangKiri;
}

void Cari_Koordinat()
{
    koordinat_x_DepanKiri = ((double)Depan_Kiri_Count/(double)Resolusi)*pi*diameter*cos(45.0f*rtd);
    koordinat_x_DepanKanan = ((double)Depan_Kanan_Count/(double)Resolusi)*pi*diameter*cos(135.0f*rtd);
    koordinat_x_BelakangKanan = ((double)Belakang_Kanan_Count/(double)Resolusi)*pi*diameter*cos(225.0f*rtd);
    koordinat_x_BelakangKiri = ((double)Belakang_Kiri_Count/(double)Resolusi)*pi*diameter*cos(315.0f*rtd);
    
    koordinat_y_DepanKiri = ((double)Depan_Kiri_Count/(double)Resolusi)*pi*diameter*sin(45.0f*rtd);
    koordinat_y_DepanKanan = ((double)Depan_Kanan_Count/(double)Resolusi)*pi*diameter*sin(135.0f*rtd);
    koordinat_y_BelakangKanan = ((double)Belakang_Kanan_Count/(double)Resolusi)*pi*diameter*sin(225.0f*rtd);
    koordinat_y_BelakangKiri = ((double)Belakang_Kiri_Count/(double)Resolusi)*pi*diameter*sin(315.0f*rtd);
    
    orientasi = ((double)(koordinat_x_DepanKanan + koordinat_x_BelakangKanan) - (double)(koordinat_x_DepanKiri + koordinat_x_BelakangKiri))/(4.0f*13.0f);
    orientasi = orientasi*57.29;
    koordinat_y = (koordinat_x_DepanKiri + koordinat_x_DepanKanan + koordinat_x_BelakangKiri + koordinat_x_BelakangKanan)/2;
    koordinat_x = (koordinat_y_DepanKiri + koordinat_y_DepanKanan + koordinat_y_BelakangKiri + koordinat_y_BelakangKanan)/2;
}

int cari_abs(float a)
{
    if(a<0)
    {
        return -1;
    }
    else if(a>0)
    {
        return 1;
    }
    else 
    {
        return 0;
    }
}

void cari_inverse(float setpoint_sudut, float target_x, float target_y)
{
    target_x = target_x - koordinat_x;
    target_y = target_y - koordinat_y;
    float target_vx;
    float target_vy;
    target_x = (double)target_x/100.0f;
    target_y = (double)target_y/100.0f;
    if(fabs(target_x)>fabs(target_y))
    {
        if((target_x<0)&&(target_y<0))
        {
            target_vx = -0.6;
            target_vy = ((double)target_y/(double)target_x)*target_vx;
        }
        else if((target_x<0)&&(target_y>0))
        {
            target_vx = -0.6;
            target_vy = ((double)target_y/(double)target_x)*target_vx*-1;
        }
        else if((target_x>0)&&(target_y>0))
        {
            target_vx = 0.6;
            target_vy = ((double)target_y/(double)target_x)*target_vx;
        }
        else if((target_x>0)&&(target_y<0))
        {
            target_vx = 0.6;
            target_vy = ((double)target_y/(double)target_x)*target_vx*-1;
        }
        else if((target_y==0)&&(target_x>0))
        {
            target_vx = 0.6;
            target_vy = 0;
        }
        else if((target_y==0)&&(target_x<0))
        {
            target_vx = -0.6;
            target_vy = 0;
        }
    }
    else if(fabs(target_x)<fabs(target_y))
    {
        if((target_x<0)&&(target_y<0))
        {
            target_vy = -0.6;
            target_vx = ((double)target_x/(double)target_y)*target_vy;
        }
        else if((target_x<0)&&(target_y>0))
        {
            target_vy = -0.6;
            target_vx = ((double)target_x/(double)target_y)*target_vy*-1;
        }
        else if((target_x>0)&&(target_y>0))
        {
            target_vy = 0.6;
            target_vx = ((double)target_x/(double)target_y)*target_vy;
        }
        else if((target_x>0)&&(target_y<0))
        {
            target_vy = 0.6;
            target_vx = ((double)target_x/(double)target_y)*target_vy*-1;
        }
        else if((target_x==0)&&(target_y>0))
        {
            target_vy = 0.6;
            target_vx = 0;
        }
        else if((target_x==0)&&(target_y<0))
        {
            target_vy = -0.6;
            target_vx = 0;
        }
    }
    else if(target_x==target_y)
    {
        if((target_x<0)&&(target_y<0))
        {
            target_vy = -0.6;
            target_vx = target_vy;
        }
        else if((target_x<0)&&(target_y>0))
        {
            target_vy = -0.6;
            target_vx = -1*target_vy;
        }
        else if((target_x>0)&&(target_y<0))
        {
            target_vy = 0.6;
            target_vx = -1*target_vy;
        }
        else if((target_x>0)&&(target_y>0))
        {
            target_vy = 0.6;
            target_vx = target_vy;
        }
    }
    setpoint_sudut = setpoint_sudut*rtd;
    target_depan_kiri = (1/radius)*(sin(45.0f*rtd)*target_vx + cos(45.0f*rtd)*target_vy + radius*setpoint_sudut);
    target_depan_kanan = (1/radius)*(sin(135.0f*rtd)*target_vx + cos(135.0f*rtd)*target_vy + radius*setpoint_sudut);
    target_belakang_kanan = (1/radius)*(sin(225.0f*rtd)*target_vx + cos(225.0f*rtd)*target_vy + radius*setpoint_sudut);
    target_belakang_kiri = (1/radius)*(sin(315.0f*rtd)*target_vx + cos(315.0f*rtd)*target_vy + radius*setpoint_sudut);
    target_depan_kiri = (target_depan_kiri*624.0f)/(2.0f*3.14);
    target_depan_kanan = (target_depan_kanan*624.0f)/(2.0f*3.14);
    target_belakang_kiri = (target_belakang_kiri*624.0f)/(2.0f*3.14);
    target_belakang_kanan = (target_belakang_kanan*624.0f)/(2.0f*3.14);
    arah_depan_kiri = cari_abs(target_depan_kiri);
    arah_depan_kanan = cari_abs(target_depan_kanan);
    arah_belakang_kiri = cari_abs(target_belakang_kiri);
    arah_belakang_kanan = cari_abs(target_belakang_kanan);
}

void set_inverse()
{
    DepanKananPID.setSetPoint(fabs(target_depan_kanan));
    BelakangKananPID.setSetPoint(fabs(target_belakang_kanan));
    BelakangKiriPID.setSetPoint(fabs(target_belakang_kiri));
    DepanKiriPID.setSetPoint(fabs(target_depan_kiri));
}

void jalan_maju()
{
    t_jalan.reset();
    t_jalan.start();
    proses_kecepatan();
    Get_Count();
    Cari_Koordinat();
    pc.printf("Koordinat X : %d\n", koordinat_x);
    pc.printf("Koordinat Y : %d\n", koordinat_y);
    BelakangKanan.speed(BelakangKananPID.compute()*-1);
    BelakangKiri.speed(BelakangKiriPID.compute());
    DepanKanan.speed(DepanKananPID.compute()*-1);
    DepanKiri.speed(DepanKiriPID.compute());
    while(t_jalan.read_ms()<=20)
    {
    }
    t_jalan.reset();
}

void jalan_mundur()
{
    t_jalan.reset();
    t_jalan.start();
    proses_kecepatan();
    Get_Count();
    Cari_Koordinat();
    pc.printf("Koordinat X : %d\n", koordinat_x);
    pc.printf("Koordinat Y : %d\n", koordinat_y);
    BelakangKanan.speed(BelakangKananPID.compute());
    BelakangKiri.speed(BelakangKiriPID.compute()*-1);
    DepanKanan.speed(DepanKananPID.compute());
    DepanKiri.speed(DepanKiriPID.compute()*-1);
    while(t_jalan.read_ms()<=20)
    {
    }
    t_jalan.reset();
}

void jalan_kanan()
{
    t_jalan.reset();
    t_jalan.start();
    proses_kecepatan();
    Get_Count();
    Cari_Koordinat();
    pc.printf("Koordinat X : %d\n", koordinat_x);
    pc.printf("Koordinat Y : %d\n", koordinat_y);
    BelakangKanan.speed(BelakangKananPID.compute()*-1);
    BelakangKiri.speed(BelakangKiriPID.compute()*-1);
    DepanKanan.speed(DepanKananPID.compute());
    DepanKiri.speed(DepanKiriPID.compute());
    while(t_jalan.read_ms()<=20)
    {
    }
    t_jalan.reset();
}

void jalan_kiri()
{
    t_jalan.reset();
    t_jalan.start();
    proses_kecepatan();
    Get_Count();
    Cari_Koordinat();
    pc.printf("Koordinat X : %d\n", koordinat_x);
    pc.printf("Koordinat Y : %d\n", koordinat_y);
    BelakangKanan.speed(BelakangKananPID.compute());
    BelakangKiri.speed(BelakangKiriPID.compute());
    DepanKanan.speed(DepanKananPID.compute()*-1);
    DepanKiri.speed(DepanKiriPID.compute()*-1);
    while(t_jalan.read_ms()<=20)
    {
    }
    t_jalan.reset();
}

void berhenti()
{
    t_jalan.reset();
    t_jalan.start();
    pc.printf("Koordinat X : %d\n", koordinat_x);
    pc.printf("Koordinat Y : %d\n", koordinat_y);
    DepanKanan.brake(0);
    BelakangKanan.brake(0);
    BelakangKiri.brake(0);
    DepanKiri.brake(0);
    while(t_jalan.read_ms()<=20)
    {
    }
    t_jalan.reset();
}

void ke_setpoint_x()
{
    if(koordinat_x<setpoint_x)
    {
        kosongkan();
        while(koordinat_x<setpoint_x)
        {
            jalan_kanan();
        }
        DepanKanan.brake();
        BelakangKanan.brake();
        BelakangKiri.brake();
        DepanKiri.brake();
    }
    else if(koordinat_x>setpoint_x)
    {
        kosongkan();
        while(koordinat_x>setpoint_x)
        {
            jalan_kiri();
        }
        DepanKanan.brake();
        BelakangKanan.brake();
        BelakangKiri.brake();
        DepanKiri.brake();
    }
    else if(koordinat_x==setpoint_x)
    {
        berhenti();
    }
}

void ke_setpoint_y()
{
    if(koordinat_y<setpoint_y)
    {
        kosongkan();
        while(koordinat_y<setpoint_y)
        {
            jalan_maju();
        }
        DepanKanan.brake();
        BelakangKanan.brake();
        BelakangKiri.brake();
        DepanKiri.brake();
    }
    else if(koordinat_y>setpoint_y)
    {
        kosongkan();
        while(koordinat_y>setpoint_y)
        {
            jalan_mundur();
        }
        DepanKanan.brake();
        BelakangKanan.brake();
        BelakangKiri.brake();
        DepanKiri.brake();
    }
    else if(koordinat_y==setpoint_y)
    {
        berhenti();
    }
}

int main()
{   
    pc.baud(38400);
    bluetooth.baud(9600);
    bluetooth.attach(&fungsi_bluetooth, Serial::RxIrq);
    
    DepanKanan.period(0.01f);    
    BelakangKanan.period(0.01f);
    BelakangKiri.period(0.01f);  //Set motor PWM periods to 20KHz.
    DepanKiri.period(0.01f);

    DepanKananPID.setInputLimits(0, 3000);//Input  units: counts per second.
    DepanKananPID.setOutputLimits(0.0, 0.9);//Output units: PwmOut duty cycle as %.
    DepanKananPID.setMode(AUTO_MODE);
    
    BelakangKananPID.setInputLimits(0, 3000);
    BelakangKananPID.setOutputLimits(0.0, 0.9);
    BelakangKananPID.setMode(AUTO_MODE);
    
    BelakangKiriPID.setInputLimits(0, 3000);
    BelakangKiriPID.setOutputLimits(0.0, 0.9);
    BelakangKiriPID.setMode(AUTO_MODE);
    
    DepanKiriPID.setInputLimits(0, 3000);
    DepanKiriPID.setOutputLimits(0.0, 0.9);
    DepanKiriPID.setMode(AUTO_MODE);
    
    DepanKananPID.setSetPoint(1400);
    BelakangKananPID.setSetPoint(1400);
    BelakangKiriPID.setSetPoint(1400);
    DepanKiriPID.setSetPoint(1400); 
    while(1)
    {
        
        if(Jalan == Jalan_Maju)
        {
            kosongkan();
            while(Jalan== Jalan_Maju)
            {
                t_jalan.reset();
                t_jalan.start();
                proses_kecepatan();
                Get_Count();
                Cari_Koordinat();
                //pc.printf("Koordinat X : %d\n", koordinat_x);
                //pc.printf("Koordinat Y : %d\n", koordinat_y);
                pc.printf("Orientasi : %f\n", orientasi);
                BelakangKanan.speed(BelakangKananPID.compute()*-1);
                BelakangKiri.speed(BelakangKiriPID.compute());
                DepanKanan.speed(DepanKananPID.compute()*-1);
                DepanKiri.speed(DepanKiriPID.compute());
                while(t_jalan.read_ms()<=20)
                {
                }
                t_jalan.reset();
            }
            DepanKanan.brake();
            BelakangKanan.brake();
            BelakangKiri.brake();
            DepanKiri.brake();
        }
        
        else if(Jalan == Jalan_Mundur)
        {
            kosongkan();
            while(Jalan== Jalan_Mundur)
            {
                t_jalan.reset();
                t_jalan.start();
                proses_kecepatan();
                Get_Count();
                Cari_Koordinat();
                //pc.printf("Koordinat X : %d\n", koordinat_x);
                //pc.printf("Koordinat Y : %d\n", koordinat_y);
                pc.printf("Orientasi : %f\n", orientasi);
                BelakangKanan.speed(BelakangKananPID.compute());
                BelakangKiri.speed(BelakangKiriPID.compute()*-1);
                DepanKanan.speed(DepanKananPID.compute());
                DepanKiri.speed(DepanKiriPID.compute()*-1);
                while(t_jalan.read_ms()<=20)
                {
                }
                t_jalan.reset();
            }
            DepanKanan.brake();
            BelakangKanan.brake();
            BelakangKiri.brake();
            DepanKiri.brake();
        }
        
        else if(Jalan == Jalan_Kanan)
        {
            kosongkan();
            while(Jalan== Jalan_Kanan)
            {
                t_jalan.reset();
                t_jalan.start();
                proses_kecepatan();
                Get_Count();
                Cari_Koordinat();
                //pc.printf("Koordinat X : %d\n", koordinat_x);
                //pc.printf("Koordinat Y : %d\n", koordinat_y);
                pc.printf("Orientasi : %f\n", orientasi);
                BelakangKanan.speed(BelakangKananPID.compute());
                BelakangKiri.speed(BelakangKiriPID.compute());
                DepanKanan.speed(DepanKananPID.compute());
                DepanKiri.speed(DepanKiriPID.compute());
                while(t_jalan.read_ms()<=20)
                {
                }
                t_jalan.reset();
            }
            DepanKanan.brake();
            BelakangKanan.brake();
            BelakangKiri.brake();
            DepanKiri.brake();
        }
        
        else if(Jalan == Jalan_Kiri)
        {
            kosongkan();
            while(Jalan== Jalan_Kiri)
            {
                t_jalan.reset();
                t_jalan.start();
                proses_kecepatan();
                Get_Count();
                Cari_Koordinat();
                //pc.printf("Koordinat X : %d\n", koordinat_x);
                //pc.printf("Koordinat Y : %d\n", koordinat_y);
                pc.printf("Orientasi : %f\n", orientasi);
                BelakangKanan.speed(BelakangKananPID.compute()*-1);
                BelakangKiri.speed(BelakangKiriPID.compute()*-1);
                DepanKanan.speed(DepanKananPID.compute()*-1);
                DepanKiri.speed(DepanKiriPID.compute()*-1);
                while(t_jalan.read_ms()<=20)
                {
                }
                t_jalan.reset();
            }
            DepanKanan.brake();
            BelakangKanan.brake();
            BelakangKiri.brake();
            DepanKiri.brake();
        }
        else if(Jalan == ke_setpoint)
        {
            DepanKananPID.setSetPoint(1400);
            BelakangKananPID.setSetPoint(1400);
            BelakangKiriPID.setSetPoint(1400);
            DepanKiriPID.setSetPoint(1400);
            while(Jalan == ke_setpoint)
            {
                ke_setpoint_x();
                ke_setpoint_y();
            }
        }
        else if(Jalan == mode_inverse)
        {
            kosongkan();
            cari_inverse(0, (float)setpoint_x, (float)setpoint_y);
            set_inverse();
            while((koordinat_x!=setpoint_x)&&(koordinat_y!=setpoint_y))
            {
                t_jalan.reset();
                t_jalan.start();
                proses_kecepatan();
                Get_Count();
                Cari_Koordinat();
                BelakangKanan.speed(BelakangKananPID.compute()*arah_belakang_kanan);
                BelakangKiri.speed(BelakangKiriPID.compute()*arah_belakang_kiri);
                DepanKanan.speed(DepanKananPID.compute()*arah_depan_kanan);
                DepanKiri.speed(DepanKiriPID.compute()*arah_depan_kiri);
                pc.printf("Koordinat X : %d\n", koordinat_x);
                pc.printf("Koordinat Y : %d\n", koordinat_y);
                while(t_jalan.read_ms()<=20)
                {
                }
                t_jalan.reset();
            }
            DepanKanan.brake();
            BelakangKanan.brake();
            BelakangKiri.brake();
            DepanKiri.brake();
        }
        else if(Jalan == Berhenti)
        {
            while(Jalan==Berhenti)
            {
                t_jalan.reset();
                t_jalan.start();
                pc.printf("Koordinat X : %d\n", koordinat_x);
                pc.printf("Koordinat Y : %d\n", koordinat_y);
                //pc.printf("Orientasi : %f\n", orientasi);
                DepanKanan.brake(0);
                BelakangKanan.brake(0);
                BelakangKiri.brake(0);
                DepanKiri.brake(0);
                while(t_jalan.read_ms()<=20)
                {
                }
                t_jalan.reset();    
            }
        }
    }                       
}