Robot Sulistia, Full Kodingan Lomba KRTMI 2022
Dependencies: MPU9250 Motor PID QEI SRF05 mbed
PIDremote.cpp
- Committer:
- photonicbabe
- Date:
- 24 months ago
- Revision:
- 0:a2b784a0e760
File content as of revision 0:a2b784a0e760:
#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(); } } } }