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