KRTMI_Titanium2022
PID Odometry Inverse Tanpa Yaw
Dependencies: MPU9250 Motor PID QEI mbed
Diff: PIDremote.cpp
- Revision:
- 0:9f9e6b69aa8a
--- /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