KRTMI_Titanium2022
Robot Sulistia, Full Kodingan Lomba KRTMI 2022
Dependencies: MPU9250 Motor PID QEI SRF05 mbed
Embed:
(wiki syntax)
Show/hide line numbers
PIDremote.cpp
00001 #include "mbed.h" 00002 #include "Motor.h" 00003 #include "QEI.h" 00004 #include "PID.h" 00005 00006 #define i2c_address 0x60 00007 00008 Serial pc(USBTX, USBRX); 00009 Serial bluetooth(PB_6, PA_10); 00010 I2C CMPS(PB_4, PA_8); 00011 //SRF05 ultra(PA_9, PC_7); 00012 float jarak; 00013 00014 DigitalOut trigPin1(PA_9); // sensor tengah 00015 DigitalIn echoPin1(PC_7); 00016 DigitalIn ir(PB_9); 00017 int eksistensi_koin; 00018 int ada = 0; 00019 int tidak_ada = 1; 00020 int i; 00021 int kecepatan_state; 00022 00023 QEI DepanKananQEI(PB_8, PC_9, NC, 986); //roda depan kanan PB_1, PB_15, NC, 624 00024 QEI BelakangKananQEI(PB_1, PB_15, NC, 986); //roda belakang kiri PB_14, PB_13, NC, 624 00025 QEI DepanKiriQEI(PC_12, PC_10, NC, 986); //roda depan kiri 00026 QEI BelakangKiriQEI(PB_7, PA_15, NC, 986); //roda belakang kiri 00027 00028 //Tuning Di alas 00029 //PID DepanKananPID(0.40, 0.020, 0.00001, 0.02); //0000001 Kc, Ti, Td MOTOR baru nomor 1 00030 //PID BelakangKananPID(0.37, 0.018, 0.0000000001, 0.02); //Kc, Ti, Td MOTOR baru nomor 2 00031 //PID BelakangKiriPID(0.496, 0.0565, 0.0000000001, 0.02); //Kc, Ti, Td MOTOR baru nomor 3 00032 //PID DepanKiriPID(0.85, 0.08, 0.000000000001, 0.02); //Kc, Ti, Td MOTOR baru nomor 4 00033 00034 //Tuning Di Lantai 00035 //PID DepanKananPID(0.5, 0.020, 0.00001, 0.02); //0000001 Kc, Ti, Td MOTOR baru nomor 1 00036 //PID BelakangKananPID(0.5, 0.022, 0.00000000001, 0.02); 00037 //PID BelakangKiriPID(0.496, 0.0565, 0.0000000001, 0.02); //Kc, Ti, Td MOTOR baru nomor 3 00038 //PID DepanKiriPID(0.85, 0.08, 0.000000000001, 0.02); //Kc, Ti, Td MOTOR baru nomor 4 00039 00040 PID DepanKananPID(0.8, 0.1, 0.00000001, 0.02); //0000001 Kc, Ti, Td MOTOR baru nomor 1 00041 PID BelakangKananPID(0.78, 0.1, 0.0000001, 0.02); 00042 PID BelakangKiriPID(0.78, 0.1, 0.000000001, 0.02); //Kc, Ti, Td MOTOR baru nomor 3 00043 PID DepanKiriPID(0.87, 0.1, 0.000001, 0.02); //Kc, Ti, Td MOTOR baru nomor 4 00044 00045 00046 00047 Motor DepanKanan(PC_6, PA_11, PB_12); 00048 Motor DepanKiri(PA_0, PA_4, PB_0); 00049 Motor BelakangKanan(PC_8, PC_5, PA_12); 00050 Motor BelakangKiri(PA_1, PC_1, PC_0); 00051 00052 PwmOut Servo(PA_7); 00053 PwmOut Servo2(PB_10); 00054 00055 Timer t_jalan; 00056 Timer t2; 00057 Timer t; 00058 Timer t_capit; 00059 00060 char nilai, huruf; 00061 int Jalan; 00062 int Jalan_Maju =1; 00063 int Berhenti = 0; 00064 int Jalan_Mundur = 2; 00065 int Jalan_Kanan = 3; 00066 int Jalan_Kiri = 4; 00067 int ke_setpoint = 5; 00068 int mode_inverse = 6; 00069 int Rotasi_Kanan = 7; 00070 int Rotasi_Kiri = 8; 00071 int Belok_Kanan = 9; 00072 int Belok_Kiri = 10; 00073 int Muter_Kiri = 11; 00074 int Muter_Kanan =12; 00075 int Capit_Buka = 13; 00076 int Capit_Muter = 14; 00077 int Cari_Koin1 = 15; 00078 int Cari_Koin2 = 16; 00079 int Mundur_Belok_Kanan = 17; 00080 int Mundur_Belok_Kiri = 18; 00081 int Ganti_Kecepatan = 19; 00082 00083 int DepanKananPulses = 0; 00084 int DepanKananPrevPulses = 0; 00085 float DepanKananVelocity = 0.0; 00086 00087 int BelakangKananPulses = 0; 00088 int BelakangKananPrevPulses = 0; 00089 float BelakangKananVelocity = 0.0; 00090 00091 int BelakangKiriPulses = 0; 00092 int BelakangKiriPrevPulses = 0; 00093 float BelakangKiriVelocity = 0.0; 00094 00095 int DepanKiriPulses = 0; 00096 int DepanKiriPrevPulses = 0; 00097 float DepanKiriVelocity = 0.0; 00098 00099 int Depan_Kanan_Count; 00100 int Depan_Kiri_Count; 00101 int Belakang_Kanan_Count; 00102 int Belakang_Kiri_Count; 00103 00104 int d_pulse_DepanKanan; 00105 int d_pulse_DepanKiri; 00106 int d_pulse_BelakangKanan; 00107 int d_pulse_BelakangKiri; 00108 00109 int koordinat_x_DepanKiri; 00110 int koordinat_x_DepanKanan; 00111 int koordinat_x_BelakangKiri; 00112 int koordinat_x_BelakangKanan; 00113 int koordinat_y_DepanKiri; 00114 int koordinat_y_DepanKanan; 00115 int koordinat_y_BelakangKiri; 00116 int koordinat_y_BelakangKanan; 00117 float koordinat_x; 00118 float koordinat_y; 00119 int Resolusi = 986; 00120 float pi = 3.141; 00121 float diameter = 5.8f; 00122 float rtd = pi/180.0f; 00123 int setpoint_x; 00124 int setpoint_y; 00125 float radius = 0.029; 00126 float orientasi; 00127 int orientasi_2; 00128 int target_orientasi; 00129 float waktu; 00130 int BEARING_Register; 00131 char bits[2]; 00132 int _byteHigh; 00133 int _byteLow; 00134 float bearing; 00135 00136 float target_depan_kiri; 00137 float target_depan_kanan; 00138 float target_belakang_kiri; 00139 float target_belakang_kanan; 00140 float arah_depan_kiri; 00141 float arah_depan_kanan; 00142 float arah_belakang_kiri; 00143 float arah_belakang_kanan; 00144 00145 float temp_x_DepanKiri; 00146 float temp_x_DepanKanan; 00147 float temp_x_BelakangKanan; 00148 float temp_x_BelakangKiri; 00149 float temp_y_DepanKiri; 00150 float temp_y_DepanKanan; 00151 float temp_y_BelakangKanan; 00152 float temp_y_BelakangKiri; 00153 float temp_x; 00154 float temp_y; 00155 float terhadap_x; 00156 float terhadap_y; 00157 int koordinat_x_bulat; 00158 int koordinat_y_bulat; 00159 float target_x_basis; 00160 float target_y_basis; 00161 float sudut_awal; 00162 int capit_state; 00163 int capit2_state; 00164 00165 void fungsi_bluetooth(void) 00166 { 00167 if(bluetooth.readable()) 00168 { 00169 setpoint_x = bluetooth.getc(); 00170 setpoint_y = bluetooth.getc(); 00171 int target_sudut = bluetooth.getc(); 00172 nilai = bluetooth.getc(); 00173 if (nilai == 'S') 00174 { 00175 huruf = 'S'; 00176 Jalan = Berhenti; 00177 } 00178 else if (nilai == 'F') 00179 { 00180 huruf = 'F'; 00181 Jalan = Jalan_Maju; 00182 } 00183 else if(nilai =='B') 00184 { 00185 huruf = 'B'; 00186 Jalan = Jalan_Mundur; 00187 } 00188 else if(nilai =='L') 00189 { 00190 huruf = 'L'; 00191 Jalan = Jalan_Kiri; 00192 } 00193 else if(nilai =='K') 00194 { 00195 huruf = 'K'; 00196 Jalan = Belok_Kiri; 00197 } 00198 else if(nilai =='A') 00199 { 00200 huruf = 'A'; 00201 Jalan = Belok_Kanan; 00202 } 00203 else if(nilai =='R') 00204 { 00205 huruf = 'R'; 00206 Jalan = Jalan_Kanan; 00207 } 00208 else if(nilai =='Q') 00209 { 00210 huruf = 'Q'; 00211 Jalan = Muter_Kiri; 00212 } 00213 else if(nilai =='E') 00214 { 00215 huruf = 'E'; 00216 Jalan = Muter_Kanan; 00217 } 00218 else if(nilai =='M') 00219 { 00220 huruf = 'M'; 00221 Jalan = Rotasi_Kanan; 00222 } 00223 else if(nilai =='W') 00224 { 00225 huruf = 'W'; 00226 Jalan = Rotasi_Kiri; 00227 } 00228 else if(nilai == 'U') 00229 { 00230 huruf = 'U'; 00231 Jalan = Mundur_Belok_Kiri; 00232 } 00233 else if(nilai == 'V') 00234 { 00235 huruf = 'V'; 00236 Jalan = Mundur_Belok_Kanan; 00237 } 00238 else if(nilai =='T') 00239 { 00240 huruf = 'T'; 00241 Jalan = Capit_Buka; 00242 } 00243 else if(nilai =='O') 00244 { 00245 huruf = 'O'; 00246 Jalan = Capit_Muter; 00247 } 00248 else if(nilai =='J') 00249 { 00250 huruf = 'J'; 00251 Jalan = Cari_Koin1; 00252 //Jalan = ke_setpoint; 00253 } 00254 else if(nilai =='H') 00255 { 00256 huruf = 'H'; 00257 Jalan = Cari_Koin2; 00258 //Jalan = ke_setpoint; 00259 } 00260 00261 else if(nilai =='Y') 00262 { 00263 huruf = 'G'; 00264 Jalan = ke_setpoint; 00265 } 00266 else if(nilai == 'G') 00267 { 00268 huruf = 'G'; 00269 Jalan = mode_inverse; 00270 } 00271 else if(nilai == 'X') 00272 { 00273 huruf = 'X'; 00274 Jalan = Ganti_Kecepatan; 00275 } 00276 else if(nilai == 'S') 00277 { 00278 Jalan = Berhenti; 00279 } 00280 else if(nilai == 'p') 00281 { 00282 NVIC_SystemReset(); 00283 } 00284 } 00285 } 00286 00287 void kosongkan() 00288 { 00289 BelakangKananQEI.reset(); 00290 DepanKananQEI.reset(); 00291 DepanKiriQEI.reset(); 00292 BelakangKiriQEI.reset(); 00293 00294 DepanKananPulses = 0; 00295 DepanKananPrevPulses = 0; 00296 DepanKananVelocity = 0.0; 00297 00298 BelakangKananPulses = 0; 00299 BelakangKananPrevPulses = 0; 00300 BelakangKananVelocity = 0.0; 00301 00302 BelakangKiriPulses = 0; 00303 BelakangKiriPrevPulses = 0; 00304 BelakangKiriVelocity = 0.0; 00305 00306 DepanKiriPulses = 0; 00307 DepanKiriPrevPulses = 0; 00308 DepanKiriVelocity = 0.0; 00309 } 00310 00311 void proses_kecepatan() 00312 { 00313 DepanKananPulses = DepanKananQEI.getPulses(); 00314 d_pulse_DepanKanan = DepanKananPulses - DepanKananPrevPulses; 00315 DepanKananVelocity = (DepanKananPulses - DepanKananPrevPulses) / 0.02; 00316 DepanKananPrevPulses = DepanKananPulses; 00317 DepanKananPID.setProcessValue(fabs(DepanKananVelocity)); 00318 00319 DepanKiriPulses = DepanKiriQEI.getPulses(); 00320 d_pulse_DepanKiri = DepanKiriPulses - DepanKiriPrevPulses; 00321 DepanKiriVelocity = (DepanKiriPulses - DepanKiriPrevPulses) / 0.02; 00322 DepanKiriPrevPulses = DepanKiriPulses; 00323 DepanKiriPID.setProcessValue(fabs(DepanKiriVelocity)); 00324 00325 BelakangKananPulses = BelakangKananQEI.getPulses(); 00326 d_pulse_BelakangKanan = BelakangKananPulses - BelakangKananPrevPulses; 00327 BelakangKananVelocity = (BelakangKananPulses - BelakangKananPrevPulses) / 0.02; 00328 BelakangKananPrevPulses = BelakangKananPulses; 00329 BelakangKananPID.setProcessValue(fabs(BelakangKananVelocity)); 00330 00331 BelakangKiriPulses = BelakangKiriQEI.getPulses(); 00332 d_pulse_BelakangKiri = BelakangKiriPulses - BelakangKiriPrevPulses; 00333 BelakangKiriVelocity = (BelakangKiriPulses - BelakangKiriPrevPulses) / 0.02; 00334 BelakangKiriPrevPulses = BelakangKiriPulses; 00335 BelakangKiriPID.setProcessValue(fabs(BelakangKiriVelocity)); 00336 } 00337 00338 float Get_Yaw() 00339 { 00340 CMPS.unlock(); 00341 CMPS.start(); 00342 // to indicate an i2c read, shift the 7 bit address up 1 bit and set bit 0 to a 1 00343 CMPS.write(i2c_address << 1); 00344 int writeResult = CMPS.write(BEARING_Register); 00345 if(writeResult != 1) 00346 { 00347 pc.printf("%d\n", writeResult); 00348 return bearing; 00349 } 00350 else 00351 { 00352 CMPS.stop(); 00353 CMPS.read(i2c_address <<1, bits, 2); 00354 _byteHigh = bits[0]; 00355 _byteLow = bits[1]; 00356 bearing = ((_byteHigh<<=8) + _byteLow) / 10; 00357 return bearing; 00358 } 00359 } 00360 00361 void Get_Count() 00362 { 00363 Depan_Kanan_Count = Depan_Kanan_Count + d_pulse_DepanKanan; 00364 Depan_Kiri_Count = Depan_Kiri_Count + d_pulse_DepanKiri; 00365 Belakang_Kanan_Count = Belakang_Kanan_Count + d_pulse_BelakangKanan; 00366 Belakang_Kiri_Count = Belakang_Kiri_Count + d_pulse_BelakangKiri; 00367 00368 temp_y_DepanKiri = ((double)d_pulse_DepanKiri/(double)Resolusi)*pi*diameter*cos(45.0f*rtd); 00369 temp_y_DepanKanan = ((double)d_pulse_DepanKanan/(double)Resolusi)*pi*diameter*cos(135.0f*rtd); 00370 temp_y_BelakangKanan = ((double)d_pulse_BelakangKanan/(double)Resolusi)*pi*diameter*cos(225.0f*rtd); 00371 temp_y_BelakangKiri = ((double)d_pulse_BelakangKiri/(double)Resolusi)*pi*diameter*cos(315.0f*rtd); 00372 00373 temp_x_DepanKiri = ((double)d_pulse_DepanKiri/(double)Resolusi)*pi*diameter*sin(45.0f*rtd); 00374 temp_x_DepanKanan = ((double)d_pulse_DepanKanan/(double)Resolusi)*pi*diameter*sin(135.0f*rtd); 00375 temp_x_BelakangKanan = ((double)d_pulse_BelakangKanan/(double)Resolusi)*pi*diameter*sin(225.0f*rtd); 00376 temp_x_BelakangKiri = ((double)d_pulse_BelakangKiri/(double)Resolusi)*pi*diameter*sin(315.0f*rtd); 00377 00378 temp_x = (temp_x_DepanKiri + temp_x_DepanKanan + temp_x_BelakangKiri + temp_x_BelakangKanan)/2; 00379 temp_y = (temp_y_DepanKiri + temp_y_DepanKanan + temp_y_BelakangKiri + temp_y_BelakangKanan)/2; 00380 00381 terhadap_x = -1*(double)temp_y*sin(orientasi*rtd) + (double)temp_x*cos(orientasi*rtd); 00382 terhadap_y = (double)temp_y*cos(orientasi*rtd) + (double)temp_x*sin(orientasi*rtd); 00383 } 00384 00385 void Cari_Koordinat() 00386 { 00387 orientasi = -1.0f * ((double)Get_Yaw()- sudut_awal); 00388 orientasi_2 = orientasi; 00389 00390 koordinat_y = koordinat_y + terhadap_y; 00391 koordinat_x = koordinat_x + terhadap_x; 00392 koordinat_x_bulat = koordinat_x; 00393 koordinat_y_bulat = koordinat_y; 00394 } 00395 00396 float cari_abs(float a) 00397 { 00398 if(a<0) 00399 { 00400 if(fabs(a)<20) 00401 { 00402 return 0.0f; 00403 } 00404 else if(fabs(a)>=20) 00405 { 00406 return -1.0f; 00407 } 00408 } 00409 else if(a>0) 00410 { 00411 if(fabs(a)<20) 00412 { 00413 return 0.0f; 00414 } 00415 else if(fabs(a)>=20) 00416 { 00417 return 1.0f; 00418 } 00419 } 00420 else 00421 { 00422 return 0.0f; 00423 } 00424 } 00425 00426 void cari_inverse(float setpoint_sudut, float target_x, float target_y) 00427 { 00428 //mengganti basis vektor berdasarkan sudut(rotasi axis) 00429 setpoint_sudut = setpoint_sudut*rtd; 00430 target_x = target_x - koordinat_x; 00431 target_y = target_y - koordinat_y; 00432 target_x_basis = target_x*cos(setpoint_sudut*-1.0f) - target_y*sin(setpoint_sudut*-1.0f); 00433 target_y_basis = target_x*sin(setpoint_sudut*-1.0f) + target_y*cos(setpoint_sudut*-1.0f); 00434 target_x = target_x_basis; 00435 target_y = target_y_basis; 00436 //meengganti satuan target 00437 float target_vx; 00438 float target_vy; 00439 target_x = (double)target_x/100.0f; 00440 target_y = (double)target_y/100.0f; 00441 //mencari target kecepatan translasi dari robot 00442 //maks kecepatan translasi per aksis 0.6 m/s 00443 if(fabs(target_x)>fabs(target_y)) 00444 { 00445 if((target_x<0)&&(target_y<0)) 00446 { 00447 target_vx = -0.6; 00448 target_vy = ((double)target_y/(double)target_x)*target_vx; 00449 } 00450 else if((target_x<0)&&(target_y>0)) 00451 { 00452 target_vx = -0.6; 00453 target_vy = ((double)target_y/(double)target_x)*target_vx; 00454 } 00455 else if((target_x>0)&&(target_y>0)) 00456 { 00457 target_vx = 0.6; 00458 target_vy = ((double)target_y/(double)target_x)*target_vx; 00459 } 00460 else if((target_x>0)&&(target_y<0)) 00461 { 00462 target_vx = 0.6; 00463 target_vy = ((double)target_y/(double)target_x)*target_vx; 00464 } 00465 else if((target_y==0)&&(target_x>0)) 00466 { 00467 target_vx = 0.6; 00468 target_vy = 0; 00469 } 00470 else if((target_y==0)&&(target_x<0)) 00471 { 00472 target_vx = -0.6; 00473 target_vy = 0; 00474 } 00475 } 00476 else if(fabs(target_x)<fabs(target_y)) 00477 { 00478 if((target_x<0)&&(target_y<0)) 00479 { 00480 target_vy = -0.6; 00481 target_vx = ((double)target_x/(double)target_y)*target_vy; 00482 } 00483 else if((target_x<0)&&(target_y>0)) 00484 { 00485 target_vy = 0.6; 00486 target_vx = ((double)target_x/(double)target_y)*target_vy; 00487 } 00488 else if((target_x>0)&&(target_y>0)) 00489 { 00490 target_vy = 0.6; 00491 target_vx = ((double)target_x/(double)target_y)*target_vy; 00492 } 00493 else if((target_x>0)&&(target_y<0)) 00494 { 00495 target_vy = -0.6; 00496 target_vx = ((double)target_x/(double)target_y)*target_vy; 00497 } 00498 else if((target_x==0)&&(target_y>0)) 00499 { 00500 target_vy = 0.6; 00501 target_vx = 0; 00502 } 00503 else if((target_x==0)&&(target_y<0)) 00504 { 00505 target_vy = -0.6; 00506 target_vx = 0; 00507 } 00508 } 00509 else if(fabs(target_x)==fabs(target_y)) 00510 { 00511 if((target_x<0)&&(target_y<0)) 00512 { 00513 target_vy = -0.6; 00514 target_vx = target_vy; 00515 } 00516 else if((target_x<0)&&(target_y>0)) 00517 { 00518 target_vy = 0.6; 00519 target_vx = -1*target_vy; 00520 } 00521 else if((target_x>0)&&(target_y<0)) 00522 { 00523 target_vy = -0.6; 00524 target_vx = -1*target_vy; 00525 } 00526 else if((target_x>0)&&(target_y>0)) 00527 { 00528 target_vy = 0.6; 00529 target_vx = target_vy; 00530 } 00531 } 00532 //mencari kecepatan sudut tiap roda berdasarkan target kecepatan translasi 00533 target_depan_kiri = (1/radius)*(sin(45.0f*rtd)*target_vx + cos(45.0f*rtd)*target_vy); 00534 target_depan_kanan = (1/radius)*(sin(135.0f*rtd)*target_vx + cos(135.0f*rtd)*target_vy); 00535 target_belakang_kanan = (1/radius)*(sin(225.0f*rtd)*target_vx + cos(225.0f*rtd)*target_vy); 00536 target_belakang_kiri = (1/radius)*(sin(315.0f*rtd)*target_vx + cos(315.0f*rtd)*target_vy); 00537 target_depan_kiri = (target_depan_kiri*624.0f)/(2.0f*3.14); 00538 target_depan_kanan = (target_depan_kanan*624.0f)/(2.0f*3.14); 00539 target_belakang_kiri = (target_belakang_kiri*624.0f)/(2.0f*3.14); 00540 target_belakang_kanan = (target_belakang_kanan*624.0f)/(2.0f*3.14); 00541 //mengambil arah gerak motor 00542 arah_depan_kiri = cari_abs(target_depan_kiri); 00543 arah_depan_kanan = cari_abs(target_depan_kanan); 00544 arah_belakang_kiri = cari_abs(target_belakang_kiri); 00545 arah_belakang_kanan = cari_abs(target_belakang_kanan); 00546 } 00547 00548 void set_inverse() 00549 { 00550 //memasukkan nilai target kecepatan sudut untuk setpoint PID 00551 if(arah_depan_kanan!=0) 00552 { 00553 if(arah_depan_kiri==0) 00554 { 00555 DepanKananPID.setSetPoint(fabs(target_depan_kanan)); 00556 BelakangKiriPID.setSetPoint(fabs(target_belakang_kiri)); 00557 BelakangKananPID.setSetPoint(0); 00558 DepanKiriPID.setSetPoint(0); 00559 } 00560 else if(arah_depan_kiri!=0) 00561 { 00562 DepanKananPID.setSetPoint(fabs(target_depan_kanan)); 00563 BelakangKiriPID.setSetPoint(fabs(target_belakang_kiri)); 00564 BelakangKananPID.setSetPoint(fabs(target_belakang_kanan)); 00565 DepanKiriPID.setSetPoint(fabs(target_depan_kiri)); 00566 } 00567 } 00568 else if(arah_depan_kiri!=0) 00569 { 00570 if(arah_depan_kanan==0) 00571 { 00572 DepanKananPID.setSetPoint(0); 00573 BelakangKiriPID.setSetPoint(0); 00574 BelakangKananPID.setSetPoint(fabs(target_belakang_kanan)); 00575 DepanKiriPID.setSetPoint(fabs(target_depan_kiri)); 00576 } 00577 else if(arah_depan_kanan!=0) 00578 { 00579 DepanKananPID.setSetPoint(fabs(target_depan_kanan)); 00580 BelakangKiriPID.setSetPoint(fabs(target_belakang_kiri)); 00581 BelakangKananPID.setSetPoint(fabs(target_belakang_kanan)); 00582 DepanKiriPID.setSetPoint(fabs(target_depan_kiri)); 00583 } 00584 } 00585 } 00586 00587 void jalan_maju() 00588 { 00589 t_jalan.reset(); 00590 t_jalan.start(); 00591 proses_kecepatan(); 00592 Get_Count(); 00593 Cari_Koordinat(); 00594 pc.printf("Jarak : %d\n", jarak); 00595 //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat); 00596 BelakangKanan.speed(BelakangKananPID.compute()*-1); 00597 BelakangKiri.speed(BelakangKiriPID.compute()); 00598 DepanKanan.speed(DepanKananPID.compute()*-1); 00599 DepanKiri.speed(DepanKiriPID.compute()); 00600 while(t_jalan.read_ms()<=20) 00601 { 00602 } 00603 t_jalan.reset(); 00604 } 00605 00606 void jalan_mundur() 00607 { 00608 t_jalan.reset(); 00609 t_jalan.start(); 00610 proses_kecepatan(); 00611 Get_Count(); 00612 Cari_Koordinat(); 00613 pc.printf("Koordinat X : %d\n", koordinat_x_bulat); 00614 pc.printf("Koordinat Y : %d\n", koordinat_y_bulat); 00615 BelakangKanan.speed(BelakangKananPID.compute()); 00616 BelakangKiri.speed(BelakangKiriPID.compute()*-1); 00617 DepanKanan.speed(DepanKananPID.compute()); 00618 DepanKiri.speed(DepanKiriPID.compute()*-1); 00619 while(t_jalan.read_ms()<=20) 00620 { 00621 } 00622 t_jalan.reset(); 00623 } 00624 00625 void jalan_kanan() 00626 { 00627 t_jalan.reset(); 00628 t_jalan.start(); 00629 proses_kecepatan(); 00630 Get_Count(); 00631 Cari_Koordinat(); 00632 pc.printf("Koordinat X : %d\n", koordinat_x_bulat); 00633 pc.printf("Koordinat Y : %d\n", koordinat_y_bulat); 00634 BelakangKanan.speed(BelakangKananPID.compute()*-1); 00635 BelakangKiri.speed(BelakangKiriPID.compute()*-1); 00636 DepanKanan.speed(DepanKananPID.compute()); 00637 DepanKiri.speed(DepanKiriPID.compute()); 00638 while(t_jalan.read_ms()<=20) 00639 { 00640 } 00641 t_jalan.reset(); 00642 } 00643 00644 void jalan_kiri() 00645 { 00646 t_jalan.reset(); 00647 t_jalan.start(); 00648 proses_kecepatan(); 00649 Get_Count(); 00650 Cari_Koordinat(); 00651 pc.printf("Koordinat X : %d\n", koordinat_x_bulat); 00652 pc.printf("Koordinat Y : %d\n", koordinat_y_bulat); 00653 BelakangKanan.speed(BelakangKananPID.compute()); 00654 BelakangKiri.speed(BelakangKiriPID.compute()); 00655 DepanKanan.speed(DepanKananPID.compute()*-1); 00656 DepanKiri.speed(DepanKiriPID.compute()*-1); 00657 while(t_jalan.read_ms()<=20) 00658 { 00659 } 00660 t_jalan.reset(); 00661 } 00662 00663 void jalan_rotasi_kanan() 00664 { 00665 t_jalan.reset(); 00666 t_jalan.start(); 00667 proses_kecepatan(); 00668 Get_Count(); 00669 Cari_Koordinat(); 00670 //pc.printf("Koordinat X : %d\n", koordinat_x_bulat); 00671 //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat); 00672 pc.printf("Orientasi : %d\n", orientasi_2); 00673 BelakangKanan.speed(BelakangKananPID.compute()); 00674 BelakangKiri.speed(BelakangKiriPID.compute()); 00675 DepanKanan.speed(DepanKananPID.compute()); 00676 DepanKiri.speed(DepanKiriPID.compute()); 00677 while(t_jalan.read_ms()<=20) 00678 { 00679 } 00680 t_jalan.reset(); 00681 } 00682 00683 void jalan_rotasi_kiri() 00684 { 00685 t_jalan.reset(); 00686 t_jalan.start(); 00687 proses_kecepatan(); 00688 Get_Count(); 00689 Cari_Koordinat(); 00690 //pc.printf("Koordinat X : %d\n", koordinat_x_bulat); 00691 //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat); 00692 pc.printf("Orientasi : %d\n", orientasi_2); 00693 BelakangKanan.speed(BelakangKananPID.compute()*-1); 00694 BelakangKiri.speed(BelakangKiriPID.compute()*-1); 00695 DepanKanan.speed(DepanKananPID.compute()*-1); 00696 DepanKiri.speed(DepanKiriPID.compute()*-1); 00697 while(t_jalan.read_ms()<=20) 00698 { 00699 } 00700 t_jalan.reset(); 00701 } 00702 00703 void berhenti() 00704 { 00705 t_jalan.reset(); 00706 t_jalan.start(); 00707 //pc.printf("Koordinat X : %d\n", koordinat_x_bulat); 00708 //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat); 00709 pc.printf("Orientasi : %d\n", orientasi_2); 00710 DepanKanan.brake(0); 00711 BelakangKanan.brake(0); 00712 BelakangKiri.brake(0); 00713 DepanKiri.brake(0); 00714 while(t_jalan.read_ms()<=20) 00715 { 00716 } 00717 t_jalan.reset(); 00718 } 00719 00720 void ke_setpoint_x() 00721 { 00722 if(koordinat_x_bulat<setpoint_x) 00723 { 00724 kosongkan(); 00725 while(koordinat_x_bulat<setpoint_x) 00726 { 00727 jalan_kanan(); 00728 } 00729 DepanKanan.brake(); 00730 BelakangKanan.brake(); 00731 BelakangKiri.brake(); 00732 DepanKiri.brake(); 00733 } 00734 else if(koordinat_x_bulat>setpoint_x) 00735 { 00736 kosongkan(); 00737 while(koordinat_x_bulat>setpoint_x) 00738 { 00739 jalan_kiri(); 00740 } 00741 DepanKanan.brake(); 00742 BelakangKanan.brake(); 00743 BelakangKiri.brake(); 00744 DepanKiri.brake(); 00745 } 00746 else if(koordinat_x_bulat==setpoint_x) 00747 { 00748 berhenti(); 00749 } 00750 } 00751 00752 void ke_setpoint_y() 00753 { 00754 if(koordinat_y_bulat<setpoint_y) 00755 { 00756 kosongkan(); 00757 while(koordinat_y_bulat<setpoint_y) 00758 { 00759 jalan_maju(); 00760 } 00761 DepanKanan.brake(); 00762 BelakangKanan.brake(); 00763 BelakangKiri.brake(); 00764 DepanKiri.brake(); 00765 } 00766 else if(koordinat_y_bulat>setpoint_y) 00767 { 00768 kosongkan(); 00769 while(koordinat_y>setpoint_y) 00770 { 00771 jalan_mundur(); 00772 } 00773 DepanKanan.brake(); 00774 BelakangKanan.brake(); 00775 BelakangKiri.brake(); 00776 DepanKiri.brake(); 00777 } 00778 else if(koordinat_y_bulat==setpoint_y) 00779 { 00780 berhenti(); 00781 } 00782 } 00783 00784 void mode_cepat() 00785 { 00786 target_depan_kanan = 1800.0f; 00787 target_depan_kiri = 1800.0f; 00788 target_belakang_kanan = 1800.0f; 00789 target_belakang_kiri = 1800.0f; 00790 } 00791 00792 void mode_lambat() 00793 { 00794 target_depan_kanan = 1000.0f; 00795 target_depan_kiri = 1000.0f; 00796 target_belakang_kanan = 1000.0f; 00797 target_belakang_kiri = 1000.0f; 00798 } 00799 00800 00801 void ke_target_orientasi() 00802 { 00803 if(orientasi_2>target_orientasi) 00804 { 00805 kosongkan(); 00806 while(orientasi_2<target_orientasi) 00807 { 00808 jalan_rotasi_kiri(); 00809 } 00810 DepanKanan.brake(); 00811 BelakangKanan.brake(); 00812 BelakangKiri.brake(); 00813 DepanKiri.brake(); 00814 } 00815 else if(orientasi_2<target_orientasi) 00816 { 00817 kosongkan(); 00818 while(orientasi_2>target_orientasi) 00819 { 00820 jalan_rotasi_kanan(); 00821 } 00822 DepanKanan.brake(); 00823 BelakangKanan.brake(); 00824 BelakangKiri.brake(); 00825 DepanKiri.brake(); 00826 } 00827 else if(orientasi_2==target_orientasi) 00828 { 00829 berhenti(); 00830 } 00831 } 00832 00833 void ke_target_orientasi2() 00834 { 00835 while(orientasi_2<target_orientasi) 00836 { 00837 jalan_rotasi_kiri(); 00838 if(orientasi_2 == target_orientasi) 00839 { 00840 berhenti(); 00841 } 00842 } 00843 while(orientasi_2>target_orientasi) 00844 { 00845 jalan_rotasi_kanan(); 00846 if(orientasi_2 == target_orientasi) 00847 { 00848 berhenti(); 00849 } 00850 } 00851 } 00852 00853 void jalan_inverse() 00854 { 00855 if((arah_depan_kiri==0)&&(arah_belakang_kanan==0)==1) 00856 { 00857 BelakangKiri.speed(BelakangKiriPID.compute()*arah_belakang_kiri); 00858 DepanKanan.speed(DepanKananPID.compute()*arah_depan_kanan); 00859 } 00860 else if((arah_depan_kanan==0)&&(arah_belakang_kiri==0)==1) 00861 { 00862 BelakangKanan.speed(BelakangKiriPID.compute()*arah_belakang_kanan); 00863 DepanKiri.speed(DepanKananPID.compute()*arah_depan_kiri); 00864 } 00865 else if(arah_depan_kiri!=0) 00866 { 00867 if(arah_depan_kanan!=0) 00868 { 00869 BelakangKiri.speed(BelakangKiriPID.compute()*arah_belakang_kiri); 00870 DepanKanan.speed(DepanKananPID.compute()*arah_depan_kanan); 00871 BelakangKanan.speed(BelakangKiriPID.compute()*arah_belakang_kanan); 00872 DepanKiri.speed(DepanKananPID.compute()*arah_depan_kiri); 00873 } 00874 } 00875 } 00876 00877 void cari_koin1() 00878 { 00879 target_orientasi = 180; 00880 ke_target_orientasi(); 00881 while(jarak>=23) 00882 { 00883 //jarak = ultra.read(); 00884 jalan_maju(); 00885 } 00886 //while(jarak<=20) 00887 //{ 00888 // jalan_maju(); 00889 //} 00890 //while(eksistensi_koin!=0) 00891 //{ 00892 // jalan_kanan(); 00893 //} 00894 } 00895 00896 float sensor(){ 00897 float jarak2; 00898 int durasi; 00899 trigPin1=1; 00900 t.reset(); 00901 wait_us(10); 00902 trigPin1=0; 00903 while (echoPin1 == 0){ 00904 }t.start(); 00905 while (echoPin1 == 1) 00906 { 00907 } 00908 t.stop(); 00909 durasi = t.read_us(); 00910 jarak2 = (double)(durasi) / 29.0f /2.0f ; // 29 kecepatan suara di udara (2.9 detik untuk 1 kilometer), dibagi 2 karena bolak-balik 00911 return jarak2; 00912 } 00913 00914 void cari_koin2() 00915 { 00916 while(orientasi != 180) 00917 { 00918 ke_target_orientasi(); 00919 } 00920 while(jarak>=23) 00921 { 00922 //jarak = ultra.read(); 00923 jalan_maju(); 00924 } 00925 //while(eksistensi_koin!=0) 00926 //{ 00927 // jalan_kanan(); 00928 //} 00929 } 00930 00931 int main() 00932 { 00933 pc.baud(38400); 00934 bluetooth.baud(9600); 00935 bluetooth.attach(&fungsi_bluetooth, Serial::RxIrq); 00936 00937 BEARING_Register = 0x02; 00938 DepanKanan.period(0.01f); 00939 BelakangKanan.period(0.01f); 00940 BelakangKiri.period(0.01f); //Set motor PWM periods to 20KHz. 00941 DepanKiri.period(0.01f); 00942 00943 DepanKananPID.setInputLimits(0, 3000);//Input units: counts per second. 00944 DepanKananPID.setOutputLimits(0.0, 0.9);//Output units: PwmOut duty cycle as %. 00945 DepanKananPID.setMode(AUTO_MODE); 00946 00947 BelakangKananPID.setInputLimits(0, 3000); 00948 BelakangKananPID.setOutputLimits(0.0, 0.9); 00949 BelakangKananPID.setMode(AUTO_MODE); 00950 00951 BelakangKiriPID.setInputLimits(0, 3000); 00952 BelakangKiriPID.setOutputLimits(0.0, 0.9); 00953 BelakangKiriPID.setMode(AUTO_MODE); 00954 00955 DepanKiriPID.setInputLimits(0, 3000); 00956 DepanKiriPID.setOutputLimits(0.0, 0.9); 00957 DepanKiriPID.setMode(AUTO_MODE); 00958 00959 DepanKananPID.setSetPoint(1400); 00960 BelakangKananPID.setSetPoint(1400); 00961 BelakangKiriPID.setSetPoint(1400); 00962 DepanKiriPID.setSetPoint(1400); 00963 sudut_awal = Get_Yaw(); 00964 while(1) 00965 { 00966 00967 if(Jalan == Jalan_Maju) 00968 { 00969 DepanKananPID.setSetPoint(target_depan_kanan); 00970 BelakangKananPID.setSetPoint(target_belakang_kanan); 00971 BelakangKiriPID.setSetPoint(target_belakang_kiri); 00972 DepanKiriPID.setSetPoint(target_depan_kiri); 00973 kosongkan(); 00974 while(Jalan== Jalan_Maju) 00975 { 00976 t_jalan.reset(); 00977 t_jalan.start(); 00978 proses_kecepatan(); 00979 Get_Count(); 00980 Cari_Koordinat(); 00981 pc.printf("Koordinat X : %d\n", koordinat_x_bulat); 00982 //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat); 00983 pc.printf("Orientasi : %f\n", orientasi); 00984 BelakangKanan.speed(BelakangKananPID.compute()*-1); 00985 BelakangKiri.speed(BelakangKiriPID.compute()); 00986 DepanKanan.speed(DepanKananPID.compute()*-1); 00987 DepanKiri.speed(DepanKiriPID.compute()); 00988 while(t_jalan.read_ms()<=20) 00989 { 00990 } 00991 t_jalan.reset(); 00992 } 00993 DepanKanan.brake(); 00994 BelakangKanan.brake(); 00995 BelakangKiri.brake(); 00996 DepanKiri.brake(); 00997 } 00998 00999 else if(Jalan == Jalan_Mundur) 01000 { 01001 DepanKananPID.setSetPoint(target_depan_kanan); 01002 BelakangKananPID.setSetPoint(target_belakang_kanan); 01003 BelakangKiriPID.setSetPoint(target_belakang_kiri); 01004 DepanKiriPID.setSetPoint(target_depan_kiri); 01005 kosongkan(); 01006 while(Jalan== Jalan_Mundur) 01007 { 01008 t_jalan.reset(); 01009 t_jalan.start(); 01010 proses_kecepatan(); 01011 Get_Count(); 01012 Cari_Koordinat(); 01013 //pc.printf("Koordinat X : %d\n", koordinat_x_bulat); 01014 //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat); 01015 pc.printf("Orientasi : %f\n", orientasi); 01016 BelakangKanan.speed(BelakangKananPID.compute()); 01017 BelakangKiri.speed(BelakangKiriPID.compute()*-1); 01018 DepanKanan.speed(DepanKananPID.compute()); 01019 DepanKiri.speed(DepanKiriPID.compute()*-1); 01020 while(t_jalan.read_ms()<=20) 01021 { 01022 } 01023 t_jalan.reset(); 01024 } 01025 DepanKanan.brake(); 01026 BelakangKanan.brake(); 01027 BelakangKiri.brake(); 01028 DepanKiri.brake(); 01029 } 01030 01031 else if(Jalan == Jalan_Kiri) 01032 { 01033 DepanKananPID.setSetPoint(target_depan_kanan); 01034 BelakangKananPID.setSetPoint(target_belakang_kanan); 01035 BelakangKiriPID.setSetPoint(target_belakang_kiri); 01036 DepanKiriPID.setSetPoint(target_depan_kiri); 01037 kosongkan(); 01038 while(Jalan== Jalan_Kiri) 01039 { 01040 t_jalan.reset(); 01041 t_jalan.start(); 01042 proses_kecepatan(); 01043 Get_Count(); 01044 Cari_Koordinat(); 01045 //pc.printf("Koordinat X : %d\n", koordinat_x_bulat); 01046 //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat); 01047 pc.printf("Orientasi : %f\n", orientasi); 01048 BelakangKanan.speed(BelakangKananPID.compute()); 01049 BelakangKiri.speed(BelakangKiriPID.compute()); 01050 DepanKanan.speed(DepanKananPID.compute()*-1); 01051 DepanKiri.speed(DepanKiriPID.compute()*-1); 01052 while(t_jalan.read_ms()<=20) 01053 { 01054 } 01055 t_jalan.reset(); 01056 } 01057 DepanKanan.brake(); 01058 BelakangKanan.brake(); 01059 BelakangKiri.brake(); 01060 DepanKiri.brake(); 01061 } 01062 else if(Jalan == Jalan_Kanan) 01063 { 01064 DepanKananPID.setSetPoint(target_depan_kanan); 01065 BelakangKananPID.setSetPoint(target_belakang_kanan); 01066 BelakangKiriPID.setSetPoint(target_belakang_kiri); 01067 DepanKiriPID.setSetPoint(target_depan_kiri); 01068 kosongkan(); 01069 while(Jalan== Jalan_Kanan) 01070 { 01071 t_jalan.reset(); 01072 t_jalan.start(); 01073 proses_kecepatan(); 01074 Get_Count(); 01075 Cari_Koordinat(); 01076 //pc.printf("Koordinat X : %d\n", koordinat_x_bulat); 01077 //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat); 01078 pc.printf("Orientasi : %f\n", orientasi); 01079 BelakangKanan.speed(BelakangKananPID.compute()*-1); 01080 BelakangKiri.speed(BelakangKiriPID.compute()*-1); 01081 DepanKanan.speed(DepanKananPID.compute()); 01082 DepanKiri.speed(DepanKiriPID.compute()); 01083 while(t_jalan.read_ms()<=20) 01084 { 01085 } 01086 t_jalan.reset(); 01087 } 01088 DepanKanan.brake(1); 01089 BelakangKanan.brake(1); 01090 BelakangKiri.brake(1); 01091 DepanKiri.brake(1); 01092 } 01093 01094 else if(Jalan == Rotasi_Kanan) 01095 { 01096 DepanKananPID.setSetPoint(target_depan_kanan-800.0f); 01097 BelakangKananPID.setSetPoint(target_belakang_kanan-800.0f); 01098 BelakangKiriPID.setSetPoint(target_belakang_kiri-800.0f); 01099 DepanKiriPID.setSetPoint(target_depan_kiri-800.0f); 01100 kosongkan(); 01101 while(Jalan== Rotasi_Kanan) 01102 { 01103 t_jalan.reset(); 01104 t_jalan.start(); 01105 proses_kecepatan(); 01106 Get_Count(); 01107 Cari_Koordinat(); 01108 //pc.printf("Koordinat X : %d\n", koordinat_x_bulat); 01109 //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat); 01110 pc.printf("Orientasi : %f\n", orientasi); 01111 BelakangKanan.speed(BelakangKananPID.compute()); 01112 BelakangKiri.speed(BelakangKiriPID.compute()); 01113 DepanKanan.speed(DepanKananPID.compute()); 01114 DepanKiri.speed(DepanKiriPID.compute()); 01115 while(t_jalan.read_ms()<=20) 01116 { 01117 } 01118 t_jalan.reset(); 01119 } 01120 DepanKanan.brake(); 01121 BelakangKanan.brake(); 01122 BelakangKiri.brake(); 01123 DepanKiri.brake(); 01124 } 01125 01126 else if(Jalan == Rotasi_Kiri) 01127 { 01128 DepanKananPID.setSetPoint(target_depan_kanan-800.0f); 01129 BelakangKananPID.setSetPoint(target_belakang_kanan-800.0f); 01130 BelakangKiriPID.setSetPoint(target_belakang_kiri-800.0f); 01131 DepanKiriPID.setSetPoint(target_depan_kiri-800.0f); 01132 kosongkan(); 01133 while(Jalan== Rotasi_Kiri) 01134 { 01135 t_jalan.reset(); 01136 t_jalan.start(); 01137 proses_kecepatan(); 01138 Get_Count(); 01139 Cari_Koordinat(); 01140 //pc.printf("Koordinat X : %d\n", koordinat_x_bulat); 01141 //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat); 01142 pc.printf("Orientasi : %f\n", orientasi); 01143 BelakangKanan.speed(BelakangKananPID.compute()*-1); 01144 BelakangKiri.speed(BelakangKiriPID.compute()*-1); 01145 DepanKanan.speed(DepanKananPID.compute()*-1); 01146 DepanKiri.speed(DepanKiriPID.compute()*-1); 01147 while(t_jalan.read_ms()<=20) 01148 { 01149 } 01150 t_jalan.reset(); 01151 } 01152 DepanKanan.brake(); 01153 BelakangKanan.brake(); 01154 BelakangKiri.brake(); 01155 DepanKiri.brake(); 01156 } 01157 01158 else if(Jalan == Belok_Kanan) 01159 { 01160 DepanKananPID.setSetPoint(target_depan_kanan-1000.0f); 01161 BelakangKananPID.setSetPoint(target_belakang_kanan-1000.0f); 01162 BelakangKiriPID.setSetPoint(target_belakang_kiri); 01163 DepanKiriPID.setSetPoint(target_depan_kiri); 01164 kosongkan(); 01165 while(Jalan== Belok_Kanan) 01166 { 01167 t_jalan.reset(); 01168 t_jalan.start(); 01169 proses_kecepatan(); 01170 Get_Count(); 01171 Cari_Koordinat(); 01172 //pc.printf("Koordinat X : %d\n", koordinat_x_bulat); 01173 //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat); 01174 pc.printf("Orientasi : %f\n", orientasi); 01175 BelakangKanan.speed(BelakangKananPID.compute()*-1); 01176 BelakangKiri.speed(BelakangKiriPID.compute()); 01177 DepanKanan.speed(DepanKananPID.compute()*-1); 01178 DepanKiri.speed(DepanKiriPID.compute()); 01179 while(t_jalan.read_ms()<=20) 01180 { 01181 } 01182 t_jalan.reset(); 01183 } 01184 DepanKanan.brake(); 01185 BelakangKanan.brake(); 01186 BelakangKiri.brake(); 01187 DepanKiri.brake(); 01188 } 01189 01190 else if(Jalan == Mundur_Belok_Kanan) 01191 { 01192 DepanKananPID.setSetPoint(target_depan_kanan-1000.0f); 01193 BelakangKananPID.setSetPoint(target_belakang_kanan-1000.0f); 01194 BelakangKiriPID.setSetPoint(target_belakang_kiri); 01195 DepanKiriPID.setSetPoint(target_depan_kiri); 01196 kosongkan(); 01197 while(Jalan== Mundur_Belok_Kanan) 01198 { 01199 t_jalan.reset(); 01200 t_jalan.start(); 01201 proses_kecepatan(); 01202 Get_Count(); 01203 Cari_Koordinat(); 01204 //pc.printf("Koordinat X : %d\n", koordinat_x_bulat); 01205 //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat); 01206 pc.printf("Orientasi : %f\n", orientasi); 01207 BelakangKanan.speed(BelakangKananPID.compute()); 01208 BelakangKiri.speed(BelakangKiriPID.compute()*-1); 01209 DepanKanan.speed(DepanKananPID.compute()); 01210 DepanKiri.speed(DepanKiriPID.compute()*-1); 01211 while(t_jalan.read_ms()<=20) 01212 { 01213 } 01214 t_jalan.reset(); 01215 } 01216 DepanKanan.brake(); 01217 BelakangKanan.brake(); 01218 BelakangKiri.brake(); 01219 DepanKiri.brake(); 01220 } 01221 01222 else if(Jalan == Mundur_Belok_Kiri) 01223 { 01224 DepanKananPID.setSetPoint(target_depan_kanan); 01225 BelakangKananPID.setSetPoint(target_belakang_kanan); 01226 BelakangKiriPID.setSetPoint(target_belakang_kiri-1000.0f); 01227 DepanKiriPID.setSetPoint(target_depan_kiri-1000.0f); 01228 kosongkan(); 01229 while(Jalan== Mundur_Belok_Kiri) 01230 { 01231 t_jalan.reset(); 01232 t_jalan.start(); 01233 proses_kecepatan(); 01234 Get_Count(); 01235 Cari_Koordinat(); 01236 //pc.printf("Koordinat X : %d\n", koordinat_x_bulat); 01237 //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat); 01238 pc.printf("Orientasi : %f\n", orientasi); 01239 BelakangKanan.speed(BelakangKananPID.compute()); 01240 BelakangKiri.speed(BelakangKiriPID.compute()*-1); 01241 DepanKanan.speed(DepanKananPID.compute()); 01242 DepanKiri.speed(DepanKiriPID.compute()*-1); 01243 while(t_jalan.read_ms()<=20) 01244 { 01245 } 01246 t_jalan.reset(); 01247 } 01248 DepanKanan.brake(); 01249 BelakangKanan.brake(); 01250 BelakangKiri.brake(); 01251 DepanKiri.brake(); 01252 } 01253 01254 else if(Jalan == Belok_Kiri) 01255 { 01256 DepanKananPID.setSetPoint(target_depan_kanan); 01257 BelakangKananPID.setSetPoint(target_belakang_kanan); 01258 BelakangKiriPID.setSetPoint(target_belakang_kiri-1000); 01259 DepanKiriPID.setSetPoint(target_depan_kiri-1000); 01260 kosongkan(); 01261 while(Jalan== Belok_Kiri) 01262 { 01263 t_jalan.reset(); 01264 t_jalan.start(); 01265 proses_kecepatan(); 01266 Get_Count(); 01267 Cari_Koordinat(); 01268 //pc.printf("Koordinat X : %d\n", koordinat_x_bulat); 01269 //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat); 01270 pc.printf("Orientasi : %f\n", orientasi); 01271 BelakangKanan.speed(BelakangKananPID.compute()*-1); 01272 BelakangKiri.speed(BelakangKiriPID.compute()); 01273 DepanKanan.speed(DepanKananPID.compute()*-1); 01274 DepanKiri.speed(DepanKiriPID.compute()); 01275 while(t_jalan.read_ms()<=20) 01276 { 01277 } 01278 t_jalan.reset(); 01279 } 01280 DepanKanan.brake(); 01281 BelakangKanan.brake(); 01282 BelakangKiri.brake(); 01283 DepanKiri.brake(); 01284 } 01285 01286 else if(Jalan == Muter_Kanan) 01287 { 01288 //DepanKananPID.setSetPoint(1400); 01289 BelakangKananPID.setSetPoint(1400); 01290 //BelakangKiriPID.setSetPoint(1400); 01291 DepanKiriPID.setSetPoint(1400); 01292 kosongkan(); 01293 while(Jalan== Muter_Kanan) 01294 { 01295 t_jalan.reset(); 01296 t_jalan.start(); 01297 proses_kecepatan(); 01298 Get_Count(); 01299 Cari_Koordinat(); 01300 //pc.printf("Koordinat X : %d\n", koordinat_x_bulat); 01301 //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat); 01302 pc.printf("Orientasi : %f\n", orientasi); 01303 BelakangKanan.speed(BelakangKananPID.compute()*-1); 01304 //BelakangKiri.speed(BelakangKiriPID.compute()*-1); 01305 //DepanKanan.speed(DepanKananPID.compute()); 01306 DepanKiri.speed(DepanKiriPID.compute()); 01307 while(t_jalan.read_ms()<=20) 01308 { 01309 } 01310 t_jalan.reset(); 01311 } 01312 DepanKanan.brake(); 01313 BelakangKanan.brake(); 01314 BelakangKiri.brake(); 01315 DepanKiri.brake(); 01316 } 01317 01318 else if(Jalan == Muter_Kiri) 01319 { 01320 DepanKananPID.setSetPoint(1400); 01321 //BelakangKananPID.setSetPoint(1400); 01322 BelakangKiriPID.setSetPoint(1400); 01323 //DepanKiriPID.setSetPoint(1400); 01324 kosongkan(); 01325 while(Jalan== Muter_Kiri) 01326 { 01327 t_jalan.reset(); 01328 t_jalan.start(); 01329 proses_kecepatan(); 01330 Get_Count(); 01331 Cari_Koordinat(); 01332 //pc.printf("Koordinat X : %d\n", koordinat_x_bulat); 01333 //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat); 01334 pc.printf("Orientasi : %f\n", orientasi); 01335 //BelakangKanan.speed(BelakangKananPID.compute()); 01336 BelakangKiri.speed(BelakangKiriPID.compute()*1); 01337 DepanKanan.speed(DepanKananPID.compute()*-1); 01338 //DepanKiri.speed(DepanKiriPID.compute()*-1); 01339 while(t_jalan.read_ms()<=20) 01340 { 01341 } 01342 t_jalan.reset(); 01343 } 01344 DepanKanan.brake(); 01345 BelakangKanan.brake(); 01346 BelakangKiri.brake(); 01347 DepanKiri.brake(); 01348 } 01349 01350 else if(Jalan == Ganti_Kecepatan) 01351 { 01352 if(kecepatan_state==0) 01353 { 01354 mode_lambat(); 01355 wait_ms(200); 01356 kecepatan_state =1; 01357 } 01358 else if(kecepatan_state==1) 01359 { 01360 mode_cepat(); 01361 wait_ms(200); 01362 kecepatan_state = 0; 01363 } 01364 } 01365 01366 else if(Jalan == ke_setpoint) 01367 { 01368 DepanKananPID.setSetPoint(1400); 01369 BelakangKananPID.setSetPoint(1400); 01370 BelakangKiriPID.setSetPoint(1400); 01371 DepanKiriPID.setSetPoint(1400); 01372 //while(Jalan == ke_setpoint) 01373 //{ 01374 //ke_setpoint_x(); 01375 //ke_setpoint_y(); 01376 ke_target_orientasi(); 01377 //} 01378 } 01379 else if(Jalan == mode_inverse) 01380 { 01381 kosongkan(); 01382 cari_inverse(orientasi, (float)setpoint_x, (float)setpoint_y); 01383 set_inverse(); 01384 while(Jalan == mode_inverse) 01385 { 01386 if((koordinat_x_bulat!=setpoint_x)&&(koordinat_y_bulat!=setpoint_y)) 01387 { 01388 t_jalan.reset(); 01389 t_jalan.start(); 01390 proses_kecepatan(); 01391 Get_Count(); 01392 Cari_Koordinat(); 01393 if(arah_depan_kanan!=0) 01394 { 01395 if(arah_depan_kiri==0) 01396 { 01397 BelakangKiri.speed(BelakangKiriPID.compute()*arah_belakang_kiri); 01398 DepanKanan.speed(DepanKananPID.compute()*arah_depan_kanan); 01399 BelakangKanan.brake(0); 01400 DepanKiri.brake(0); 01401 } 01402 else if(arah_depan_kiri!=0) 01403 { 01404 BelakangKiri.speed(BelakangKiriPID.compute()*arah_belakang_kiri); 01405 DepanKanan.speed(DepanKananPID.compute()*arah_depan_kanan); 01406 BelakangKanan.speed(BelakangKananPID.compute()*arah_belakang_kanan); 01407 DepanKiri.speed(DepanKiriPID.compute()*arah_depan_kiri); 01408 } 01409 } 01410 else if(arah_depan_kiri!=0) 01411 { 01412 if(arah_depan_kanan==0) 01413 { 01414 BelakangKanan.speed(BelakangKananPID.compute()*arah_belakang_kanan); 01415 DepanKiri.speed(DepanKiriPID.compute()*arah_depan_kiri); 01416 BelakangKiri.brake(0); 01417 DepanKanan.brake(0); 01418 } 01419 else if(arah_depan_kanan!=0) 01420 { 01421 BelakangKiri.speed(BelakangKiriPID.compute()*arah_belakang_kiri); 01422 DepanKanan.speed(DepanKananPID.compute()*arah_depan_kanan); 01423 BelakangKanan.speed(BelakangKananPID.compute()*arah_belakang_kanan); 01424 DepanKiri.speed(DepanKiriPID.compute()*arah_depan_kiri); 01425 } 01426 } 01427 pc.printf("Koordinat X : %d\n", koordinat_x_bulat); 01428 pc.printf("Koordinat Y : %d\n", koordinat_y_bulat); 01429 pc.printf("Orientasi : %d\n", orientasi_2); 01430 //pc.printf("X basis : %f\n", target_x_basis); 01431 //pc.printf("Y basis : %f\n", target_y_basis); 01432 //pc.printf("Depan Kiri : %f\n", target_depan_kiri); 01433 //pc.printf("Depan Kanan : %f\n", target_depan_kanan); 01434 //pc.printf("Belakang Kanan : %f\n", target_belakang_kanan); 01435 //pc.printf("Belakang Kiri : %f\n", target_belakang_kiri); 01436 while(t_jalan.read_ms()<=20) 01437 { 01438 } 01439 t_jalan.reset(); 01440 } 01441 } 01442 } 01443 else if(Jalan == Capit_Buka) 01444 { 01445 if(capit_state==0) 01446 { 01447 Servo.pulsewidth_us(1230); 01448 capit_state=1; 01449 wait_ms(200); 01450 } 01451 else if(capit_state==1) 01452 { 01453 Servo.pulsewidth_us(2400); 01454 capit_state=0; 01455 wait_ms(200); 01456 } 01457 /*else if(capit_state==2) 01458 { 01459 Servo.pulsewidth_us(1500); 01460 capit_state=0; 01461 wait_ms(200); 01462 } 01463 */ 01464 } 01465 else if(Jalan == Capit_Muter) 01466 { 01467 if(capit2_state==0) 01468 { 01469 for(i=1800; i>910; i--) 01470 { 01471 t_capit.reset(); 01472 t_capit.start(); 01473 Servo2.pulsewidth_us(i); 01474 while(t_capit.read_ms()<1) 01475 { 01476 } 01477 t_capit.stop(); 01478 } 01479 capit2_state=1; 01480 } 01481 else if(capit2_state==1) 01482 { 01483 for(i=910; i<1800; i=i++) 01484 { 01485 t_capit.reset(); 01486 t_capit.start(); 01487 Servo2.pulsewidth_us(i); 01488 while(t_capit.read_us()<600) 01489 { 01490 } 01491 t_capit.stop(); 01492 } 01493 capit2_state=0; 01494 } 01495 } 01496 else if(Jalan == Cari_Koin1) 01497 { 01498 DepanKananPID.setSetPoint(1000); 01499 BelakangKananPID.setSetPoint(1000); 01500 BelakangKiriPID.setSetPoint(1000); 01501 DepanKiriPID.setSetPoint(1000); 01502 eksistensi_koin = ir.read(); 01503 jarak = sensor(); 01504 while(Jalan == Cari_Koin1) 01505 { 01506 //target_orientasi = 180; 01507 //ke_target_orientasi2(); 01508 Servo.pulsewidth_us(2290); 01509 wait_ms(100); 01510 while(jarak>16.0f) 01511 { 01512 //jalan_maju(); 01513 jarak = sensor(); 01514 jalan_maju(); 01515 } 01516 berhenti(); 01517 wait_ms(300); 01518 while(eksistensi_koin== tidak_ada) 01519 { 01520 eksistensi_koin = ir.read(); 01521 jalan_kiri(); 01522 } 01523 berhenti(); 01524 wait_ms(300); 01525 Servo.pulsewidth_us(1190); 01526 wait_ms(100); 01527 t2.reset(); 01528 t2.start(); 01529 while(t2.read_ms()<1500) 01530 { 01531 jalan_mundur(); 01532 } 01533 t2.stop(); 01534 //while(jarak<90) 01535 //{ 01536 // jalan_mundur(); 01537 // jarak = sensor(); 01538 //} 01539 //Servo2.pulsewidth_us(2200); 01540 //target_orientasi = 0; 01541 //ke_target_orientasi2(); 01542 } 01543 } 01544 else if(Jalan == Cari_Koin2) 01545 { 01546 while(Jalan == Cari_Koin2) 01547 { 01548 cari_koin2(); 01549 } 01550 } 01551 else if(Jalan == Berhenti) 01552 { 01553 kosongkan(); 01554 while(Jalan==Berhenti) 01555 { 01556 t_jalan.reset(); 01557 t_jalan.start(); 01558 proses_kecepatan(); 01559 Get_Count(); 01560 Cari_Koordinat(); 01561 //pc.printf("Koordinat X : %d\n", koordinat_x_bulat); 01562 //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat); 01563 pc.printf("Orientasi : %d\n", orientasi_2); 01564 DepanKanan.brake(0); 01565 BelakangKanan.brake(0); 01566 BelakangKiri.brake(0); 01567 DepanKiri.brake(0); 01568 while(t_jalan.read_ms()<=20) 01569 { 01570 } 01571 t_jalan.reset(); 01572 } 01573 } 01574 } 01575 }
Generated on Tue Oct 4 2022 10:50:35 by
1.7.2