KRTMI_Titanium2022
Robot Sulistia, Full Kodingan Lomba KRTMI 2022
Dependencies: MPU9250 Motor PID QEI SRF05 mbed
Diff: PIDremote.cpp
- Revision:
- 0:a2b784a0e760
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/PIDremote.cpp Tue Oct 04 10:49:03 2022 +0000
@@ -0,0 +1,1575 @@
+#include "mbed.h"
+#include "Motor.h"
+#include "QEI.h"
+#include "PID.h"
+
+#define i2c_address 0x60
+
+Serial pc(USBTX, USBRX);
+Serial bluetooth(PB_6, PA_10);
+I2C CMPS(PB_4, PA_8);
+//SRF05 ultra(PA_9, PC_7);
+float jarak;
+
+DigitalOut trigPin1(PA_9); // sensor tengah
+DigitalIn echoPin1(PC_7);
+DigitalIn ir(PB_9);
+int eksistensi_koin;
+int ada = 0;
+int tidak_ada = 1;
+int i;
+int kecepatan_state;
+
+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_12, PC_10, NC, 986); //roda depan kiri
+QEI BelakangKiriQEI(PB_7, PA_15, NC, 986); //roda belakang kiri
+
+//Tuning Di alas
+//PID DepanKananPID(0.40, 0.020, 0.00001, 0.02); //0000001 Kc, Ti, Td MOTOR baru nomor 1
+//PID BelakangKananPID(0.37, 0.018, 0.0000000001, 0.02); //Kc, Ti, Td MOTOR baru nomor 2
+//PID BelakangKiriPID(0.496, 0.0565, 0.0000000001, 0.02); //Kc, Ti, Td MOTOR baru nomor 3
+//PID DepanKiriPID(0.85, 0.08, 0.000000000001, 0.02); //Kc, Ti, Td MOTOR baru nomor 4
+
+//Tuning Di Lantai
+//PID DepanKananPID(0.5, 0.020, 0.00001, 0.02); //0000001 Kc, Ti, Td MOTOR baru nomor 1
+//PID BelakangKananPID(0.5, 0.022, 0.00000000001, 0.02);
+//PID BelakangKiriPID(0.496, 0.0565, 0.0000000001, 0.02); //Kc, Ti, Td MOTOR baru nomor 3
+//PID DepanKiriPID(0.85, 0.08, 0.000000000001, 0.02); //Kc, Ti, Td MOTOR baru nomor 4
+
+PID DepanKananPID(0.8, 0.1, 0.00000001, 0.02); //0000001 Kc, Ti, Td MOTOR baru nomor 1
+PID BelakangKananPID(0.78, 0.1, 0.0000001, 0.02);
+PID BelakangKiriPID(0.78, 0.1, 0.000000001, 0.02); //Kc, Ti, Td MOTOR baru nomor 3
+PID DepanKiriPID(0.87, 0.1, 0.000001, 0.02); //Kc, Ti, Td MOTOR baru nomor 4
+
+
+
+Motor DepanKanan(PC_6, PA_11, PB_12);
+Motor DepanKiri(PA_0, PA_4, PB_0);
+Motor BelakangKanan(PC_8, PC_5, PA_12);
+Motor BelakangKiri(PA_1, PC_1, PC_0);
+
+PwmOut Servo(PA_7);
+PwmOut Servo2(PB_10);
+
+Timer t_jalan;
+Timer t2;
+Timer t;
+Timer t_capit;
+
+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 Rotasi_Kanan = 7;
+int Rotasi_Kiri = 8;
+int Belok_Kanan = 9;
+int Belok_Kiri = 10;
+int Muter_Kiri = 11;
+int Muter_Kanan =12;
+int Capit_Buka = 13;
+int Capit_Muter = 14;
+int Cari_Koin1 = 15;
+int Cari_Koin2 = 16;
+int Mundur_Belok_Kanan = 17;
+int Mundur_Belok_Kiri = 18;
+int Ganti_Kecepatan = 19;
+
+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;
+float koordinat_x;
+float 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;
+int orientasi_2;
+int target_orientasi;
+float waktu;
+int BEARING_Register;
+char bits[2];
+int _byteHigh;
+int _byteLow;
+float bearing;
+
+float target_depan_kiri;
+float target_depan_kanan;
+float target_belakang_kiri;
+float target_belakang_kanan;
+float arah_depan_kiri;
+float arah_depan_kanan;
+float arah_belakang_kiri;
+float arah_belakang_kanan;
+
+float temp_x_DepanKiri;
+float temp_x_DepanKanan;
+float temp_x_BelakangKanan;
+float temp_x_BelakangKiri;
+float temp_y_DepanKiri;
+float temp_y_DepanKanan;
+float temp_y_BelakangKanan;
+float temp_y_BelakangKiri;
+float temp_x;
+float temp_y;
+float terhadap_x;
+float terhadap_y;
+int koordinat_x_bulat;
+int koordinat_y_bulat;
+float target_x_basis;
+float target_y_basis;
+float sudut_awal;
+int capit_state;
+int capit2_state;
+
+void fungsi_bluetooth(void)
+{
+ if(bluetooth.readable())
+ {
+ setpoint_x = bluetooth.getc();
+ setpoint_y = bluetooth.getc();
+ int target_sudut = 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 =='K')
+ {
+ huruf = 'K';
+ Jalan = Belok_Kiri;
+ }
+ else if(nilai =='A')
+ {
+ huruf = 'A';
+ Jalan = Belok_Kanan;
+ }
+ else if(nilai =='R')
+ {
+ huruf = 'R';
+ Jalan = Jalan_Kanan;
+ }
+ else if(nilai =='Q')
+ {
+ huruf = 'Q';
+ Jalan = Muter_Kiri;
+ }
+ else if(nilai =='E')
+ {
+ huruf = 'E';
+ Jalan = Muter_Kanan;
+ }
+ else if(nilai =='M')
+ {
+ huruf = 'M';
+ Jalan = Rotasi_Kanan;
+ }
+ else if(nilai =='W')
+ {
+ huruf = 'W';
+ Jalan = Rotasi_Kiri;
+ }
+ else if(nilai == 'U')
+ {
+ huruf = 'U';
+ Jalan = Mundur_Belok_Kiri;
+ }
+ else if(nilai == 'V')
+ {
+ huruf = 'V';
+ Jalan = Mundur_Belok_Kanan;
+ }
+ else if(nilai =='T')
+ {
+ huruf = 'T';
+ Jalan = Capit_Buka;
+ }
+ else if(nilai =='O')
+ {
+ huruf = 'O';
+ Jalan = Capit_Muter;
+ }
+ else if(nilai =='J')
+ {
+ huruf = 'J';
+ Jalan = Cari_Koin1;
+ //Jalan = ke_setpoint;
+ }
+ else if(nilai =='H')
+ {
+ huruf = 'H';
+ Jalan = Cari_Koin2;
+ //Jalan = ke_setpoint;
+ }
+
+ else if(nilai =='Y')
+ {
+ huruf = 'G';
+ Jalan = ke_setpoint;
+ }
+ else if(nilai == 'G')
+ {
+ huruf = 'G';
+ Jalan = mode_inverse;
+ }
+ else if(nilai == 'X')
+ {
+ huruf = 'X';
+ Jalan = Ganti_Kecepatan;
+ }
+ else if(nilai == 'S')
+ {
+ Jalan = Berhenti;
+ }
+ else if(nilai == 'p')
+ {
+ NVIC_SystemReset();
+ }
+ }
+}
+
+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));
+}
+
+float Get_Yaw()
+{
+ CMPS.unlock();
+ CMPS.start();
+ // to indicate an i2c read, shift the 7 bit address up 1 bit and set bit 0 to a 1
+ CMPS.write(i2c_address << 1);
+ int writeResult = CMPS.write(BEARING_Register);
+ if(writeResult != 1)
+ {
+ pc.printf("%d\n", writeResult);
+ return bearing;
+ }
+ else
+ {
+ CMPS.stop();
+ CMPS.read(i2c_address <<1, bits, 2);
+ _byteHigh = bits[0];
+ _byteLow = bits[1];
+ bearing = ((_byteHigh<<=8) + _byteLow) / 10;
+ return bearing;
+ }
+}
+
+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;
+
+ temp_y_DepanKiri = ((double)d_pulse_DepanKiri/(double)Resolusi)*pi*diameter*cos(45.0f*rtd);
+ temp_y_DepanKanan = ((double)d_pulse_DepanKanan/(double)Resolusi)*pi*diameter*cos(135.0f*rtd);
+ temp_y_BelakangKanan = ((double)d_pulse_BelakangKanan/(double)Resolusi)*pi*diameter*cos(225.0f*rtd);
+ temp_y_BelakangKiri = ((double)d_pulse_BelakangKiri/(double)Resolusi)*pi*diameter*cos(315.0f*rtd);
+
+ temp_x_DepanKiri = ((double)d_pulse_DepanKiri/(double)Resolusi)*pi*diameter*sin(45.0f*rtd);
+ temp_x_DepanKanan = ((double)d_pulse_DepanKanan/(double)Resolusi)*pi*diameter*sin(135.0f*rtd);
+ temp_x_BelakangKanan = ((double)d_pulse_BelakangKanan/(double)Resolusi)*pi*diameter*sin(225.0f*rtd);
+ temp_x_BelakangKiri = ((double)d_pulse_BelakangKiri/(double)Resolusi)*pi*diameter*sin(315.0f*rtd);
+
+ temp_x = (temp_x_DepanKiri + temp_x_DepanKanan + temp_x_BelakangKiri + temp_x_BelakangKanan)/2;
+ temp_y = (temp_y_DepanKiri + temp_y_DepanKanan + temp_y_BelakangKiri + temp_y_BelakangKanan)/2;
+
+ terhadap_x = -1*(double)temp_y*sin(orientasi*rtd) + (double)temp_x*cos(orientasi*rtd);
+ terhadap_y = (double)temp_y*cos(orientasi*rtd) + (double)temp_x*sin(orientasi*rtd);
+}
+
+void Cari_Koordinat()
+{
+ orientasi = -1.0f * ((double)Get_Yaw()- sudut_awal);
+ orientasi_2 = orientasi;
+
+ koordinat_y = koordinat_y + terhadap_y;
+ koordinat_x = koordinat_x + terhadap_x;
+ koordinat_x_bulat = koordinat_x;
+ koordinat_y_bulat = koordinat_y;
+}
+
+float cari_abs(float a)
+{
+ if(a<0)
+ {
+ if(fabs(a)<20)
+ {
+ return 0.0f;
+ }
+ else if(fabs(a)>=20)
+ {
+ return -1.0f;
+ }
+ }
+ else if(a>0)
+ {
+ if(fabs(a)<20)
+ {
+ return 0.0f;
+ }
+ else if(fabs(a)>=20)
+ {
+ return 1.0f;
+ }
+ }
+ else
+ {
+ return 0.0f;
+ }
+}
+
+void cari_inverse(float setpoint_sudut, float target_x, float target_y)
+{
+ //mengganti basis vektor berdasarkan sudut(rotasi axis)
+ setpoint_sudut = setpoint_sudut*rtd;
+ target_x = target_x - koordinat_x;
+ target_y = target_y - koordinat_y;
+ target_x_basis = target_x*cos(setpoint_sudut*-1.0f) - target_y*sin(setpoint_sudut*-1.0f);
+ target_y_basis = target_x*sin(setpoint_sudut*-1.0f) + target_y*cos(setpoint_sudut*-1.0f);
+ target_x = target_x_basis;
+ target_y = target_y_basis;
+ //meengganti satuan target
+ float target_vx;
+ float target_vy;
+ target_x = (double)target_x/100.0f;
+ target_y = (double)target_y/100.0f;
+ //mencari target kecepatan translasi dari robot
+ //maks kecepatan translasi per aksis 0.6 m/s
+ 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;
+ }
+ 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;
+ }
+ 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;
+ }
+ 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;
+ }
+ 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(fabs(target_x)==fabs(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;
+ }
+ }
+ //mencari kecepatan sudut tiap roda berdasarkan target kecepatan translasi
+ target_depan_kiri = (1/radius)*(sin(45.0f*rtd)*target_vx + cos(45.0f*rtd)*target_vy);
+ target_depan_kanan = (1/radius)*(sin(135.0f*rtd)*target_vx + cos(135.0f*rtd)*target_vy);
+ target_belakang_kanan = (1/radius)*(sin(225.0f*rtd)*target_vx + cos(225.0f*rtd)*target_vy);
+ target_belakang_kiri = (1/radius)*(sin(315.0f*rtd)*target_vx + cos(315.0f*rtd)*target_vy);
+ 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);
+ //mengambil arah gerak motor
+ 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()
+{
+ //memasukkan nilai target kecepatan sudut untuk setpoint PID
+ if(arah_depan_kanan!=0)
+ {
+ if(arah_depan_kiri==0)
+ {
+ DepanKananPID.setSetPoint(fabs(target_depan_kanan));
+ BelakangKiriPID.setSetPoint(fabs(target_belakang_kiri));
+ BelakangKananPID.setSetPoint(0);
+ DepanKiriPID.setSetPoint(0);
+ }
+ else if(arah_depan_kiri!=0)
+ {
+ DepanKananPID.setSetPoint(fabs(target_depan_kanan));
+ BelakangKiriPID.setSetPoint(fabs(target_belakang_kiri));
+ BelakangKananPID.setSetPoint(fabs(target_belakang_kanan));
+ DepanKiriPID.setSetPoint(fabs(target_depan_kiri));
+ }
+ }
+ else if(arah_depan_kiri!=0)
+ {
+ if(arah_depan_kanan==0)
+ {
+ DepanKananPID.setSetPoint(0);
+ BelakangKiriPID.setSetPoint(0);
+ BelakangKananPID.setSetPoint(fabs(target_belakang_kanan));
+ DepanKiriPID.setSetPoint(fabs(target_depan_kiri));
+ }
+ else if(arah_depan_kanan!=0)
+ {
+ DepanKananPID.setSetPoint(fabs(target_depan_kanan));
+ BelakangKiriPID.setSetPoint(fabs(target_belakang_kiri));
+ BelakangKananPID.setSetPoint(fabs(target_belakang_kanan));
+ DepanKiriPID.setSetPoint(fabs(target_depan_kiri));
+ }
+ }
+}
+
+void jalan_maju()
+{
+ t_jalan.reset();
+ t_jalan.start();
+ proses_kecepatan();
+ Get_Count();
+ Cari_Koordinat();
+ pc.printf("Jarak : %d\n", jarak);
+ //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
+ 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_bulat);
+ pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
+ 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_bulat);
+ pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
+ 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_bulat);
+ pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
+ 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 jalan_rotasi_kanan()
+{
+ t_jalan.reset();
+ t_jalan.start();
+ proses_kecepatan();
+ Get_Count();
+ Cari_Koordinat();
+ //pc.printf("Koordinat X : %d\n", koordinat_x_bulat);
+ //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
+ pc.printf("Orientasi : %d\n", orientasi_2);
+ 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();
+}
+
+void jalan_rotasi_kiri()
+{
+ t_jalan.reset();
+ t_jalan.start();
+ proses_kecepatan();
+ Get_Count();
+ Cari_Koordinat();
+ //pc.printf("Koordinat X : %d\n", koordinat_x_bulat);
+ //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
+ pc.printf("Orientasi : %d\n", orientasi_2);
+ 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();
+}
+
+void berhenti()
+{
+ t_jalan.reset();
+ t_jalan.start();
+ //pc.printf("Koordinat X : %d\n", koordinat_x_bulat);
+ //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
+ pc.printf("Orientasi : %d\n", orientasi_2);
+ 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_bulat<setpoint_x)
+ {
+ kosongkan();
+ while(koordinat_x_bulat<setpoint_x)
+ {
+ jalan_kanan();
+ }
+ DepanKanan.brake();
+ BelakangKanan.brake();
+ BelakangKiri.brake();
+ DepanKiri.brake();
+ }
+ else if(koordinat_x_bulat>setpoint_x)
+ {
+ kosongkan();
+ while(koordinat_x_bulat>setpoint_x)
+ {
+ jalan_kiri();
+ }
+ DepanKanan.brake();
+ BelakangKanan.brake();
+ BelakangKiri.brake();
+ DepanKiri.brake();
+ }
+ else if(koordinat_x_bulat==setpoint_x)
+ {
+ berhenti();
+ }
+}
+
+void ke_setpoint_y()
+{
+ if(koordinat_y_bulat<setpoint_y)
+ {
+ kosongkan();
+ while(koordinat_y_bulat<setpoint_y)
+ {
+ jalan_maju();
+ }
+ DepanKanan.brake();
+ BelakangKanan.brake();
+ BelakangKiri.brake();
+ DepanKiri.brake();
+ }
+ else if(koordinat_y_bulat>setpoint_y)
+ {
+ kosongkan();
+ while(koordinat_y>setpoint_y)
+ {
+ jalan_mundur();
+ }
+ DepanKanan.brake();
+ BelakangKanan.brake();
+ BelakangKiri.brake();
+ DepanKiri.brake();
+ }
+ else if(koordinat_y_bulat==setpoint_y)
+ {
+ berhenti();
+ }
+}
+
+void mode_cepat()
+{
+ target_depan_kanan = 1800.0f;
+ target_depan_kiri = 1800.0f;
+ target_belakang_kanan = 1800.0f;
+ target_belakang_kiri = 1800.0f;
+}
+
+void mode_lambat()
+{
+ target_depan_kanan = 1000.0f;
+ target_depan_kiri = 1000.0f;
+ target_belakang_kanan = 1000.0f;
+ target_belakang_kiri = 1000.0f;
+}
+
+
+void ke_target_orientasi()
+{
+ if(orientasi_2>target_orientasi)
+ {
+ kosongkan();
+ while(orientasi_2<target_orientasi)
+ {
+ jalan_rotasi_kiri();
+ }
+ DepanKanan.brake();
+ BelakangKanan.brake();
+ BelakangKiri.brake();
+ DepanKiri.brake();
+ }
+ else if(orientasi_2<target_orientasi)
+ {
+ kosongkan();
+ while(orientasi_2>target_orientasi)
+ {
+ jalan_rotasi_kanan();
+ }
+ DepanKanan.brake();
+ BelakangKanan.brake();
+ BelakangKiri.brake();
+ DepanKiri.brake();
+ }
+ else if(orientasi_2==target_orientasi)
+ {
+ berhenti();
+ }
+}
+
+void ke_target_orientasi2()
+{
+ while(orientasi_2<target_orientasi)
+ {
+ jalan_rotasi_kiri();
+ if(orientasi_2 == target_orientasi)
+ {
+ berhenti();
+ }
+ }
+ while(orientasi_2>target_orientasi)
+ {
+ jalan_rotasi_kanan();
+ if(orientasi_2 == target_orientasi)
+ {
+ berhenti();
+ }
+ }
+}
+
+void jalan_inverse()
+{
+ if((arah_depan_kiri==0)&&(arah_belakang_kanan==0)==1)
+ {
+ BelakangKiri.speed(BelakangKiriPID.compute()*arah_belakang_kiri);
+ DepanKanan.speed(DepanKananPID.compute()*arah_depan_kanan);
+ }
+ else if((arah_depan_kanan==0)&&(arah_belakang_kiri==0)==1)
+ {
+ BelakangKanan.speed(BelakangKiriPID.compute()*arah_belakang_kanan);
+ DepanKiri.speed(DepanKananPID.compute()*arah_depan_kiri);
+ }
+ else if(arah_depan_kiri!=0)
+ {
+ if(arah_depan_kanan!=0)
+ {
+ BelakangKiri.speed(BelakangKiriPID.compute()*arah_belakang_kiri);
+ DepanKanan.speed(DepanKananPID.compute()*arah_depan_kanan);
+ BelakangKanan.speed(BelakangKiriPID.compute()*arah_belakang_kanan);
+ DepanKiri.speed(DepanKananPID.compute()*arah_depan_kiri);
+ }
+ }
+}
+
+void cari_koin1()
+{
+ target_orientasi = 180;
+ ke_target_orientasi();
+ while(jarak>=23)
+ {
+ //jarak = ultra.read();
+ jalan_maju();
+ }
+ //while(jarak<=20)
+ //{
+ // jalan_maju();
+ //}
+ //while(eksistensi_koin!=0)
+ //{
+ // jalan_kanan();
+ //}
+}
+
+float sensor(){
+ float jarak2;
+ int durasi;
+ trigPin1=1;
+ t.reset();
+ wait_us(10);
+ trigPin1=0;
+ while (echoPin1 == 0){
+ }t.start();
+ while (echoPin1 == 1)
+ {
+ }
+ t.stop();
+ durasi = t.read_us();
+ jarak2 = (double)(durasi) / 29.0f /2.0f ; // 29 kecepatan suara di udara (2.9 detik untuk 1 kilometer), dibagi 2 karena bolak-balik
+ return jarak2;
+}
+
+void cari_koin2()
+{
+ while(orientasi != 180)
+ {
+ ke_target_orientasi();
+ }
+ while(jarak>=23)
+ {
+ //jarak = ultra.read();
+ jalan_maju();
+ }
+ //while(eksistensi_koin!=0)
+ //{
+ // jalan_kanan();
+ //}
+}
+
+int main()
+{
+ pc.baud(38400);
+ bluetooth.baud(9600);
+ bluetooth.attach(&fungsi_bluetooth, Serial::RxIrq);
+
+ BEARING_Register = 0x02;
+ 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);
+ sudut_awal = Get_Yaw();
+ while(1)
+ {
+
+ if(Jalan == Jalan_Maju)
+ {
+ DepanKananPID.setSetPoint(target_depan_kanan);
+ BelakangKananPID.setSetPoint(target_belakang_kanan);
+ BelakangKiriPID.setSetPoint(target_belakang_kiri);
+ DepanKiriPID.setSetPoint(target_depan_kiri);
+ 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_bulat);
+ //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
+ 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)
+ {
+ DepanKananPID.setSetPoint(target_depan_kanan);
+ BelakangKananPID.setSetPoint(target_belakang_kanan);
+ BelakangKiriPID.setSetPoint(target_belakang_kiri);
+ DepanKiriPID.setSetPoint(target_depan_kiri);
+ 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_bulat);
+ //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
+ 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_Kiri)
+ {
+ DepanKananPID.setSetPoint(target_depan_kanan);
+ BelakangKananPID.setSetPoint(target_belakang_kanan);
+ BelakangKiriPID.setSetPoint(target_belakang_kiri);
+ DepanKiriPID.setSetPoint(target_depan_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_bulat);
+ //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
+ pc.printf("Orientasi : %f\n", orientasi);
+ 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();
+ }
+ DepanKanan.brake();
+ BelakangKanan.brake();
+ BelakangKiri.brake();
+ DepanKiri.brake();
+ }
+ else if(Jalan == Jalan_Kanan)
+ {
+ DepanKananPID.setSetPoint(target_depan_kanan);
+ BelakangKananPID.setSetPoint(target_belakang_kanan);
+ BelakangKiriPID.setSetPoint(target_belakang_kiri);
+ DepanKiriPID.setSetPoint(target_depan_kiri);
+ 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_bulat);
+ //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
+ pc.printf("Orientasi : %f\n", orientasi);
+ 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();
+ }
+ DepanKanan.brake(1);
+ BelakangKanan.brake(1);
+ BelakangKiri.brake(1);
+ DepanKiri.brake(1);
+ }
+
+ else if(Jalan == Rotasi_Kanan)
+ {
+ DepanKananPID.setSetPoint(target_depan_kanan-800.0f);
+ BelakangKananPID.setSetPoint(target_belakang_kanan-800.0f);
+ BelakangKiriPID.setSetPoint(target_belakang_kiri-800.0f);
+ DepanKiriPID.setSetPoint(target_depan_kiri-800.0f);
+ kosongkan();
+ while(Jalan== Rotasi_Kanan)
+ {
+ t_jalan.reset();
+ t_jalan.start();
+ proses_kecepatan();
+ Get_Count();
+ Cari_Koordinat();
+ //pc.printf("Koordinat X : %d\n", koordinat_x_bulat);
+ //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
+ 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 == Rotasi_Kiri)
+ {
+ DepanKananPID.setSetPoint(target_depan_kanan-800.0f);
+ BelakangKananPID.setSetPoint(target_belakang_kanan-800.0f);
+ BelakangKiriPID.setSetPoint(target_belakang_kiri-800.0f);
+ DepanKiriPID.setSetPoint(target_depan_kiri-800.0f);
+ kosongkan();
+ while(Jalan== Rotasi_Kiri)
+ {
+ t_jalan.reset();
+ t_jalan.start();
+ proses_kecepatan();
+ Get_Count();
+ Cari_Koordinat();
+ //pc.printf("Koordinat X : %d\n", koordinat_x_bulat);
+ //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
+ 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 == Belok_Kanan)
+ {
+ DepanKananPID.setSetPoint(target_depan_kanan-1000.0f);
+ BelakangKananPID.setSetPoint(target_belakang_kanan-1000.0f);
+ BelakangKiriPID.setSetPoint(target_belakang_kiri);
+ DepanKiriPID.setSetPoint(target_depan_kiri);
+ kosongkan();
+ while(Jalan== Belok_Kanan)
+ {
+ t_jalan.reset();
+ t_jalan.start();
+ proses_kecepatan();
+ Get_Count();
+ Cari_Koordinat();
+ //pc.printf("Koordinat X : %d\n", koordinat_x_bulat);
+ //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
+ 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 == Mundur_Belok_Kanan)
+ {
+ DepanKananPID.setSetPoint(target_depan_kanan-1000.0f);
+ BelakangKananPID.setSetPoint(target_belakang_kanan-1000.0f);
+ BelakangKiriPID.setSetPoint(target_belakang_kiri);
+ DepanKiriPID.setSetPoint(target_depan_kiri);
+ kosongkan();
+ while(Jalan== Mundur_Belok_Kanan)
+ {
+ t_jalan.reset();
+ t_jalan.start();
+ proses_kecepatan();
+ Get_Count();
+ Cari_Koordinat();
+ //pc.printf("Koordinat X : %d\n", koordinat_x_bulat);
+ //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
+ 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 == Mundur_Belok_Kiri)
+ {
+ DepanKananPID.setSetPoint(target_depan_kanan);
+ BelakangKananPID.setSetPoint(target_belakang_kanan);
+ BelakangKiriPID.setSetPoint(target_belakang_kiri-1000.0f);
+ DepanKiriPID.setSetPoint(target_depan_kiri-1000.0f);
+ kosongkan();
+ while(Jalan== Mundur_Belok_Kiri)
+ {
+ t_jalan.reset();
+ t_jalan.start();
+ proses_kecepatan();
+ Get_Count();
+ Cari_Koordinat();
+ //pc.printf("Koordinat X : %d\n", koordinat_x_bulat);
+ //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
+ 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 == Belok_Kiri)
+ {
+ DepanKananPID.setSetPoint(target_depan_kanan);
+ BelakangKananPID.setSetPoint(target_belakang_kanan);
+ BelakangKiriPID.setSetPoint(target_belakang_kiri-1000);
+ DepanKiriPID.setSetPoint(target_depan_kiri-1000);
+ kosongkan();
+ while(Jalan== Belok_Kiri)
+ {
+ t_jalan.reset();
+ t_jalan.start();
+ proses_kecepatan();
+ Get_Count();
+ Cari_Koordinat();
+ //pc.printf("Koordinat X : %d\n", koordinat_x_bulat);
+ //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
+ 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 == Muter_Kanan)
+ {
+ //DepanKananPID.setSetPoint(1400);
+ BelakangKananPID.setSetPoint(1400);
+ //BelakangKiriPID.setSetPoint(1400);
+ DepanKiriPID.setSetPoint(1400);
+ kosongkan();
+ while(Jalan== Muter_Kanan)
+ {
+ t_jalan.reset();
+ t_jalan.start();
+ proses_kecepatan();
+ Get_Count();
+ Cari_Koordinat();
+ //pc.printf("Koordinat X : %d\n", koordinat_x_bulat);
+ //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
+ pc.printf("Orientasi : %f\n", orientasi);
+ 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();
+ }
+ DepanKanan.brake();
+ BelakangKanan.brake();
+ BelakangKiri.brake();
+ DepanKiri.brake();
+ }
+
+ else if(Jalan == Muter_Kiri)
+ {
+ DepanKananPID.setSetPoint(1400);
+ //BelakangKananPID.setSetPoint(1400);
+ BelakangKiriPID.setSetPoint(1400);
+ //DepanKiriPID.setSetPoint(1400);
+ kosongkan();
+ while(Jalan== Muter_Kiri)
+ {
+ t_jalan.reset();
+ t_jalan.start();
+ proses_kecepatan();
+ Get_Count();
+ Cari_Koordinat();
+ //pc.printf("Koordinat X : %d\n", koordinat_x_bulat);
+ //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
+ pc.printf("Orientasi : %f\n", orientasi);
+ //BelakangKanan.speed(BelakangKananPID.compute());
+ 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 == Ganti_Kecepatan)
+ {
+ if(kecepatan_state==0)
+ {
+ mode_lambat();
+ wait_ms(200);
+ kecepatan_state =1;
+ }
+ else if(kecepatan_state==1)
+ {
+ mode_cepat();
+ wait_ms(200);
+ kecepatan_state = 0;
+ }
+ }
+
+ 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();
+ ke_target_orientasi();
+ //}
+ }
+ else if(Jalan == mode_inverse)
+ {
+ kosongkan();
+ cari_inverse(orientasi, (float)setpoint_x, (float)setpoint_y);
+ set_inverse();
+ while(Jalan == mode_inverse)
+ {
+ if((koordinat_x_bulat!=setpoint_x)&&(koordinat_y_bulat!=setpoint_y))
+ {
+ t_jalan.reset();
+ t_jalan.start();
+ proses_kecepatan();
+ Get_Count();
+ Cari_Koordinat();
+ if(arah_depan_kanan!=0)
+ {
+ if(arah_depan_kiri==0)
+ {
+ BelakangKiri.speed(BelakangKiriPID.compute()*arah_belakang_kiri);
+ DepanKanan.speed(DepanKananPID.compute()*arah_depan_kanan);
+ BelakangKanan.brake(0);
+ DepanKiri.brake(0);
+ }
+ else if(arah_depan_kiri!=0)
+ {
+ BelakangKiri.speed(BelakangKiriPID.compute()*arah_belakang_kiri);
+ DepanKanan.speed(DepanKananPID.compute()*arah_depan_kanan);
+ BelakangKanan.speed(BelakangKananPID.compute()*arah_belakang_kanan);
+ DepanKiri.speed(DepanKiriPID.compute()*arah_depan_kiri);
+ }
+ }
+ else if(arah_depan_kiri!=0)
+ {
+ if(arah_depan_kanan==0)
+ {
+ BelakangKanan.speed(BelakangKananPID.compute()*arah_belakang_kanan);
+ DepanKiri.speed(DepanKiriPID.compute()*arah_depan_kiri);
+ BelakangKiri.brake(0);
+ DepanKanan.brake(0);
+ }
+ else if(arah_depan_kanan!=0)
+ {
+ BelakangKiri.speed(BelakangKiriPID.compute()*arah_belakang_kiri);
+ DepanKanan.speed(DepanKananPID.compute()*arah_depan_kanan);
+ BelakangKanan.speed(BelakangKananPID.compute()*arah_belakang_kanan);
+ DepanKiri.speed(DepanKiriPID.compute()*arah_depan_kiri);
+ }
+ }
+ pc.printf("Koordinat X : %d\n", koordinat_x_bulat);
+ pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
+ pc.printf("Orientasi : %d\n", orientasi_2);
+ //pc.printf("X basis : %f\n", target_x_basis);
+ //pc.printf("Y basis : %f\n", target_y_basis);
+ //pc.printf("Depan Kiri : %f\n", target_depan_kiri);
+ //pc.printf("Depan Kanan : %f\n", target_depan_kanan);
+ //pc.printf("Belakang Kanan : %f\n", target_belakang_kanan);
+ //pc.printf("Belakang Kiri : %f\n", target_belakang_kiri);
+ while(t_jalan.read_ms()<=20)
+ {
+ }
+ t_jalan.reset();
+ }
+ }
+ }
+ else if(Jalan == Capit_Buka)
+ {
+ if(capit_state==0)
+ {
+ Servo.pulsewidth_us(1230);
+ capit_state=1;
+ wait_ms(200);
+ }
+ else if(capit_state==1)
+ {
+ Servo.pulsewidth_us(2400);
+ capit_state=0;
+ wait_ms(200);
+ }
+ /*else if(capit_state==2)
+ {
+ Servo.pulsewidth_us(1500);
+ capit_state=0;
+ wait_ms(200);
+ }
+ */
+ }
+ else if(Jalan == Capit_Muter)
+ {
+ if(capit2_state==0)
+ {
+ for(i=1800; i>910; i--)
+ {
+ t_capit.reset();
+ t_capit.start();
+ Servo2.pulsewidth_us(i);
+ while(t_capit.read_ms()<1)
+ {
+ }
+ t_capit.stop();
+ }
+ capit2_state=1;
+ }
+ else if(capit2_state==1)
+ {
+ for(i=910; i<1800; i=i++)
+ {
+ t_capit.reset();
+ t_capit.start();
+ Servo2.pulsewidth_us(i);
+ while(t_capit.read_us()<600)
+ {
+ }
+ t_capit.stop();
+ }
+ capit2_state=0;
+ }
+ }
+ else if(Jalan == Cari_Koin1)
+ {
+ DepanKananPID.setSetPoint(1000);
+ BelakangKananPID.setSetPoint(1000);
+ BelakangKiriPID.setSetPoint(1000);
+ DepanKiriPID.setSetPoint(1000);
+ eksistensi_koin = ir.read();
+ jarak = sensor();
+ while(Jalan == Cari_Koin1)
+ {
+ //target_orientasi = 180;
+ //ke_target_orientasi2();
+ Servo.pulsewidth_us(2290);
+ wait_ms(100);
+ while(jarak>16.0f)
+ {
+ //jalan_maju();
+ jarak = sensor();
+ jalan_maju();
+ }
+ berhenti();
+ wait_ms(300);
+ while(eksistensi_koin== tidak_ada)
+ {
+ eksistensi_koin = ir.read();
+ jalan_kiri();
+ }
+ berhenti();
+ wait_ms(300);
+ Servo.pulsewidth_us(1190);
+ wait_ms(100);
+ t2.reset();
+ t2.start();
+ while(t2.read_ms()<1500)
+ {
+ jalan_mundur();
+ }
+ t2.stop();
+ //while(jarak<90)
+ //{
+ // jalan_mundur();
+ // jarak = sensor();
+ //}
+ //Servo2.pulsewidth_us(2200);
+ //target_orientasi = 0;
+ //ke_target_orientasi2();
+ }
+ }
+ else if(Jalan == Cari_Koin2)
+ {
+ while(Jalan == Cari_Koin2)
+ {
+ cari_koin2();
+ }
+ }
+ else if(Jalan == Berhenti)
+ {
+ kosongkan();
+ while(Jalan==Berhenti)
+ {
+ t_jalan.reset();
+ t_jalan.start();
+ proses_kecepatan();
+ Get_Count();
+ Cari_Koordinat();
+ //pc.printf("Koordinat X : %d\n", koordinat_x_bulat);
+ //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
+ pc.printf("Orientasi : %d\n", orientasi_2);
+ 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