Revision 0:9f9e6b69aa8a, committed 20 months ago

Comitter:

photonicbabe

Date:

Mon Oct 31 12:38:10 2022 +0000

Commit message:

PIDOdoInverse Tanpa Yaw

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU9250.lib	Mon Oct 31 12:38:10 2022 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/Edutech/code/MPU9250/#98a0cccbc509

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor.lib	Mon Oct 31 12:38:10 2022 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/simon/code/Motor/#f265e441bcd9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.lib	Mon Oct 31 12:38:10 2022 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/aberk/code/PID/#6e12a3e5af19

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PIDremote.cpp	Mon Oct 31 12:38:10 2022 +0000
@@ -0,0 +1,714 @@
+#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();    
+            }
+        }
+    }                       
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Mon Oct 31 12:38:10 2022 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/aberk/code/QEI/#5c2ad81551aa

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon Oct 31 12:38:10 2022 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file