KRAI 2017
Update 12 Maret 2017. Tambahan kombinasi tombol
Dependencies: DigitDisplay Motor PID Ping mbed millis
Fork of
MainProgram_BaseBaru_20Feb_malam
by 
KRAI 2017
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 00002 /****************************************************************************/ 00003 /* PROGRAM UNTUK PID CLOSED LOOP */ 00004 /* */ 00005 /* Last Update : 11 Maret 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 => Power Screw Aktif */ 00029 /* Tombol segitiga => Aktif motor Launcher */ 00030 /* Tombol lingkaran => Aktif Pneumatik Launcher */ 00031 /* Tombol L1 => Pivot Kiri */ 00032 /* Tombol R1 => Pivot Kanan */ 00033 /* Tombol L3 => PWM Motor Belakang Dikurangi */ 00034 /* Tombol R3 => PWM Motor Belakang Ditambah */ 00035 /* Tombol L2 => PWM Motor Depan Dikurangi */ 00036 /* Tombol R2 => PWM Motor Depan Ditambah */ 00037 /* */ 00038 /* Bismillahirahmanirrahim */ 00039 /* Jagalah Kebersihan Kodingan */ 00040 /****************************************************************************/ 00041 00042 #include "mbed.h" 00043 #include "JoystickPS3.h" 00044 #include "Motor.h" 00045 #include "encoderKRAI.h" 00046 #include "millis.h" 00047 #include "Ping.h" 00048 #include "DigitDisplay.h" 00049 00050 /***********************************************/ 00051 /* Konstanta dan Variabel */ 00052 /***********************************************/ 00053 #define PI 3.14159265 00054 #define D_ENCODER 10 // Diameter Roda Encoder 00055 #define D_ROBOT 80 // Diameter Roda Robot 00056 #define PERPINDAHAN 1 // Perpindahan ke kanan dan kiri 00057 00058 // Variable Atas 00059 double speed, speed2; 00060 const double maxSpeed = 0.95, minSpeed = 0.0; 00061 const double kpA=0.6757, kdA=0.06757, kiA=0.00006757; 00062 double p,i,d; 00063 double p2,i2,d2; 00064 double last_error = 0, current_error, sum_error = 0; 00065 double last_error2 = 0, current_error2, sum_error2 = 0; 00066 float rpm, rpm2; 00067 float target_rpm = 8.0, target_rpm2 = 10.0; 00068 const float maxRPM = 28, minRPM = 0; // Limit 25 atau 27 00069 00070 const float pwmPowerUp = 0.8; 00071 const float pwmPowerDown = -0.9; 00072 00073 float jarak_ping=0; 00074 00075 // Variable Bawah 00076 float Vt; 00077 float keliling_enc = PI*D_ENCODER; 00078 float keliling_robot = PI*D_ROBOT; 00079 float speedT = 0.2; 00080 float vpid = 0; 00081 float PIVOT = 0.2; // PWM Pivot Kanan, Pivot Kiri 00082 float tuneDpn = 0.62; // Tunning PWM motor Depan 00083 float tuneBlk = 0.62; // Tunning PWM motor belakang 00084 float tuneR = 0.72; 00085 float tuneL = 0.72; 00086 float serong = 0.65; 00087 float rasio = 1.4545; 00088 00089 /* variable tunning */ 00090 float tunning_L; 00091 float tunning_R; 00092 float tunning_Dpn; 00093 float tunning_Blk; 00094 00095 /* Variabel Encoder Bawah */ 00096 float errTetha, Tetha; // Variabel yang didapatkan encoder 00097 00098 /* Deklarasi Variable Millis */ 00099 unsigned long int previousMillis = 0; // PID launcher 00100 unsigned long int currentMillis; 00101 unsigned long int previousMillis2 = 0; // PID launcher 00102 unsigned long int currentMillis2; 00103 unsigned long int previousMillis3 = 0; // Pneumatik 00104 unsigned long int previousMillis4 = 0; // Ping 00105 unsigned long int previousMillis5 = 0; // Display 00106 00107 /* Variabel Stick */ 00108 //Logic untuk masuk aktuator 00109 int case_joy; 00110 bool isLauncher = false; 00111 bool isReload = false; 00112 bool ReloadOn = false; 00113 bool flag_Pneu = false; 00114 00115 /*****************************************************/ 00116 /* Definisi Prosedur, Fungsi dan Setting Pinout */ 00117 /*****************************************************/ 00118 00119 /* Fungsi dan Procedur Encoder */ 00120 void init_speed(); // 00121 void aktuator(); // Pergerakan aktuator berdasarkan case joystick 00122 int case_joystick(); // Mendapatkan case dari joystick 00123 //void speedKalibrasiMotor(); // Pertambahan target RPM motor atas melalui joystick 00124 void setCenter(); // Prosedur reset encoder, posisi saat itu diset jadi titik (0,0) 00125 float getTetha(); // Fungsi mendapatkan error Tetha 00126 00127 /* Inisialisasi Pin TX-RX Joystik dan PC */ 00128 joysticknucleo joystick(PA_0,PA_1); 00129 Serial pc(USBTX,USBRX); 00130 00131 /* Deklarasi Encoder Base */ 00132 encoderKRAI encoderBase(PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), 2xresolusi, mode pembacaan 00133 00134 /* Deklarasi Encoder Launcher */ 00135 encoderKRAI encLauncherBlk( PC_10, PC_11, 28, encoderKRAI::X4_ENCODING); 00136 encoderKRAI encLauncherDpn( PD_2, PC_12, 28, encoderKRAI::X4_ENCODING); 00137 00138 /* Deklarasi Motor Base */ 00139 Motor motorDpn(PB_9, PA_12, PC_5); // pwm, fwd, rev 00140 Motor motorBlk(PB_6, PB_1, PB_12); 00141 Motor motorL (PA_11, PA_6, PA_7); 00142 Motor motorR (PB_7, PA_14, PA_15); 00143 00144 /* Deklarasi Motor Launcher */ 00145 Motor launcherDpn(PA_8,PC_2,PC_1); // pwm ,fwd, rev 00146 Motor launcherBlk(PA_10, PC_3, PC_0); // pwm, fwd, rev 00147 Motor powerScrew(PA_9, PA_4, PC_15); // pwm, fwd, rev 00148 00149 /* Deklarasi Penumatik Launcher */ 00150 DigitalOut pneumatik(PB_3, PullUp); 00151 00152 /*Dekalrasi Limit Switch */ 00153 //DigitalIn infraAtas(PC_9, PullUp); 00154 DigitalIn limitTengah(PB_10, PullUp); 00155 DigitalIn limitBawah(PC_8, PullUp); 00156 00157 /*deklarasi PING ultrasonic*/ 00158 Ping pingAtas(PC_9); 00159 00160 /*Deklarasi Display*/ 00161 DigitDisplay display (D15, D4); 00162 00163 /****************************************************/ 00164 /* Deklarasi Fungsi dan Procedure */ 00165 /****************************************************/ 00166 int case_joystick() 00167 { 00168 //---------------------------------------------------// 00169 // Gerak Motor Base // 00170 // Case 1 : Pivot kanan // 00171 // Case 2 : Pivot Kiri // 00172 // Case 3 : Kanan // 00173 // Case 4 : Kiri // 00174 // Case 5 : Break // 00175 //---------------------------------------------------// 00176 00177 int caseJoystick; 00178 if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { 00179 // Pivot Kanan 00180 caseJoystick = 1; 00181 } 00182 else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { 00183 // Pivot Kiri 00184 caseJoystick = 2; 00185 } 00186 else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) { 00187 // Kanan + Rotasi kanan 00188 caseJoystick = 17; 00189 } 00190 else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) { 00191 // Kanan + Rotasi kiri 00192 caseJoystick = 18; 00193 } 00194 else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { 00195 // Kiri + Rotasi kanan 00196 caseJoystick = 19; 00197 } 00198 else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { 00199 // Kanan + Rotasi kiri 00200 caseJoystick = 20; 00201 } 00202 else if ((joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { 00203 // Maju + Rotasi kanan 00204 caseJoystick = 21; 00205 } 00206 else if ((!joystick.R1)&&(joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { 00207 // Maju + Rotasi kiri 00208 caseJoystick = 22; 00209 } 00210 else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { 00211 // Mundur + Rotasi kanan 00212 caseJoystick = 23; 00213 } 00214 else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { 00215 // Mundur + Rotasi kiri 00216 caseJoystick = 24; 00217 } 00218 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.segitiga_click)) { 00219 // Kanan + segitiga 00220 caseJoystick = 25; 00221 } 00222 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.segitiga_click)) { 00223 // Kiri + segitiga 00224 caseJoystick = 26; 00225 } 00226 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.lingkaran_click)) { 00227 // Kanan + lingkaran 00228 caseJoystick = 27; 00229 } 00230 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.lingkaran_click)) { 00231 // Kiri + lingkaran 00232 caseJoystick = 28; 00233 } 00234 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.kotak_click)) { 00235 // Kanan + kotak 00236 caseJoystick = 29; 00237 } 00238 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.kotak_click)) { 00239 // Kiri + kotak 00240 caseJoystick = 30; 00241 } 00242 else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) { 00243 // Serong kanan maju 00244 caseJoystick = 13; 00245 //pc.printf("bawah"); 00246 } 00247 else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { 00248 // Serong kiri maju 00249 caseJoystick = 14; 00250 //pc.printf("bawah"); 00251 } 00252 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) { 00253 // Serong kanan mundur 00254 caseJoystick = 15; 00255 //pc.printf("bawah"); 00256 } 00257 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { 00258 // Serong kiri mundur 00259 caseJoystick = 16; 00260 //pc.printf("bawah"); 00261 } 00262 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) { 00263 // Kanan 00264 caseJoystick = 3; 00265 //pc.printf("kanan"); 00266 } 00267 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { 00268 // Kiri 00269 caseJoystick = 4; 00270 //pc.printf("kiri"); 00271 } 00272 else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { 00273 // Atas -- Maju 00274 caseJoystick = 8; 00275 //pc.printf("atas"); 00276 } 00277 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { 00278 // Bawah -- Mundur 00279 caseJoystick = 9; 00280 //pc.printf("bawah"); 00281 } 00282 else if (joystick.segitiga_click){ 00283 // Motor Launcher 00284 caseJoystick = 5; 00285 } 00286 else if (joystick.R2_click){ 00287 // Target Pulse PID ++ Motor Depan 00288 caseJoystick = 6; 00289 } 00290 else if (joystick.L2_click){ 00291 // Target Pulse PID -- Motor 00292 caseJoystick = 7; 00293 } 00294 else if (joystick.silang_click){ 00295 // Pnemuatik ON 00296 caseJoystick = 10; 00297 } 00298 else if ((joystick.lingkaran_click)&&(!joystick.kotak_click)) { 00299 // Power Screw Up 00300 caseJoystick = 11; 00301 } 00302 else if ((joystick.kotak_click)&&(!joystick.lingkaran_click)) { 00303 // Power Screw Down 00304 caseJoystick = 12; 00305 } 00306 00307 return(caseJoystick); 00308 } 00309 00310 float getTetha(){ 00311 // Fungsi untuk mendapatkan nilai tetha 00312 float busur, tetha; 00313 busur = ((encoderBase.getPulses())/(float)(2000.0)*keliling_enc); 00314 tetha = busur/keliling_robot*360; 00315 00316 return -(tetha); 00317 } 00318 00319 float pidBase(float Kp, float Ki, float Kd) 00320 { 00321 int errorP; 00322 errorP = getTetha(); 00323 if (errorP<3.5 && errorP>(-3.5)) 00324 errorP = 0; 00325 return (float)Kp*errorP; 00326 } 00327 00328 void setCenter(){ 00329 // Fungsi untuk menentukan center dari robot 00330 encoderBase.reset(); 00331 } 00332 00333 void init_rpm (){ 00334 if (target_rpm>maxRPM-2){ 00335 target_rpm = maxRPM-2; 00336 } 00337 if (target_rpm<minRPM){ 00338 target_rpm = minRPM; 00339 } 00340 if (target_rpm2>maxRPM){ 00341 target_rpm2 = maxRPM; 00342 } 00343 if (target_rpm2<minRPM+2){ 00344 target_rpm2 = minRPM+2; 00345 } 00346 } 00347 00348 void aktuator() 00349 { 00350 switch (case_joy) { 00351 case (1): 00352 { 00353 // Pivot Kanan 00354 motorDpn.speed(-PIVOT); 00355 motorBlk.speed(-PIVOT); 00356 motorR.speed(-rasio*PIVOT); 00357 motorL.speed(-rasio*PIVOT); 00358 break; 00359 } 00360 case (2): 00361 { 00362 // Pivot Kiri 00363 motorDpn.speed(PIVOT); 00364 motorBlk.speed(PIVOT); 00365 motorR.speed(rasio*PIVOT); 00366 motorL.speed(rasio*PIVOT); 00367 break; 00368 } 00369 case (17): 00370 { 00371 // Kanan + Rotasi Kanan 00372 motorDpn.speed(-PIVOT); 00373 motorBlk.speed(-PIVOT); 00374 motorR.speed(-rasio*PIVOT); 00375 motorL.speed(-rasio*PIVOT); 00376 break; 00377 } 00378 case (18): 00379 { 00380 // Kanan + Rotasi Kiri 00381 motorDpn.speed(PIVOT); 00382 motorBlk.speed(PIVOT); 00383 motorR.speed(rasio*PIVOT); 00384 motorL.speed(rasio*PIVOT); 00385 break; 00386 } 00387 case (19): 00388 { 00389 // Kiri + Rotasi Kanan 00390 motorDpn.speed(-PIVOT); 00391 motorBlk.speed(-PIVOT); 00392 motorR.speed(-rasio*PIVOT); 00393 motorL.speed(-rasio*PIVOT); 00394 break; 00395 } 00396 case (20): 00397 { 00398 // Kiri + Rotasi Kiri 00399 motorDpn.speed(PIVOT); 00400 motorBlk.speed(PIVOT); 00401 motorR.speed(rasio*PIVOT); 00402 motorL.speed(rasio*PIVOT); 00403 break; 00404 } 00405 case (21): 00406 { 00407 // Maju + Rotasi Kanan 00408 motorDpn.speed(-PIVOT); 00409 motorBlk.speed(-PIVOT); 00410 motorR.speed(-rasio*PIVOT); 00411 motorL.speed(-rasio*PIVOT); 00412 break; 00413 } 00414 case (22): 00415 { 00416 // Maju + Rotasi Kiri 00417 motorDpn.speed(PIVOT); 00418 motorBlk.speed(PIVOT); 00419 motorR.speed(rasio*PIVOT); 00420 motorL.speed(rasio*PIVOT); 00421 break; 00422 } 00423 case (23): 00424 { 00425 // Mundur + Rotasi Kanan 00426 motorDpn.speed(-PIVOT); 00427 motorBlk.speed(-PIVOT); 00428 motorR.speed(-rasio*PIVOT); 00429 motorL.speed(-rasio*PIVOT); 00430 break; 00431 } 00432 case (24): 00433 { 00434 // Mundur + Rotasi Kiri 00435 motorDpn.speed(PIVOT); 00436 motorBlk.speed(PIVOT); 00437 motorR.speed(rasio*PIVOT); 00438 motorL.speed(rasio*PIVOT); 00439 break; 00440 } 00441 case (25): 00442 { 00443 // Kanan + segitiga 00444 isLauncher = !isLauncher; 00445 break; 00446 } 00447 case (26): 00448 { 00449 // Kiri + segitiga 00450 isLauncher = !isLauncher; 00451 break; 00452 } 00453 case (27): 00454 { 00455 // Kanan + lingkaran 00456 ReloadOn = !ReloadOn; 00457 isReload = false; 00458 break; 00459 } 00460 case (28): 00461 { 00462 // Kiri + lingkaran 00463 ReloadOn = !ReloadOn; 00464 isReload = false; 00465 break; 00466 } 00467 case (29): 00468 { 00469 // Kanan + kotak 00470 ReloadOn = !ReloadOn; 00471 isReload = true; 00472 break; 00473 } 00474 case (30): 00475 { 00476 // Kiri + kotak 00477 ReloadOn = !ReloadOn; 00478 isReload = true; 00479 break; 00480 } 00481 case (13) : 00482 { 00483 // Serong kanan maju 00484 motorDpn.speed(-serong); 00485 motorL.brake(-rasio*serong); 00486 motorBlk.speed(serong); 00487 motorR.brake(rasio*serong); 00488 break; 00489 } 00490 case (14) : 00491 { 00492 // Serong kiri maju 00493 motorDpn.speed(serong); 00494 motorR.brake(rasio*serong); 00495 motorBlk.speed(-serong); 00496 motorL.brake(-rasio*serong); 00497 break; 00498 } 00499 case (15) : 00500 { 00501 // Serong kanan mundur 00502 motorR.brake(-rasio*serong); 00503 motorBlk.speed(serong); 00504 motorL.brake(rasio*serong); 00505 break; 00506 } 00507 case (16) : 00508 { 00509 // Serong kiri mundur 00510 motorDpn.speed(serong); 00511 motorL.brake(rasio*serong); 00512 motorBlk.speed(-serong); 00513 motorR.brake(-rasio*serong); 00514 break; 00515 } 00516 case (3) : 00517 { 00518 // Kanan 00519 motorDpn.speed(-tuneDpn); 00520 motorBlk.speed(tuneBlk); 00521 motorR.brake(1); 00522 motorL.brake(1); 00523 break; 00524 } 00525 case (4) : 00526 { 00527 // Kiri 00528 motorDpn.speed(tuneDpn); 00529 motorBlk.speed(-tuneBlk); 00530 motorR.brake(1); 00531 motorL.brake(1); 00532 break; 00533 } 00534 case (8) : 00535 { 00536 // Maju 00537 //init_rpm(); 00538 motorR.speed(tuneR); 00539 motorL.speed(-tuneL); 00540 motorDpn.brake(1); 00541 motorBlk.brake(1); 00542 break; 00543 } 00544 case (9) : 00545 { 00546 // Mundur 00547 //init_rpm(); 00548 motorR.speed(-tuneR); 00549 motorL.speed(tuneL); 00550 motorDpn.brake(1); 00551 motorBlk.brake(1); 00552 break; 00553 } 00554 case (5) : 00555 { 00556 // Aktifkan motor atas 00557 isLauncher = !isLauncher; 00558 break; 00559 } 00560 case (6) : 00561 { 00562 // Target Pulse PID ++ Motor Depan 00563 target_rpm2 = target_rpm2+1.0; 00564 target_rpm = target_rpm+1.0; 00565 init_rpm(); 00566 break; 00567 } 00568 case (7) : 00569 { 00570 // Target Pulse PID -- Motor Depan 00571 target_rpm2 = target_rpm2-1.0; 00572 target_rpm = target_rpm-1.0; 00573 init_rpm(); 00574 break; 00575 } 00576 case (10) : 00577 { 00578 // Pneumatik 00579 pneumatik = 0; 00580 previousMillis3 = millis(); 00581 flag_Pneu = true; 00582 break; 00583 } 00584 case (11) : 00585 { 00586 // Power Screw Up 00587 ReloadOn = !ReloadOn; 00588 isReload = false; 00589 break; 00590 } 00591 case (12) : 00592 { 00593 // Power Screw Down 00594 ReloadOn = !ReloadOn; 00595 isReload = true; 00596 break; 00597 } 00598 default : 00599 { 00600 motorDpn.brake(1); 00601 motorBlk.brake(1); 00602 motorR.brake(1); 00603 motorL.brake(1); 00604 } 00605 } // End Switch 00606 } 00607 00608 void reloader() 00609 { 00610 if(ReloadOn){ 00611 if(isReload){ 00612 powerScrew.speed(pwmPowerDown); 00613 if(!limitBawah){ 00614 isReload = false; 00615 ReloadOn = false; 00616 } 00617 } 00618 else if(!limitTengah){ 00619 isReload = true; 00620 } 00621 else if((jarak_ping > 6.5) && !flag_Pneu){ 00622 powerScrew.speed(pwmPowerUp); 00623 } 00624 else if((jarak_ping < 6 ) && !flag_Pneu) { 00625 powerScrew.speed(-0.1); 00626 } 00627 else{ 00628 powerScrew.brake(1); 00629 } 00630 } 00631 else{ 00632 powerScrew.brake(1); 00633 } 00634 } 00635 00636 00637 void launcher() 00638 { 00639 if (isLauncher) 00640 { 00641 currentMillis = millis(); 00642 currentMillis2 = millis(); 00643 00644 // PID Launcher Depan 00645 if (currentMillis-previousMillis>=12.5) 00646 { 00647 rpm = (float)encLauncherBlk.getPulses(); 00648 current_error = target_rpm - rpm; 00649 sum_error = sum_error + current_error; 00650 p = current_error*kpA; 00651 d = (current_error-last_error)*kdA/12.5; 00652 i = sum_error*kiA*12.5; 00653 speed = p + d + i; 00654 //init_speed(); 00655 if(speed > maxSpeed){ 00656 launcherBlk.speed(maxSpeed); 00657 } 00658 else if ( speed < minSpeed){ 00659 launcherBlk.speed(minSpeed); 00660 } 00661 else { 00662 launcherBlk.speed(speed); 00663 } 00664 last_error = current_error; 00665 encLauncherBlk.reset(); 00666 //pc.printf("%.04lf\n",rpm); 00667 previousMillis = currentMillis; 00668 } 00669 if (currentMillis2-previousMillis2>=12.5) 00670 { 00671 rpm2 = (float)encLauncherDpn.getPulses(); 00672 current_error2 = target_rpm2 - rpm2; 00673 sum_error2 = sum_error2 + current_error2; 00674 p2 = current_error2*kpA; 00675 d2 = (current_error2-last_error2)*kdA/12.5; 00676 i2 = sum_error2*kiA*12.5; 00677 speed2 = p2 + d2 + i2; 00678 //init_speed(); 00679 if (speed2 > maxSpeed){ 00680 launcherDpn.speed(maxSpeed); 00681 } 00682 else if ( speed < minSpeed){ 00683 launcherDpn.speed(minSpeed); 00684 } 00685 else{ 00686 launcherDpn.speed(speed2); 00687 } 00688 last_error2 = current_error2; 00689 encLauncherDpn.reset(); 00690 previousMillis2 = currentMillis2; 00691 } 00692 } 00693 else 00694 { 00695 launcherDpn.brake(1); 00696 launcherBlk.brake(1); 00697 } 00698 } 00699 00700 /*********************************************************/ 00701 /* Main Function */ 00702 /*********************************************************/ 00703 00704 int main (void) 00705 { 00706 // Set baud rate - 115200 00707 joystick.setup(); 00708 pc.baud(115200); 00709 wait_ms(1000); 00710 00711 // initializing encoder 00712 pneumatik =1; 00713 00714 setCenter(); 00715 00716 wait_ms(500); 00717 00718 //initializing PING 00719 pingAtas.Send(); 00720 00721 pc.printf("ready...."); 00722 startMillis(); 00723 while(1) 00724 { 00725 // interupsi pembacaan PING setiap 30 ms 00726 if(millis() - previousMillis4 >= 5){ //30 00727 jarak_ping = (float)pingAtas.Read_cm()/2; 00728 00729 pingAtas.Send(); 00730 previousMillis4 = millis(); 00731 } 00732 00733 // Interrupt Serial 00734 joystick.idle(); 00735 if(joystick.readable()) 00736 { 00737 // Panggil fungsi pembacaan joystik 00738 joystick.baca_data(); 00739 // Panggil fungsi pengolahan data joystik 00740 joystick.olah_data(); 00741 // Masuk ke case joystick 00742 case_joy = case_joystick(); 00743 pc.printf("%d\n",case_joy); 00744 aktuator(); 00745 launcher(); 00746 reloader(); 00747 if ((millis()-previousMillis3 >= 320)&&(flag_Pneu)){ 00748 pneumatik = 1; 00749 flag_Pneu = false; 00750 } 00751 } 00752 else 00753 { 00754 joystick.idle(); 00755 } 00756 00757 if(millis() - previousMillis5 >= 400){ 00758 //display.write(0,((int) target_rpm2-2) / 10); 00759 //display.write(1,((int)target_rpm2-2) % 10); 00760 //display.write(2, (int)target_rpm2 / 10); 00761 //display.write(3, (int)target_rpm2 % 10); 00762 //display.setColon(true); 00763 00764 display.write(0,((int) rpm2) / 10); 00765 display.write(1,((int)rpm2) % 10); 00766 display.write(2, (int)target_rpm2 / 10); 00767 display.write(3, (int)target_rpm2 % 10); 00768 display.setColon(true); 00769 00770 previousMillis5 = millis(); 00771 } 00772 } 00773 }
Generated on Fri Jul 22 2022 22:58:39 by
1.7.2