KRTMI_Titanium2022
PID Odometry Inverse Tanpa Yaw
Dependencies: MPU9250 Motor PID QEI 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 Serial pc(USBTX, USBRX); 00007 Serial bluetooth(PB_6, PA_10); 00008 00009 QEI DepanKananQEI(PB_8, PC_9, NC, 986); //roda depan kanan PB_1, PB_15, NC, 624 00010 QEI BelakangKananQEI(PB_1, PB_15, NC, 986); //roda belakang kiri PB_14, PB_13, NC, 624 00011 QEI DepanKiriQEI(PC_10, PC_12, NC, 986); //roda depan kiri 00012 QEI BelakangKiriQEI(PA_15, PB_7, NC, 986); //roda belakang kiri 00013 00014 PID DepanKananPID(0.62, 0.1, 0.00001, 0.02); //0000001 Kc, Ti, Td MOTOR baru nomor 1 00015 PID BelakangKananPID(0.6, 0.1, 0.000001, 0.02); //Kc, Ti, Td MOTOR baru nomor 2 00016 PID BelakangKiriPID(0.89, 0.1, 0.000000000001, 0.02); //Kc, Ti, Td MOTOR baru nomor 3 00017 PID DepanKiriPID(0.68, 0.1, 0.0000001, 0.02); //Kc, Ti, Td MOTOR baru nomor 4 00018 00019 Motor DepanKanan(PA_11, PC_8, PC_6);//PB_9, PA_12, PC_5 00020 Motor DepanKiri(PA_0, PB_0, PA_4); 00021 Motor BelakangKanan(PB_9, PC_5, PA_12);//PA_11, PC_6, PC_8 00022 Motor BelakangKiri(PA_1, PC_0, PC_1); 00023 00024 Timer t_jalan; 00025 00026 char nilai, huruf; 00027 int Jalan; 00028 int Jalan_Maju =1; 00029 int Berhenti = 0; 00030 int Jalan_Mundur = 2; 00031 int Jalan_Kanan = 3; 00032 int Jalan_Kiri = 4; 00033 int ke_setpoint = 5; 00034 int mode_inverse = 6; 00035 00036 int DepanKananPulses = 0; 00037 int DepanKananPrevPulses = 0; 00038 float DepanKananVelocity = 0.0; 00039 00040 int BelakangKananPulses = 0; 00041 int BelakangKananPrevPulses = 0; 00042 float BelakangKananVelocity = 0.0; 00043 00044 int BelakangKiriPulses = 0; 00045 int BelakangKiriPrevPulses = 0; 00046 float BelakangKiriVelocity = 0.0; 00047 00048 int DepanKiriPulses = 0; 00049 int DepanKiriPrevPulses = 0; 00050 float DepanKiriVelocity = 0.0; 00051 00052 int Depan_Kanan_Count; 00053 int Depan_Kiri_Count; 00054 int Belakang_Kanan_Count; 00055 int Belakang_Kiri_Count; 00056 00057 int d_pulse_DepanKanan; 00058 int d_pulse_DepanKiri; 00059 int d_pulse_BelakangKanan; 00060 int d_pulse_BelakangKiri; 00061 00062 int koordinat_x_DepanKiri; 00063 int koordinat_x_DepanKanan; 00064 int koordinat_x_BelakangKiri; 00065 int koordinat_x_BelakangKanan; 00066 int koordinat_y_DepanKiri; 00067 int koordinat_y_DepanKanan; 00068 int koordinat_y_BelakangKiri; 00069 int koordinat_y_BelakangKanan; 00070 int koordinat_x; 00071 int koordinat_y; 00072 int Resolusi = 986; 00073 float pi = 3.141; 00074 float diameter = 5.8f; 00075 float rtd = pi/180.0f; 00076 int setpoint_x; 00077 int setpoint_y; 00078 float radius = 0.029; 00079 float orientasi; 00080 00081 float target_depan_kiri; 00082 float target_depan_kanan; 00083 float target_belakang_kiri; 00084 float target_belakang_kanan; 00085 int arah_depan_kiri; 00086 int arah_depan_kanan; 00087 int arah_belakang_kiri; 00088 int arah_belakang_kanan; 00089 00090 00091 void fungsi_bluetooth(void) 00092 { 00093 if(bluetooth.readable()) 00094 { 00095 setpoint_x = bluetooth.getc(); 00096 setpoint_y = bluetooth.getc(); 00097 nilai = bluetooth.getc(); 00098 if (nilai == 'S') 00099 { 00100 huruf = 'S'; 00101 Jalan = Berhenti; 00102 } 00103 else if (nilai == 'F') 00104 { 00105 huruf = 'F'; 00106 Jalan = Jalan_Maju; 00107 } 00108 else if(nilai =='B') 00109 { 00110 huruf = 'B'; 00111 Jalan = Jalan_Mundur; 00112 } 00113 else if(nilai =='L') 00114 { 00115 huruf = 'L'; 00116 Jalan = Jalan_Kiri; 00117 } 00118 else if(nilai =='R') 00119 { 00120 huruf = 'R'; 00121 Jalan = Jalan_Kanan; 00122 } 00123 else if(nilai =='G') 00124 { 00125 huruf = 'G'; 00126 Jalan = ke_setpoint; 00127 } 00128 else if(nilai == 'Y') 00129 { 00130 huruf = 'G'; 00131 Jalan = mode_inverse; 00132 } 00133 else if(nilai == 'S') 00134 { 00135 Jalan = Berhenti; 00136 } 00137 } 00138 } 00139 00140 void kosongkan() 00141 { 00142 BelakangKananQEI.reset(); 00143 DepanKananQEI.reset(); 00144 DepanKiriQEI.reset(); 00145 BelakangKiriQEI.reset(); 00146 00147 DepanKananPulses = 0; 00148 DepanKananPrevPulses = 0; 00149 DepanKananVelocity = 0.0; 00150 00151 BelakangKananPulses = 0; 00152 BelakangKananPrevPulses = 0; 00153 BelakangKananVelocity = 0.0; 00154 00155 BelakangKiriPulses = 0; 00156 BelakangKiriPrevPulses = 0; 00157 BelakangKiriVelocity = 0.0; 00158 00159 DepanKiriPulses = 0; 00160 DepanKiriPrevPulses = 0; 00161 DepanKiriVelocity = 0.0; 00162 } 00163 00164 void proses_kecepatan() 00165 { 00166 DepanKananPulses = DepanKananQEI.getPulses(); 00167 d_pulse_DepanKanan = DepanKananPulses - DepanKananPrevPulses; 00168 DepanKananVelocity = (DepanKananPulses - DepanKananPrevPulses) / 0.02; 00169 DepanKananPrevPulses = DepanKananPulses; 00170 DepanKananPID.setProcessValue(fabs(DepanKananVelocity)); 00171 00172 DepanKiriPulses = DepanKiriQEI.getPulses(); 00173 d_pulse_DepanKiri = DepanKiriPulses - DepanKiriPrevPulses; 00174 DepanKiriVelocity = (DepanKiriPulses - DepanKiriPrevPulses) / 0.02; 00175 DepanKiriPrevPulses = DepanKiriPulses; 00176 DepanKiriPID.setProcessValue(fabs(DepanKiriVelocity)); 00177 00178 BelakangKananPulses = BelakangKananQEI.getPulses(); 00179 d_pulse_BelakangKanan = BelakangKananPulses - BelakangKananPrevPulses; 00180 BelakangKananVelocity = (BelakangKananPulses - BelakangKananPrevPulses) / 0.02; 00181 BelakangKananPrevPulses = BelakangKananPulses; 00182 BelakangKananPID.setProcessValue(fabs(BelakangKananVelocity)); 00183 00184 BelakangKiriPulses = BelakangKiriQEI.getPulses(); 00185 d_pulse_BelakangKiri = BelakangKiriPulses - BelakangKiriPrevPulses; 00186 BelakangKiriVelocity = (BelakangKiriPulses - BelakangKiriPrevPulses) / 0.02; 00187 BelakangKiriPrevPulses = BelakangKiriPulses; 00188 BelakangKiriPID.setProcessValue(fabs(BelakangKiriVelocity)); 00189 } 00190 00191 void Get_Count() 00192 { 00193 Depan_Kanan_Count = Depan_Kanan_Count + d_pulse_DepanKanan; 00194 Depan_Kiri_Count = Depan_Kiri_Count + d_pulse_DepanKiri; 00195 Belakang_Kanan_Count = Belakang_Kanan_Count + d_pulse_BelakangKanan; 00196 Belakang_Kiri_Count = Belakang_Kiri_Count + d_pulse_BelakangKiri; 00197 } 00198 00199 void Cari_Koordinat() 00200 { 00201 koordinat_x_DepanKiri = ((double)Depan_Kiri_Count/(double)Resolusi)*pi*diameter*cos(45.0f*rtd); 00202 koordinat_x_DepanKanan = ((double)Depan_Kanan_Count/(double)Resolusi)*pi*diameter*cos(135.0f*rtd); 00203 koordinat_x_BelakangKanan = ((double)Belakang_Kanan_Count/(double)Resolusi)*pi*diameter*cos(225.0f*rtd); 00204 koordinat_x_BelakangKiri = ((double)Belakang_Kiri_Count/(double)Resolusi)*pi*diameter*cos(315.0f*rtd); 00205 00206 koordinat_y_DepanKiri = ((double)Depan_Kiri_Count/(double)Resolusi)*pi*diameter*sin(45.0f*rtd); 00207 koordinat_y_DepanKanan = ((double)Depan_Kanan_Count/(double)Resolusi)*pi*diameter*sin(135.0f*rtd); 00208 koordinat_y_BelakangKanan = ((double)Belakang_Kanan_Count/(double)Resolusi)*pi*diameter*sin(225.0f*rtd); 00209 koordinat_y_BelakangKiri = ((double)Belakang_Kiri_Count/(double)Resolusi)*pi*diameter*sin(315.0f*rtd); 00210 00211 orientasi = ((double)(koordinat_x_DepanKanan + koordinat_x_BelakangKanan) - (double)(koordinat_x_DepanKiri + koordinat_x_BelakangKiri))/(4.0f*13.0f); 00212 orientasi = orientasi*57.29; 00213 koordinat_y = (koordinat_x_DepanKiri + koordinat_x_DepanKanan + koordinat_x_BelakangKiri + koordinat_x_BelakangKanan)/2; 00214 koordinat_x = (koordinat_y_DepanKiri + koordinat_y_DepanKanan + koordinat_y_BelakangKiri + koordinat_y_BelakangKanan)/2; 00215 } 00216 00217 int cari_abs(float a) 00218 { 00219 if(a<0) 00220 { 00221 return -1; 00222 } 00223 else if(a>0) 00224 { 00225 return 1; 00226 } 00227 else 00228 { 00229 return 0; 00230 } 00231 } 00232 00233 void cari_inverse(float setpoint_sudut, float target_x, float target_y) 00234 { 00235 target_x = target_x - koordinat_x; 00236 target_y = target_y - koordinat_y; 00237 float target_vx; 00238 float target_vy; 00239 target_x = (double)target_x/100.0f; 00240 target_y = (double)target_y/100.0f; 00241 if(fabs(target_x)>fabs(target_y)) 00242 { 00243 if((target_x<0)&&(target_y<0)) 00244 { 00245 target_vx = -0.6; 00246 target_vy = ((double)target_y/(double)target_x)*target_vx; 00247 } 00248 else if((target_x<0)&&(target_y>0)) 00249 { 00250 target_vx = -0.6; 00251 target_vy = ((double)target_y/(double)target_x)*target_vx*-1; 00252 } 00253 else if((target_x>0)&&(target_y>0)) 00254 { 00255 target_vx = 0.6; 00256 target_vy = ((double)target_y/(double)target_x)*target_vx; 00257 } 00258 else if((target_x>0)&&(target_y<0)) 00259 { 00260 target_vx = 0.6; 00261 target_vy = ((double)target_y/(double)target_x)*target_vx*-1; 00262 } 00263 else if((target_y==0)&&(target_x>0)) 00264 { 00265 target_vx = 0.6; 00266 target_vy = 0; 00267 } 00268 else if((target_y==0)&&(target_x<0)) 00269 { 00270 target_vx = -0.6; 00271 target_vy = 0; 00272 } 00273 } 00274 else if(fabs(target_x)<fabs(target_y)) 00275 { 00276 if((target_x<0)&&(target_y<0)) 00277 { 00278 target_vy = -0.6; 00279 target_vx = ((double)target_x/(double)target_y)*target_vy; 00280 } 00281 else if((target_x<0)&&(target_y>0)) 00282 { 00283 target_vy = -0.6; 00284 target_vx = ((double)target_x/(double)target_y)*target_vy*-1; 00285 } 00286 else if((target_x>0)&&(target_y>0)) 00287 { 00288 target_vy = 0.6; 00289 target_vx = ((double)target_x/(double)target_y)*target_vy; 00290 } 00291 else if((target_x>0)&&(target_y<0)) 00292 { 00293 target_vy = 0.6; 00294 target_vx = ((double)target_x/(double)target_y)*target_vy*-1; 00295 } 00296 else if((target_x==0)&&(target_y>0)) 00297 { 00298 target_vy = 0.6; 00299 target_vx = 0; 00300 } 00301 else if((target_x==0)&&(target_y<0)) 00302 { 00303 target_vy = -0.6; 00304 target_vx = 0; 00305 } 00306 } 00307 else if(target_x==target_y) 00308 { 00309 if((target_x<0)&&(target_y<0)) 00310 { 00311 target_vy = -0.6; 00312 target_vx = target_vy; 00313 } 00314 else if((target_x<0)&&(target_y>0)) 00315 { 00316 target_vy = -0.6; 00317 target_vx = -1*target_vy; 00318 } 00319 else if((target_x>0)&&(target_y<0)) 00320 { 00321 target_vy = 0.6; 00322 target_vx = -1*target_vy; 00323 } 00324 else if((target_x>0)&&(target_y>0)) 00325 { 00326 target_vy = 0.6; 00327 target_vx = target_vy; 00328 } 00329 } 00330 setpoint_sudut = setpoint_sudut*rtd; 00331 target_depan_kiri = (1/radius)*(sin(45.0f*rtd)*target_vx + cos(45.0f*rtd)*target_vy + radius*setpoint_sudut); 00332 target_depan_kanan = (1/radius)*(sin(135.0f*rtd)*target_vx + cos(135.0f*rtd)*target_vy + radius*setpoint_sudut); 00333 target_belakang_kanan = (1/radius)*(sin(225.0f*rtd)*target_vx + cos(225.0f*rtd)*target_vy + radius*setpoint_sudut); 00334 target_belakang_kiri = (1/radius)*(sin(315.0f*rtd)*target_vx + cos(315.0f*rtd)*target_vy + radius*setpoint_sudut); 00335 target_depan_kiri = (target_depan_kiri*624.0f)/(2.0f*3.14); 00336 target_depan_kanan = (target_depan_kanan*624.0f)/(2.0f*3.14); 00337 target_belakang_kiri = (target_belakang_kiri*624.0f)/(2.0f*3.14); 00338 target_belakang_kanan = (target_belakang_kanan*624.0f)/(2.0f*3.14); 00339 arah_depan_kiri = cari_abs(target_depan_kiri); 00340 arah_depan_kanan = cari_abs(target_depan_kanan); 00341 arah_belakang_kiri = cari_abs(target_belakang_kiri); 00342 arah_belakang_kanan = cari_abs(target_belakang_kanan); 00343 } 00344 00345 void set_inverse() 00346 { 00347 DepanKananPID.setSetPoint(fabs(target_depan_kanan)); 00348 BelakangKananPID.setSetPoint(fabs(target_belakang_kanan)); 00349 BelakangKiriPID.setSetPoint(fabs(target_belakang_kiri)); 00350 DepanKiriPID.setSetPoint(fabs(target_depan_kiri)); 00351 } 00352 00353 void jalan_maju() 00354 { 00355 t_jalan.reset(); 00356 t_jalan.start(); 00357 proses_kecepatan(); 00358 Get_Count(); 00359 Cari_Koordinat(); 00360 pc.printf("Koordinat X : %d\n", koordinat_x); 00361 pc.printf("Koordinat Y : %d\n", koordinat_y); 00362 BelakangKanan.speed(BelakangKananPID.compute()*-1); 00363 BelakangKiri.speed(BelakangKiriPID.compute()); 00364 DepanKanan.speed(DepanKananPID.compute()*-1); 00365 DepanKiri.speed(DepanKiriPID.compute()); 00366 while(t_jalan.read_ms()<=20) 00367 { 00368 } 00369 t_jalan.reset(); 00370 } 00371 00372 void jalan_mundur() 00373 { 00374 t_jalan.reset(); 00375 t_jalan.start(); 00376 proses_kecepatan(); 00377 Get_Count(); 00378 Cari_Koordinat(); 00379 pc.printf("Koordinat X : %d\n", koordinat_x); 00380 pc.printf("Koordinat Y : %d\n", koordinat_y); 00381 BelakangKanan.speed(BelakangKananPID.compute()); 00382 BelakangKiri.speed(BelakangKiriPID.compute()*-1); 00383 DepanKanan.speed(DepanKananPID.compute()); 00384 DepanKiri.speed(DepanKiriPID.compute()*-1); 00385 while(t_jalan.read_ms()<=20) 00386 { 00387 } 00388 t_jalan.reset(); 00389 } 00390 00391 void jalan_kanan() 00392 { 00393 t_jalan.reset(); 00394 t_jalan.start(); 00395 proses_kecepatan(); 00396 Get_Count(); 00397 Cari_Koordinat(); 00398 pc.printf("Koordinat X : %d\n", koordinat_x); 00399 pc.printf("Koordinat Y : %d\n", koordinat_y); 00400 BelakangKanan.speed(BelakangKananPID.compute()*-1); 00401 BelakangKiri.speed(BelakangKiriPID.compute()*-1); 00402 DepanKanan.speed(DepanKananPID.compute()); 00403 DepanKiri.speed(DepanKiriPID.compute()); 00404 while(t_jalan.read_ms()<=20) 00405 { 00406 } 00407 t_jalan.reset(); 00408 } 00409 00410 void jalan_kiri() 00411 { 00412 t_jalan.reset(); 00413 t_jalan.start(); 00414 proses_kecepatan(); 00415 Get_Count(); 00416 Cari_Koordinat(); 00417 pc.printf("Koordinat X : %d\n", koordinat_x); 00418 pc.printf("Koordinat Y : %d\n", koordinat_y); 00419 BelakangKanan.speed(BelakangKananPID.compute()); 00420 BelakangKiri.speed(BelakangKiriPID.compute()); 00421 DepanKanan.speed(DepanKananPID.compute()*-1); 00422 DepanKiri.speed(DepanKiriPID.compute()*-1); 00423 while(t_jalan.read_ms()<=20) 00424 { 00425 } 00426 t_jalan.reset(); 00427 } 00428 00429 void berhenti() 00430 { 00431 t_jalan.reset(); 00432 t_jalan.start(); 00433 pc.printf("Koordinat X : %d\n", koordinat_x); 00434 pc.printf("Koordinat Y : %d\n", koordinat_y); 00435 DepanKanan.brake(0); 00436 BelakangKanan.brake(0); 00437 BelakangKiri.brake(0); 00438 DepanKiri.brake(0); 00439 while(t_jalan.read_ms()<=20) 00440 { 00441 } 00442 t_jalan.reset(); 00443 } 00444 00445 void ke_setpoint_x() 00446 { 00447 if(koordinat_x<setpoint_x) 00448 { 00449 kosongkan(); 00450 while(koordinat_x<setpoint_x) 00451 { 00452 jalan_kanan(); 00453 } 00454 DepanKanan.brake(); 00455 BelakangKanan.brake(); 00456 BelakangKiri.brake(); 00457 DepanKiri.brake(); 00458 } 00459 else if(koordinat_x>setpoint_x) 00460 { 00461 kosongkan(); 00462 while(koordinat_x>setpoint_x) 00463 { 00464 jalan_kiri(); 00465 } 00466 DepanKanan.brake(); 00467 BelakangKanan.brake(); 00468 BelakangKiri.brake(); 00469 DepanKiri.brake(); 00470 } 00471 else if(koordinat_x==setpoint_x) 00472 { 00473 berhenti(); 00474 } 00475 } 00476 00477 void ke_setpoint_y() 00478 { 00479 if(koordinat_y<setpoint_y) 00480 { 00481 kosongkan(); 00482 while(koordinat_y<setpoint_y) 00483 { 00484 jalan_maju(); 00485 } 00486 DepanKanan.brake(); 00487 BelakangKanan.brake(); 00488 BelakangKiri.brake(); 00489 DepanKiri.brake(); 00490 } 00491 else if(koordinat_y>setpoint_y) 00492 { 00493 kosongkan(); 00494 while(koordinat_y>setpoint_y) 00495 { 00496 jalan_mundur(); 00497 } 00498 DepanKanan.brake(); 00499 BelakangKanan.brake(); 00500 BelakangKiri.brake(); 00501 DepanKiri.brake(); 00502 } 00503 else if(koordinat_y==setpoint_y) 00504 { 00505 berhenti(); 00506 } 00507 } 00508 00509 int main() 00510 { 00511 pc.baud(38400); 00512 bluetooth.baud(9600); 00513 bluetooth.attach(&fungsi_bluetooth, Serial::RxIrq); 00514 00515 DepanKanan.period(0.01f); 00516 BelakangKanan.period(0.01f); 00517 BelakangKiri.period(0.01f); //Set motor PWM periods to 20KHz. 00518 DepanKiri.period(0.01f); 00519 00520 DepanKananPID.setInputLimits(0, 3000);//Input units: counts per second. 00521 DepanKananPID.setOutputLimits(0.0, 0.9);//Output units: PwmOut duty cycle as %. 00522 DepanKananPID.setMode(AUTO_MODE); 00523 00524 BelakangKananPID.setInputLimits(0, 3000); 00525 BelakangKananPID.setOutputLimits(0.0, 0.9); 00526 BelakangKananPID.setMode(AUTO_MODE); 00527 00528 BelakangKiriPID.setInputLimits(0, 3000); 00529 BelakangKiriPID.setOutputLimits(0.0, 0.9); 00530 BelakangKiriPID.setMode(AUTO_MODE); 00531 00532 DepanKiriPID.setInputLimits(0, 3000); 00533 DepanKiriPID.setOutputLimits(0.0, 0.9); 00534 DepanKiriPID.setMode(AUTO_MODE); 00535 00536 DepanKananPID.setSetPoint(1400); 00537 BelakangKananPID.setSetPoint(1400); 00538 BelakangKiriPID.setSetPoint(1400); 00539 DepanKiriPID.setSetPoint(1400); 00540 while(1) 00541 { 00542 00543 if(Jalan == Jalan_Maju) 00544 { 00545 kosongkan(); 00546 while(Jalan== Jalan_Maju) 00547 { 00548 t_jalan.reset(); 00549 t_jalan.start(); 00550 proses_kecepatan(); 00551 Get_Count(); 00552 Cari_Koordinat(); 00553 //pc.printf("Koordinat X : %d\n", koordinat_x); 00554 //pc.printf("Koordinat Y : %d\n", koordinat_y); 00555 pc.printf("Orientasi : %f\n", orientasi); 00556 BelakangKanan.speed(BelakangKananPID.compute()*-1); 00557 BelakangKiri.speed(BelakangKiriPID.compute()); 00558 DepanKanan.speed(DepanKananPID.compute()*-1); 00559 DepanKiri.speed(DepanKiriPID.compute()); 00560 while(t_jalan.read_ms()<=20) 00561 { 00562 } 00563 t_jalan.reset(); 00564 } 00565 DepanKanan.brake(); 00566 BelakangKanan.brake(); 00567 BelakangKiri.brake(); 00568 DepanKiri.brake(); 00569 } 00570 00571 else if(Jalan == Jalan_Mundur) 00572 { 00573 kosongkan(); 00574 while(Jalan== Jalan_Mundur) 00575 { 00576 t_jalan.reset(); 00577 t_jalan.start(); 00578 proses_kecepatan(); 00579 Get_Count(); 00580 Cari_Koordinat(); 00581 //pc.printf("Koordinat X : %d\n", koordinat_x); 00582 //pc.printf("Koordinat Y : %d\n", koordinat_y); 00583 pc.printf("Orientasi : %f\n", orientasi); 00584 BelakangKanan.speed(BelakangKananPID.compute()); 00585 BelakangKiri.speed(BelakangKiriPID.compute()*-1); 00586 DepanKanan.speed(DepanKananPID.compute()); 00587 DepanKiri.speed(DepanKiriPID.compute()*-1); 00588 while(t_jalan.read_ms()<=20) 00589 { 00590 } 00591 t_jalan.reset(); 00592 } 00593 DepanKanan.brake(); 00594 BelakangKanan.brake(); 00595 BelakangKiri.brake(); 00596 DepanKiri.brake(); 00597 } 00598 00599 else if(Jalan == Jalan_Kanan) 00600 { 00601 kosongkan(); 00602 while(Jalan== Jalan_Kanan) 00603 { 00604 t_jalan.reset(); 00605 t_jalan.start(); 00606 proses_kecepatan(); 00607 Get_Count(); 00608 Cari_Koordinat(); 00609 //pc.printf("Koordinat X : %d\n", koordinat_x); 00610 //pc.printf("Koordinat Y : %d\n", koordinat_y); 00611 pc.printf("Orientasi : %f\n", orientasi); 00612 BelakangKanan.speed(BelakangKananPID.compute()); 00613 BelakangKiri.speed(BelakangKiriPID.compute()); 00614 DepanKanan.speed(DepanKananPID.compute()); 00615 DepanKiri.speed(DepanKiriPID.compute()); 00616 while(t_jalan.read_ms()<=20) 00617 { 00618 } 00619 t_jalan.reset(); 00620 } 00621 DepanKanan.brake(); 00622 BelakangKanan.brake(); 00623 BelakangKiri.brake(); 00624 DepanKiri.brake(); 00625 } 00626 00627 else if(Jalan == Jalan_Kiri) 00628 { 00629 kosongkan(); 00630 while(Jalan== Jalan_Kiri) 00631 { 00632 t_jalan.reset(); 00633 t_jalan.start(); 00634 proses_kecepatan(); 00635 Get_Count(); 00636 Cari_Koordinat(); 00637 //pc.printf("Koordinat X : %d\n", koordinat_x); 00638 //pc.printf("Koordinat Y : %d\n", koordinat_y); 00639 pc.printf("Orientasi : %f\n", orientasi); 00640 BelakangKanan.speed(BelakangKananPID.compute()*-1); 00641 BelakangKiri.speed(BelakangKiriPID.compute()*-1); 00642 DepanKanan.speed(DepanKananPID.compute()*-1); 00643 DepanKiri.speed(DepanKiriPID.compute()*-1); 00644 while(t_jalan.read_ms()<=20) 00645 { 00646 } 00647 t_jalan.reset(); 00648 } 00649 DepanKanan.brake(); 00650 BelakangKanan.brake(); 00651 BelakangKiri.brake(); 00652 DepanKiri.brake(); 00653 } 00654 else if(Jalan == ke_setpoint) 00655 { 00656 DepanKananPID.setSetPoint(1400); 00657 BelakangKananPID.setSetPoint(1400); 00658 BelakangKiriPID.setSetPoint(1400); 00659 DepanKiriPID.setSetPoint(1400); 00660 while(Jalan == ke_setpoint) 00661 { 00662 ke_setpoint_x(); 00663 ke_setpoint_y(); 00664 } 00665 } 00666 else if(Jalan == mode_inverse) 00667 { 00668 kosongkan(); 00669 cari_inverse(0, (float)setpoint_x, (float)setpoint_y); 00670 set_inverse(); 00671 while((koordinat_x!=setpoint_x)&&(koordinat_y!=setpoint_y)) 00672 { 00673 t_jalan.reset(); 00674 t_jalan.start(); 00675 proses_kecepatan(); 00676 Get_Count(); 00677 Cari_Koordinat(); 00678 BelakangKanan.speed(BelakangKananPID.compute()*arah_belakang_kanan); 00679 BelakangKiri.speed(BelakangKiriPID.compute()*arah_belakang_kiri); 00680 DepanKanan.speed(DepanKananPID.compute()*arah_depan_kanan); 00681 DepanKiri.speed(DepanKiriPID.compute()*arah_depan_kiri); 00682 pc.printf("Koordinat X : %d\n", koordinat_x); 00683 pc.printf("Koordinat Y : %d\n", koordinat_y); 00684 while(t_jalan.read_ms()<=20) 00685 { 00686 } 00687 t_jalan.reset(); 00688 } 00689 DepanKanan.brake(); 00690 BelakangKanan.brake(); 00691 BelakangKiri.brake(); 00692 DepanKiri.brake(); 00693 } 00694 else if(Jalan == Berhenti) 00695 { 00696 while(Jalan==Berhenti) 00697 { 00698 t_jalan.reset(); 00699 t_jalan.start(); 00700 pc.printf("Koordinat X : %d\n", koordinat_x); 00701 pc.printf("Koordinat Y : %d\n", koordinat_y); 00702 //pc.printf("Orientasi : %f\n", orientasi); 00703 DepanKanan.brake(0); 00704 BelakangKanan.brake(0); 00705 BelakangKiri.brake(0); 00706 DepanKiri.brake(0); 00707 while(t_jalan.read_ms()<=20) 00708 { 00709 } 00710 t_jalan.reset(); 00711 } 00712 } 00713 } 00714 }
Generated on Mon Oct 31 2022 12:40:30 by
1.7.2