KRAI 2017
road to japan
Dependencies: DigitDisplay Motor PID Ping mbed millis
Fork of
MainProgram_BaseBaru_fix_omni_20April
by 
KRAI 2017
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 /*tuning motor baru untuk konstanta pid baru */ 00002 /****************************************************************************/ 00003 /* PROGRAM UNTUK PID CLOSED LOOP */ 00004 /* */ 00005 /* Last Update : 16 April 2017 */ 00006 /* */ 00007 /* - Digunakan encoder autonics */ 00008 /* - Konfigurasi Motor dan Encoder sbb : */ 00009 /* ______________________ */ 00010 /* / \ Rode Depan Belakang: */ 00011 /* / 2 (Belakang) \ Omniwheel */ 00012 /* | | */ 00013 /* | 3 (kiri) 4 (kanan) | Roda Kiri Kanan: */ 00014 /* | | Omniwheel */ 00015 /* \ 1 (Depan) / */ 00016 /* \______________________/ Putaran berlawanan arah */ 00017 /* jarum jam positif */ 00018 /* SETTINGS (WAJIB!) : */ 00019 /* 1. Settings Pin Encoder, Resolusi, dan Tipe encoding di omniPos.h */ 00020 /* 2. Deklarasi penggunaan library omniPos pada bagian deklarasi encoder */ 00021 /* */ 00022 /****************************************************************************/ 00023 /* */ 00024 /* Joystick */ 00025 /* Kanan => */ 00026 /* Kiri => */ 00027 /* */ 00028 /* Tombol silang => Pneumatik aktif */ 00029 /* Tombol segitiga => Aktif motor Launcher */ 00030 /* Tombol lingkaran => Reloader naik */ 00031 /* Tombol kotak => Reloader turun */ 00032 /* Tombol L1 => Pivot Kiri */ 00033 /* Tombol R1 => Pivot Kanan */ 00034 /* Tombol L2 => Kurang PWM Motor Launcher */ 00035 /* Tombol R2 => Tambah PWM Motor Launcher */ 00036 /* */ 00037 /* Bismillahirahmanirrahim */ 00038 /* Jagalah Kebersihan Kodingan */ 00039 /****************************************************************************/ 00040 00041 #include "mbed.h" 00042 #include "JoystickPS3.h" 00043 #include "Motor.h" 00044 #include "encoderKRAI.h" 00045 #include "millis.h" 00046 #include "Ping.h" 00047 #include "DigitDisplay.h" 00048 00049 /***********************************************/ 00050 /* Konstanta dan Variabel */ 00051 /***********************************************/ 00052 #define PI 3.14159265 00053 #define D_ENCODER 10 // Diameter Roda Encoder 00054 #define D_ROBOT 80 // Diameter Roda Robot 00055 00056 // Variable Atas 00057 // indek 2 untuk motor depan, 1 untuk motor belakang 00058 double speed, speed2; 00059 const double maxSpeed = 0.95, minSpeed = -0.95, Ts = 12.5; 00060 const double kpA1=0.1478, kdA1=0.9295, kiA1=0.0004226; 00061 const double kpA2=0.13978, kdA2=0.9222, kiA2=0.00038636; 00062 double a1,b1,c1; 00063 double a2,b2,c2; 00064 double current_error1, previous_error1_1 = 0, previous_error1_2 = 0; 00065 double current_error2, previous_error2_1 = 0, previous_error2_2 = 0; 00066 double previous_speed1 = 0; 00067 double previous_speed2 = 0; 00068 00069 float rpm, rpm2; 00070 double target_rpm = 18.0, target_rpm2 = 18.0; // selisih 4 bagus, sama bagus 00071 const float maxRPM = 35, minRPM = 0; // Limit 25 atau 27 00072 00073 const float pwmPowerUp = 1.0; 00074 const float pwmPowerDown = -1.0; 00075 00076 double jarak_ping=0; 00077 double ping_target = 14; 00078 double ping_current_error, ping_previous_error1 = 0, ping_sum_error=0; 00079 double ping_Kp = -0.2747, ping_Kd =0, ping_Ts=10; 00080 double ping_pwm, ping_previous_pwm = 0; 00081 00082 // Variable Bawah 00083 float PIVOT = 0.17; // PWM Pivot Kanan, Pivot Kiri 00084 float tuneDpn = 1.0; // Tunning PWM motor Depan 00085 float tuneBlk = 1.0; // Tunning PWM motor belakang 00086 float tuneAksel = 0.6; 00087 int aksel = 0; 00088 float tuneAkselBlk = 0.4; 00089 float tuneR = 1.0; 00090 float tuneL = 1.0; 00091 float serong = 0.4; 00092 float rasio = 1.4545; 00093 float t_new = 0.1; 00094 00095 /* variable tunning */ 00096 float tunning_L; 00097 float tunning_R; 00098 float tunning_Dpn; 00099 float tunning_Blk; 00100 00101 /* Deklarasi Variable Millis */ 00102 unsigned long int previousMillis = 0; // PID launcher 00103 unsigned long int currentMillis; 00104 unsigned long int previousMillis2 = 0; // PID launcher 00105 unsigned long int currentMillis2; 00106 unsigned long int previousMillis3 = 0; // Pneumatik 00107 unsigned long int previousMillis4 = 0; // Ping 00108 unsigned long int previousMillis5 = 0; // Display 00109 00110 /* Variabel Stick */ 00111 //Logic untuk masuk aktuator 00112 int case_joy; 00113 bool isLauncher = false; 00114 bool isReload = false; 00115 bool ReloadOn = false; 00116 bool flag_Pneu = false; 00117 bool flag_paku = false; 00118 bool ready = false; 00119 00120 /*****************************************************/ 00121 /* Definisi Prosedur, Fungsi dan Setting Pinout */ 00122 /*****************************************************/ 00123 00124 /* Fungsi dan Procedur Encoder */ 00125 void init_speed(); // 00126 void aktuator(); // Pergerakan aktuator berdasarkan case joystick 00127 int case_joystick(); // Mendapatkan case dari joystick 00128 //void speedKalibrasiMotor(); // Pertambahan target RPM motor atas melalui joystick 00129 00130 /* Inisialisasi Pin TX-RX Joystik dan PC */ 00131 joysticknucleo joystick(PA_0,PA_1); 00132 Serial pc(USBTX,USBRX); 00133 00134 /* Deklarasi Encoder Base */ 00135 encoderKRAI encoderBase(PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), 2xresolusi, mode pembacaan 00136 00137 /* Deklarasi Encoder Launcher */ 00138 encoderKRAI encLauncherDpn( PC_10, PC_11, 28, encoderKRAI::X4_ENCODING); 00139 encoderKRAI encLauncherBlk( PC_12, PD_2, 28, encoderKRAI::X4_ENCODING); 00140 00141 /* Deklarasi Motor Base */ 00142 Motor motorDpn(PB_7, PC_3, PC_0); //(PB_9, PA_12, PC_5); 00143 //Motor motorBlk(PB_6, PC_14, PC_13); //(PB_6, PB_1, PB_12); (PC_7, PC_14, PC_13); 00144 Motor motorBlk(PB_2, PB_15, PB_1); 00145 Motor motorL (PB_9, PA_12, PA_6); 00146 Motor motorR (PB_8, PC_6, PC_5); //(PC_6, PB_4, PB_5); 00147 00148 /* Deklarasi Motor Launcher */ 00149 Motor launcherDpn(PA_5,PA_11,PB_12); // pwm ,fwd, rev 00150 Motor launcherBlk(PB_3, PC_4, PA_10); // pwm, fwd, rev 00151 Motor powerScrew(PB_10, PB_14, PB_13); // pwm, fwd, rev 00152 00153 /* Deklarasi Penumatik Launcher */ 00154 DigitalOut pneumatik(PA_4, PullUp); 00155 DigitalOut pneu_paku(PC_2, PullUp); 00156 00157 /*Dekalrasi Limit Switch */ 00158 //DigitalIn infraAtas(PC_9, PullUp); 00159 DigitalIn limitTengah(PA_9, PullUp); 00160 DigitalIn limitBawah(PC_7, PullUp); 00161 DigitalIn limitBawah1(PA_7, PullUp); 00162 00163 /*deklarasi PING ultrasonic*/ 00164 Ping pingAtas(PC_15); 00165 00166 /*Deklarasi Display*/ 00167 DigitDisplay display (PA_8, PC_8); 00168 00169 /****************************************************/ 00170 /* Deklarasi Fungsi dan Procedure */ 00171 /****************************************************/ 00172 int case_joystick() 00173 { 00174 //---------------------------------------------------// 00175 // Gerak Motor Base // 00176 // Case 1 : Pivot kanan // 00177 // Case 2 : Pivot Kiri // 00178 // Case 3 : Kanan // 00179 // Case 4 : Kiri // 00180 // Case 5 : Break // 00181 //---------------------------------------------------// 00182 00183 int caseJoystick; 00184 if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { 00185 // Pivot Kanan 00186 caseJoystick = 1; 00187 } 00188 else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { 00189 // Pivot Kiri 00190 caseJoystick = 2; 00191 } 00192 else if ((joystick.START_click)&&(!joystick.SELECT_click)&&(!joystick.R3_click)) { 00193 // tambah rpm dengan nilai tertentu 00194 caseJoystick = 31; 00195 } 00196 else if ((!joystick.START_click)&&(joystick.SELECT_click)&&(!joystick.R3_click)) { 00197 // kurangi rpm dengan nilai tertentu 00198 caseJoystick = 32; 00199 } 00200 else if ((!joystick.START_click)&&(!joystick.SELECT_click)&&(joystick.R3_click)) { 00201 // kurangi rpm dengan nilai tertentu 00202 caseJoystick = 33; 00203 } 00204 else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) { 00205 // Kanan + Rotasi kanan 00206 caseJoystick = 17; 00207 } 00208 else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) { 00209 // Kanan + Rotasi kiri 00210 caseJoystick = 18; 00211 } 00212 else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { 00213 // Kiri + Rotasi kanan 00214 caseJoystick = 19; 00215 } 00216 else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { 00217 // Kanan + Rotasi kiri 00218 caseJoystick = 20; 00219 } 00220 else if ((joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { 00221 // Maju + Rotasi kanan 00222 caseJoystick = 21; 00223 } 00224 else if ((!joystick.R1)&&(joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { 00225 // Maju + Rotasi kiri 00226 caseJoystick = 22; 00227 } 00228 else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { 00229 // Mundur + Rotasi kanan 00230 caseJoystick = 23; 00231 } 00232 else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { 00233 // Mundur + Rotasi kiri 00234 caseJoystick = 24; 00235 } 00236 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.segitiga_click)) { 00237 // Kanan + segitiga 00238 caseJoystick = 25; 00239 } 00240 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.segitiga_click)) { 00241 // Kiri + segitiga 00242 caseJoystick = 26; 00243 } 00244 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.lingkaran_click)) { 00245 // Kanan + lingkaran 00246 caseJoystick = 27; 00247 } 00248 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.lingkaran_click)) { 00249 // Kiri + lingkaran 00250 caseJoystick = 28; 00251 } 00252 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.kotak_click)) { 00253 // Kanan + kotak 00254 caseJoystick = 29; 00255 } 00256 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.kotak_click)) { 00257 // Kiri + kotak 00258 caseJoystick = 30; 00259 } 00260 else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) { 00261 // Serong kanan maju 00262 caseJoystick = 13; 00263 } 00264 else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { 00265 // Serong kiri maju 00266 caseJoystick = 14; 00267 } 00268 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) { 00269 // Serong kanan mundur 00270 caseJoystick = 15; 00271 } 00272 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { 00273 // Serong kiri mundur 00274 caseJoystick = 16; 00275 } 00276 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) { 00277 // Kanan 00278 caseJoystick = 3; 00279 } 00280 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { 00281 // Kiri 00282 caseJoystick = 4; 00283 } 00284 else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { 00285 // Atas -- Maju 00286 caseJoystick = 8; 00287 } 00288 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { 00289 // Bawah -- Mundur 00290 caseJoystick = 9; 00291 } 00292 else if (joystick.segitiga_click){ 00293 // Motor Launcher 00294 caseJoystick = 5; 00295 } 00296 else if (joystick.R2_click){ 00297 // Target Pulse PID ++ Motor Depan 00298 caseJoystick = 6; 00299 } 00300 else if (joystick.L2_click){ 00301 // Target Pulse PID -- Motor 00302 caseJoystick = 7; 00303 } 00304 else if (joystick.silang_click){ 00305 // Pnemuatik ON 00306 caseJoystick = 10; 00307 } 00308 else if ((joystick.lingkaran_click)&&(!joystick.kotak_click)) { 00309 // Power Screw Up 00310 caseJoystick = 11; 00311 } 00312 else if ((joystick.kotak_click)&&(!joystick.lingkaran_click)) { 00313 // Power Screw Down 00314 caseJoystick = 12; 00315 } 00316 else if (joystick.L3){ 00317 // Paku Bumi ON/OFF 00318 caseJoystick = 34; 00319 } 00320 else 00321 { 00322 tuneAksel = 0.6; 00323 aksel = 0; 00324 } 00325 00326 return(caseJoystick); 00327 } 00328 00329 00330 void init_rpm (){ 00331 if (target_rpm>maxRPM-2){ 00332 target_rpm = maxRPM-2; 00333 } 00334 if (target_rpm<minRPM){ 00335 target_rpm = minRPM; 00336 } 00337 if (target_rpm2>maxRPM){ 00338 target_rpm2 = maxRPM; 00339 } 00340 if (target_rpm2<minRPM+2){ 00341 target_rpm2 = minRPM+2; 00342 } 00343 } 00344 00345 void aktuator() 00346 { 00347 switch (case_joy) { 00348 case (1): 00349 { 00350 // Pivot Kanan 00351 motorDpn.speed(-PIVOT); 00352 motorBlk.speed(-PIVOT); 00353 motorR.speed(-rasio*PIVOT-t_new); 00354 motorL.speed(-rasio*PIVOT-t_new); 00355 break; 00356 } 00357 case (2): 00358 { 00359 // Pivot Kiri 00360 motorDpn.speed(PIVOT); 00361 motorBlk.speed(PIVOT); 00362 motorR.speed(rasio*PIVOT+t_new); 00363 motorL.speed(rasio*PIVOT+t_new); 00364 break; 00365 } 00366 case (17): 00367 { 00368 // Kanan + Rotasi Kanan 00369 motorDpn.speed(-PIVOT); 00370 motorBlk.speed(-PIVOT); 00371 motorR.speed(-rasio*PIVOT-t_new); 00372 motorL.speed(-rasio*PIVOT-t_new); 00373 break; 00374 } 00375 case (18): 00376 { 00377 // Kanan + Rotasi Kiri 00378 motorDpn.speed(PIVOT); 00379 motorBlk.speed(PIVOT); 00380 motorR.speed(rasio*PIVOT+t_new); 00381 motorL.speed(rasio*PIVOT+t_new); 00382 break; 00383 } 00384 case (19): 00385 { 00386 // Kiri + Rotasi Kanan 00387 motorDpn.speed(-PIVOT); 00388 motorBlk.speed(-PIVOT); 00389 motorR.speed(-rasio*PIVOT-t_new); 00390 motorL.speed(-rasio*PIVOT-t_new); 00391 break; 00392 } 00393 case (20): 00394 { 00395 // Kiri + Rotasi Kiri 00396 motorDpn.speed(PIVOT); 00397 motorBlk.speed(PIVOT); 00398 motorR.speed(rasio*PIVOT+t_new); 00399 motorL.speed(rasio*PIVOT+t_new); 00400 break; 00401 } 00402 case (21): 00403 { 00404 // Maju + Rotasi Kanan 00405 motorDpn.speed(-PIVOT); 00406 motorBlk.speed(-PIVOT); 00407 motorR.speed(-rasio*PIVOT-t_new); 00408 motorL.speed(-rasio*PIVOT-t_new); 00409 break; 00410 } 00411 case (22): 00412 { 00413 // Maju + Rotasi Kiri 00414 motorDpn.speed(PIVOT); 00415 motorBlk.speed(PIVOT); 00416 motorR.speed(rasio*PIVOT+t_new); 00417 motorL.speed(rasio*PIVOT+t_new); 00418 break; 00419 } 00420 case (23): 00421 { 00422 // Mundur + Rotasi Kanan 00423 motorDpn.speed(-PIVOT); 00424 motorBlk.speed(-PIVOT); 00425 motorR.speed(-rasio*PIVOT-t_new); 00426 motorL.speed(-rasio*PIVOT-t_new); 00427 break; 00428 } 00429 case (24): 00430 { 00431 // Mundur + Rotasi Kiri 00432 motorDpn.speed(PIVOT); 00433 motorBlk.speed(PIVOT); 00434 motorR.speed(rasio*PIVOT+t_new); 00435 motorL.speed(rasio*PIVOT+t_new); 00436 break; 00437 } 00438 case (25): 00439 { 00440 // Kanan + segitiga 00441 isLauncher = !isLauncher; 00442 break; 00443 } 00444 case (26): 00445 { 00446 // Kiri + segitiga 00447 isLauncher = !isLauncher; 00448 break; 00449 } 00450 case (27): 00451 { 00452 // Kanan + lingkaran 00453 ReloadOn = !ReloadOn; 00454 isReload = false; 00455 break; 00456 } 00457 case (28): 00458 { 00459 // Kiri + lingkaran 00460 ReloadOn = !ReloadOn; 00461 isReload = false; 00462 break; 00463 } 00464 case (29): 00465 { 00466 // Kanan + kotak 00467 ReloadOn = !ReloadOn; 00468 isReload = true; 00469 break; 00470 } 00471 case (30): 00472 { 00473 // Kiri + kotak 00474 ReloadOn = !ReloadOn; 00475 isReload = true; 00476 break; 00477 } 00478 case (13) : 00479 { 00480 // Serong kanan maju 00481 motorDpn.speed(-serong); 00482 motorL.speed(-serong-t_new); 00483 motorBlk.speed(serong); 00484 motorR.speed(serong+t_new); 00485 break; 00486 } 00487 case (14) : 00488 { 00489 // Serong kiri maju 00490 motorDpn.speed(serong); 00491 motorR.speed(serong+t_new); 00492 motorBlk.speed(-serong); 00493 motorL.speed(-serong-t_new); 00494 break; 00495 } 00496 case (15) : 00497 { 00498 // Serong kanan mundur 00499 motorDpn.speed(-serong); 00500 motorR.speed(-serong-t_new); 00501 motorBlk.speed(serong); 00502 motorL.speed(serong+t_new); 00503 break; 00504 } 00505 case (16) : 00506 { 00507 // Serong kiri mundur 00508 motorDpn.speed(serong); 00509 motorL.speed(serong+t_new); 00510 motorBlk.speed(-serong); 00511 motorR.speed(-serong-t_new); 00512 break; 00513 } 00514 case (3) : 00515 { 00516 // Kanan 00517 aksel++; 00518 if (aksel>300) 00519 tuneAksel = 0.9; 00520 00521 motorDpn.speed(-tuneAksel); 00522 motorBlk.speed(tuneAksel); 00523 motorR.brake(1); 00524 motorL.brake(1); 00525 break; 00526 } 00527 case (4) : 00528 { 00529 // Kiri 00530 aksel++; 00531 if (aksel>300) 00532 tuneAksel = 0.9; 00533 00534 motorDpn.speed(tuneAksel); 00535 motorBlk.speed(-tuneAksel); 00536 motorR.brake(1); 00537 motorL.brake(1); 00538 break; 00539 } 00540 case (8) : 00541 { 00542 // Maju 00543 aksel++; 00544 if (aksel>300) 00545 tuneAksel = 0.9; 00546 00547 motorR.speed(tuneAksel); 00548 motorL.speed(-tuneAksel); 00549 motorDpn.brake(1); 00550 motorBlk.brake(1); 00551 break; 00552 } 00553 case (9) : 00554 { 00555 // Mundur 00556 aksel++; 00557 if (aksel>300) 00558 tuneAksel = 0.9; 00559 00560 motorR.speed(-tuneAksel); 00561 motorL.speed(tuneAksel); 00562 motorDpn.brake(1); 00563 motorBlk.brake(1); 00564 break; 00565 } 00566 case (5) : 00567 { 00568 // Aktifkan motor atas 00569 isLauncher = !isLauncher; 00570 break; 00571 } 00572 case (6) : 00573 { 00574 // Target Pulse PID ++ Motor Depan 00575 target_rpm2 = target_rpm2+1.0; 00576 target_rpm = target_rpm+1.0; 00577 init_rpm(); 00578 break; 00579 } 00580 case (7) : 00581 { 00582 // Target Pulse PID -- Motor Depan 00583 target_rpm2 = target_rpm2-1.0; 00584 target_rpm = target_rpm-1.0; 00585 init_rpm(); 00586 break; 00587 } 00588 case (10) : 00589 { 00590 // Pneumatik 00591 if (ready) 00592 { 00593 pneumatik = 0; 00594 previousMillis3 = millis(); 00595 flag_Pneu = true; 00596 ready = false; 00597 00598 } 00599 break; 00600 } 00601 case (11) : 00602 { 00603 // Power Screw Up 00604 ReloadOn = !ReloadOn; 00605 isReload = false; 00606 break; 00607 } 00608 case (31) : 00609 { 00610 // start 00611 target_rpm2 = 23; 00612 target_rpm = 23; 00613 init_rpm(); 00614 break; 00615 } 00616 case (32) : 00617 { 00618 // select 00619 target_rpm2 = 11; 00620 target_rpm = 11; 00621 init_rpm(); 00622 break; 00623 } 00624 case (33) : 00625 { 00626 // R3 00627 target_rpm2 = 18; 00628 target_rpm = 18; 00629 init_rpm(); 00630 break; 00631 } 00632 case (12) : 00633 { 00634 // Power Screw Down 00635 ReloadOn = !ReloadOn; 00636 isReload = true; 00637 break; 00638 } 00639 case (34) :{ 00640 pneu_paku = !pneu_paku; 00641 wait_ms(50); 00642 if (pneu_paku == 1){ 00643 PIVOT = 0.17; 00644 }else{ 00645 PIVOT = 0.8; 00646 } 00647 } 00648 default : 00649 { 00650 tuneAksel = 0.6; 00651 aksel = 0; 00652 motorDpn.brake(1); 00653 motorBlk.brake(1); 00654 motorR.brake(1); 00655 motorL.brake(1); 00656 } 00657 } // End Switch 00658 } 00659 00660 void reloader() 00661 { 00662 if(ReloadOn){ 00663 if(isReload){ 00664 powerScrew.speed(pwmPowerDown); 00665 pc.printf("%.2f\n", jarak_ping); 00666 if((!limitBawah)||(!limitBawah1)){ 00667 isReload = false; 00668 ReloadOn = false; 00669 } 00670 } 00671 else if(!limitTengah){ 00672 isReload = true; 00673 } 00674 else if(!flag_Pneu){ 00675 //pc.printf("%.2f\n", ping_pwm); 00676 ping_current_error = (double) (ping_target-jarak_ping); 00677 00678 ping_sum_error += ping_current_error*ping_Ts; 00679 ping_pwm = (double) ping_Kp*ping_current_error + ping_Kd*(ping_current_error-ping_previous_error1)/ping_Ts; 00680 00681 pc.printf("%.2f\n", jarak_ping); 00682 powerScrew.speed(ping_pwm); 00683 00684 ping_previous_error1 = ping_current_error; 00685 } 00686 if ((jarak_ping>(ping_target-2))&&(jarak_ping<(ping_target+2))){ 00687 ready = true; 00688 }else{ 00689 ready = false; 00690 } 00691 } 00692 else{ 00693 powerScrew.brake(1); 00694 } 00695 } 00696 00697 00698 void launcher() 00699 { 00700 if (isLauncher) 00701 { 00702 currentMillis = millis(); 00703 currentMillis2 = millis(); 00704 00705 // PID Launcher Belakang 00706 if (currentMillis-previousMillis>=Ts) 00707 { 00708 rpm = (float)encLauncherBlk.getPulses(); 00709 current_error1 = target_rpm - rpm; 00710 a1 = kpA1 + kiA1*Ts/2 + kdA1/Ts; 00711 b1 = -kpA1 + kiA1*Ts/2 - 2*kdA1/Ts; 00712 c1 = kdA1/Ts; 00713 speed = previous_speed1 + a1*current_error1 + b1*previous_error1_1 + c1*previous_error1_2; 00714 //init_speed(); 00715 if(speed > maxSpeed){ 00716 launcherBlk.speed(maxSpeed); 00717 } 00718 else if ( speed < minSpeed){ 00719 launcherBlk.speed(minSpeed); 00720 } 00721 else { 00722 launcherBlk.speed(speed); 00723 } 00724 previous_speed1 = speed; 00725 previous_error1_2 = previous_error1_1; 00726 previous_error1_1 = current_error1; 00727 encLauncherBlk.reset(); 00728 previousMillis = currentMillis; 00729 00730 } 00731 // PID Launcher Depan 00732 if (currentMillis2-previousMillis2>=Ts) 00733 { 00734 rpm2 = (float)encLauncherDpn.getPulses(); 00735 current_error2 = target_rpm2 - rpm2; 00736 a2 = kpA2 + kiA2*Ts/2 + kdA2/Ts; 00737 b2 = -kpA2 + kiA2*Ts/2 - 2*kdA2/Ts; 00738 c2 = kdA2/Ts; 00739 speed2 = previous_speed2 + a2*current_error2 + b2*previous_error2_1 + c2*previous_error2_2; 00740 //init_speed(); 00741 if (speed2 > maxSpeed){ 00742 launcherDpn.speed(maxSpeed); 00743 } 00744 else if ( speed2 < minSpeed){ 00745 launcherDpn.speed(minSpeed); 00746 } 00747 else{ 00748 launcherDpn.speed(speed2); 00749 } 00750 previous_speed2 = speed2; 00751 previous_error2_2 = previous_error2_1; 00752 previous_error2_1 = current_error2; 00753 encLauncherDpn.reset(); 00754 previousMillis2 = currentMillis2; 00755 } 00756 //pc.printf("%.2f\t%.2f\n",rpm,rpm2); 00757 } 00758 else 00759 { 00760 launcherDpn.brake(1); 00761 launcherBlk.brake(1); 00762 00763 previous_error1_1 = 0; 00764 previous_error1_2 = 0; 00765 previous_error2_1 = 0; 00766 previous_error2_2 = 0; 00767 previous_speed1 = 0; 00768 previous_speed2 = 0; 00769 } 00770 } 00771 00772 /*********************************************************/ 00773 /* Main Function */ 00774 /*********************************************************/ 00775 00776 int main (void) 00777 { 00778 // Set baud rate - 115200 00779 joystick.setup(); 00780 pc.baud(115200); 00781 wait_ms(1000); 00782 00783 // initializing encoder 00784 pneumatik =1; 00785 00786 wait_ms(500); 00787 00788 //initializing PING 00789 pingAtas.Send(); 00790 00791 pc.printf("ready...."); 00792 startMillis(); 00793 while(1) 00794 { 00795 // interupsi pembacaan PING setiap 30 ms 00796 if(millis() - previousMillis4 >= 10){ //30 00797 jarak_ping = (float)pingAtas.Read_cm(); 00798 00799 pingAtas.Send(); 00800 previousMillis4 = millis(); 00801 } 00802 00803 // Interrupt Serial 00804 joystick.idle(); 00805 if(joystick.readable()) 00806 { 00807 // Panggil fungsi pembacaan joystik 00808 joystick.baca_data(); 00809 // Panggil fungsi pengolahan data joystik 00810 joystick.olah_data(); 00811 // Masuk ke case joystick 00812 case_joy = case_joystick(); 00813 //pc.printf("%d\n",case_joy); 00814 aktuator(); 00815 launcher(); 00816 reloader(); 00817 if ((millis()-previousMillis3 >= 320)&&(flag_Pneu)){ 00818 pneumatik = 1; 00819 flag_Pneu = false; 00820 } 00821 } 00822 else 00823 { 00824 joystick.idle(); 00825 } 00826 00827 if(millis() - previousMillis5 >= 400) 00828 { 00829 display.write(0,((int) rpm2) / 10); 00830 display.write(1,((int)rpm2) % 10); 00831 display.write(2, (int)target_rpm2 / 10); 00832 display.write(3, (int)target_rpm2 % 10); 00833 display.setColon(true); 00834 00835 previousMillis5 = millis(); 00836 } 00837 } 00838 }
Generated on Wed Aug 17 2022 08:53:13 by
1.7.2