Robot Sulistia, Full Kodingan Lomba KRTMI 2022
Dependencies: MPU9250 Motor PID QEI SRF05 mbed
Revision 0:a2b784a0e760, committed 2022-10-04
- Comitter:
- photonicbabe
- Date:
- Tue Oct 04 10:49:03 2022 +0000
- Commit message:
- Robot Sulistia, Kodingan Lomba KRTMI
Changed in this revision
diff -r 000000000000 -r a2b784a0e760 MPU9250.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPU9250.lib Tue Oct 04 10:49:03 2022 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/Edutech/code/MPU9250/#98a0cccbc509
diff -r 000000000000 -r a2b784a0e760 Motor.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Motor.lib Tue Oct 04 10:49:03 2022 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/simon/code/Motor/#f265e441bcd9
diff -r 000000000000 -r a2b784a0e760 PID.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.lib Tue Oct 04 10:49:03 2022 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/aberk/code/PID/#6e12a3e5af19
diff -r 000000000000 -r a2b784a0e760 PIDremote.cpp --- /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
diff -r 000000000000 -r a2b784a0e760 QEI.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Tue Oct 04 10:49:03 2022 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/aberk/code/QEI/#5c2ad81551aa
diff -r 000000000000 -r a2b784a0e760 SRF05.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SRF05.lib Tue Oct 04 10:49:03 2022 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/simon/code/SRF05/#e758665e072c
diff -r 000000000000 -r a2b784a0e760 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue Oct 04 10:49:03 2022 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file