KRAI 2017
Pakai akselerasi
Dependencies: DigitDisplay Motor PID Ping mbed millis
Fork of
MainProgram_BaseBaru_fix_omni_5April_fix
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 => 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 double speed, speed2; 00058 const double maxSpeed = 0.95, minSpeed = 0.0; 00059 const double kpA=0.6757, kdA=0.7757, kiA=0.00003757; 00060 double p,i,d; 00061 double p2,i2,d2; 00062 double last_error = 0, current_error, sum_error = 0; 00063 double last_error2 = 0, current_error2, sum_error2 = 0; 00064 float rpm, rpm2; 00065 float target_rpm = 17.0, target_rpm2 = 17.0; // selisih 4 bagus, sama bagus 00066 const float maxRPM = 28, minRPM = 0; // Limit 25 atau 27 00067 00068 const float pwmPowerUp = 1.0; 00069 const float pwmPowerDown = -1.0; 00070 00071 float jarak_ping=0; 00072 00073 // Variable Bawah 00074 float Vt; 00075 float keliling_enc = PI*D_ENCODER; 00076 float keliling_robot = PI*D_ROBOT; 00077 float speedT = 0.2; 00078 float PIVOT = 0.17; // PWM Pivot Kanan, Pivot Kiri 00079 float tuneDpn = 0.80; // Tunning PWM motor Depan 00080 float tuneBlk = 0.80; // Tunning PWM motor belakang 00081 float tuneAksel = 0.6; 00082 int aksel = 0; 00083 float tuneAkselBlk = 0.4; 00084 float tuneR = 0.78; 00085 float tuneL = 0.72; 00086 float serong = 0.4; 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 bool ready = false; 00115 00116 /*****************************************************/ 00117 /* Definisi Prosedur, Fungsi dan Setting Pinout */ 00118 /*****************************************************/ 00119 00120 /* Fungsi dan Procedur Encoder */ 00121 void init_speed(); // 00122 void aktuator(); // Pergerakan aktuator berdasarkan case joystick 00123 int case_joystick(); // Mendapatkan case dari joystick 00124 //void speedKalibrasiMotor(); // Pertambahan target RPM motor atas melalui joystick 00125 void setCenter(); // Prosedur reset encoder, posisi saat itu diset jadi titik (0,0) 00126 float getTetha(); // Fungsi mendapatkan error Tetha 00127 00128 /* Inisialisasi Pin TX-RX Joystik dan PC */ 00129 joysticknucleo joystick(PA_0,PA_1); 00130 Serial pc(USBTX,USBRX); 00131 00132 /* Deklarasi Encoder Base */ 00133 encoderKRAI encoderBase(PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), 2xresolusi, mode pembacaan 00134 00135 /* Deklarasi Encoder Launcher */ 00136 encoderKRAI encLauncherDpn( PC_10, PC_11, 28, encoderKRAI::X4_ENCODING); 00137 encoderKRAI encLauncherBlk( PC_12, PD_2, 28, encoderKRAI::X4_ENCODING); 00138 00139 /* Deklarasi Motor Base */ 00140 Motor motorDpn(PB_7, PC_3, PC_0); //(PB_9, PA_12, PC_5); 00141 //Motor motorBlk(PB_6, PC_14, PC_13); //(PB_6, PB_1, PB_12); (PC_7, PC_14, PC_13); 00142 Motor motorBlk(PB_2, PB_15, PB_1); 00143 Motor motorL (PB_9, PA_12, PA_6); 00144 Motor motorR (PB_8, PC_5, PC_6); //(PC_6, PB_4, PB_5); 00145 00146 /* Deklarasi Motor Launcher */ 00147 Motor launcherDpn(PA_5,PB_12,PA_11); // pwm ,fwd, rev 00148 Motor launcherBlk(PB_3, PC_4, PA_10); // pwm, fwd, rev 00149 Motor powerScrew(PB_10, PB_14, PB_13); // pwm, fwd, rev 00150 00151 /* Deklarasi Penumatik Launcher */ 00152 DigitalOut pneumatik(PA_4, PullUp); 00153 00154 /*Dekalrasi Limit Switch */ 00155 //DigitalIn infraAtas(PC_9, PullUp); 00156 DigitalIn limitTengah(PA_9, PullUp); 00157 DigitalIn limitBawah(PC_7, PullUp); 00158 DigitalIn limitBawah1(PA_7, PullUp); 00159 00160 /*deklarasi PING ultrasonic*/ 00161 Ping pingAtas(PC_15); 00162 00163 /*Deklarasi Display*/ 00164 DigitDisplay display (PA_8, PC_8); 00165 00166 /****************************************************/ 00167 /* Deklarasi Fungsi dan Procedure */ 00168 /****************************************************/ 00169 int case_joystick() 00170 { 00171 //---------------------------------------------------// 00172 // Gerak Motor Base // 00173 // Case 1 : Pivot kanan // 00174 // Case 2 : Pivot Kiri // 00175 // Case 3 : Kanan // 00176 // Case 4 : Kiri // 00177 // Case 5 : Break // 00178 //---------------------------------------------------// 00179 00180 int caseJoystick; 00181 if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { 00182 // Pivot Kanan 00183 caseJoystick = 1; 00184 } 00185 else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { 00186 // Pivot Kiri 00187 caseJoystick = 2; 00188 } 00189 else if ((joystick.START_click)&&(!joystick.SELECT_click)&&(!joystick.R3_click)) { 00190 // tambah rpm dengan nilai tertentu 00191 caseJoystick = 31; 00192 } 00193 else if ((!joystick.START_click)&&(joystick.SELECT_click)&&(!joystick.R3_click)) { 00194 // kurangi rpm dengan nilai tertentu 00195 caseJoystick = 32; 00196 } 00197 else if ((!joystick.START_click)&&(!joystick.SELECT_click)&&(joystick.R3_click)) { 00198 // kurangi rpm dengan nilai tertentu 00199 caseJoystick = 33; 00200 } 00201 else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) { 00202 // Kanan + Rotasi kanan 00203 caseJoystick = 17; 00204 } 00205 else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) { 00206 // Kanan + Rotasi kiri 00207 caseJoystick = 18; 00208 } 00209 else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { 00210 // Kiri + Rotasi kanan 00211 caseJoystick = 19; 00212 } 00213 else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { 00214 // Kanan + Rotasi kiri 00215 caseJoystick = 20; 00216 } 00217 else if ((joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { 00218 // Maju + Rotasi kanan 00219 caseJoystick = 21; 00220 } 00221 else if ((!joystick.R1)&&(joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { 00222 // Maju + Rotasi kiri 00223 caseJoystick = 22; 00224 } 00225 else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { 00226 // Mundur + Rotasi kanan 00227 caseJoystick = 23; 00228 } 00229 else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { 00230 // Mundur + Rotasi kiri 00231 caseJoystick = 24; 00232 } 00233 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.segitiga_click)) { 00234 // Kanan + segitiga 00235 caseJoystick = 25; 00236 } 00237 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.segitiga_click)) { 00238 // Kiri + segitiga 00239 caseJoystick = 26; 00240 } 00241 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.lingkaran_click)) { 00242 // Kanan + lingkaran 00243 caseJoystick = 27; 00244 } 00245 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.lingkaran_click)) { 00246 // Kiri + lingkaran 00247 caseJoystick = 28; 00248 } 00249 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.kotak_click)) { 00250 // Kanan + kotak 00251 caseJoystick = 29; 00252 } 00253 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.kotak_click)) { 00254 // Kiri + kotak 00255 caseJoystick = 30; 00256 } 00257 else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) { 00258 // Serong kanan maju 00259 caseJoystick = 13; 00260 } 00261 else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { 00262 // Serong kiri maju 00263 caseJoystick = 14; 00264 } 00265 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) { 00266 // Serong kanan mundur 00267 caseJoystick = 15; 00268 } 00269 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { 00270 // Serong kiri mundur 00271 caseJoystick = 16; 00272 } 00273 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) { 00274 // Kanan 00275 caseJoystick = 3; 00276 } 00277 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { 00278 // Kiri 00279 caseJoystick = 4; 00280 } 00281 else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { 00282 // Atas -- Maju 00283 caseJoystick = 8; 00284 } 00285 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { 00286 // Bawah -- Mundur 00287 caseJoystick = 9; 00288 } 00289 else if (joystick.segitiga_click){ 00290 // Motor Launcher 00291 caseJoystick = 5; 00292 } 00293 else if (joystick.R2_click){ 00294 // Target Pulse PID ++ Motor Depan 00295 caseJoystick = 6; 00296 } 00297 else if (joystick.L2_click){ 00298 // Target Pulse PID -- Motor 00299 caseJoystick = 7; 00300 } 00301 else if (joystick.silang_click){ 00302 // Pnemuatik ON 00303 caseJoystick = 10; 00304 } 00305 else if ((joystick.lingkaran_click)&&(!joystick.kotak_click)) { 00306 // Power Screw Up 00307 caseJoystick = 11; 00308 } 00309 else if ((joystick.kotak_click)&&(!joystick.lingkaran_click)) { 00310 // Power Screw Down 00311 caseJoystick = 12; 00312 } 00313 else 00314 { 00315 tuneAksel = 0.6; 00316 aksel = 0; 00317 } 00318 00319 return(caseJoystick); 00320 } 00321 00322 float getTetha(){ 00323 // Fungsi untuk mendapatkan nilai tetha 00324 float busur, tetha; 00325 busur = ((encoderBase.getPulses())/(float)(2000.0)*keliling_enc); 00326 tetha = busur/keliling_robot*360; 00327 00328 return -(tetha); 00329 } 00330 00331 float pidBase(float Kp, float Ki, float Kd) 00332 { 00333 int errorP; 00334 errorP = getTetha(); 00335 if (errorP<3.5 && errorP>(-3.5)) 00336 errorP = 0; 00337 return (float)Kp*errorP; 00338 } 00339 00340 void setCenter(){ 00341 // Fungsi untuk menentukan center dari robot 00342 encoderBase.reset(); 00343 } 00344 00345 void init_rpm (){ 00346 if (target_rpm>maxRPM-2){ 00347 target_rpm = maxRPM-2; 00348 } 00349 if (target_rpm<minRPM){ 00350 target_rpm = minRPM; 00351 } 00352 if (target_rpm2>maxRPM){ 00353 target_rpm2 = maxRPM; 00354 } 00355 if (target_rpm2<minRPM+2){ 00356 target_rpm2 = minRPM+2; 00357 } 00358 } 00359 00360 void aktuator() 00361 { 00362 switch (case_joy) { 00363 case (1): 00364 { 00365 // Pivot Kanan 00366 motorDpn.speed(-PIVOT); 00367 motorBlk.speed(-PIVOT); 00368 motorR.speed(-rasio*PIVOT); 00369 motorL.speed(-rasio*PIVOT); 00370 break; 00371 } 00372 case (2): 00373 { 00374 // Pivot Kiri 00375 motorDpn.speed(PIVOT); 00376 motorBlk.speed(PIVOT); 00377 motorR.speed(rasio*PIVOT); 00378 motorL.speed(rasio*PIVOT); 00379 break; 00380 } 00381 case (17): 00382 { 00383 // Kanan + Rotasi Kanan 00384 motorDpn.speed(-PIVOT); 00385 motorBlk.speed(-PIVOT); 00386 motorR.speed(-rasio*PIVOT); 00387 motorL.speed(-rasio*PIVOT); 00388 break; 00389 } 00390 case (18): 00391 { 00392 // Kanan + Rotasi Kiri 00393 motorDpn.speed(PIVOT); 00394 motorBlk.speed(PIVOT); 00395 motorR.speed(rasio*PIVOT); 00396 motorL.speed(rasio*PIVOT); 00397 break; 00398 } 00399 case (19): 00400 { 00401 // Kiri + Rotasi Kanan 00402 motorDpn.speed(-PIVOT); 00403 motorBlk.speed(-PIVOT); 00404 motorR.speed(-rasio*PIVOT); 00405 motorL.speed(-rasio*PIVOT); 00406 break; 00407 } 00408 case (20): 00409 { 00410 // Kiri + Rotasi Kiri 00411 motorDpn.speed(PIVOT); 00412 motorBlk.speed(PIVOT); 00413 motorR.speed(rasio*PIVOT); 00414 motorL.speed(rasio*PIVOT); 00415 break; 00416 } 00417 case (21): 00418 { 00419 // Maju + Rotasi Kanan 00420 motorDpn.speed(-PIVOT); 00421 motorBlk.speed(-PIVOT); 00422 motorR.speed(-rasio*PIVOT); 00423 motorL.speed(-rasio*PIVOT); 00424 break; 00425 } 00426 case (22): 00427 { 00428 // Maju + Rotasi Kiri 00429 motorDpn.speed(PIVOT); 00430 motorBlk.speed(PIVOT); 00431 motorR.speed(rasio*PIVOT); 00432 motorL.speed(rasio*PIVOT); 00433 break; 00434 } 00435 case (23): 00436 { 00437 // Mundur + Rotasi Kanan 00438 motorDpn.speed(-PIVOT); 00439 motorBlk.speed(-PIVOT); 00440 motorR.speed(-rasio*PIVOT); 00441 motorL.speed(-rasio*PIVOT); 00442 break; 00443 } 00444 case (24): 00445 { 00446 // Mundur + Rotasi Kiri 00447 motorDpn.speed(PIVOT); 00448 motorBlk.speed(PIVOT); 00449 motorR.speed(rasio*PIVOT); 00450 motorL.speed(rasio*PIVOT); 00451 break; 00452 } 00453 case (25): 00454 { 00455 // Kanan + segitiga 00456 isLauncher = !isLauncher; 00457 break; 00458 } 00459 case (26): 00460 { 00461 // Kiri + segitiga 00462 isLauncher = !isLauncher; 00463 break; 00464 } 00465 case (27): 00466 { 00467 // Kanan + lingkaran 00468 ReloadOn = !ReloadOn; 00469 isReload = false; 00470 break; 00471 } 00472 case (28): 00473 { 00474 // Kiri + lingkaran 00475 ReloadOn = !ReloadOn; 00476 isReload = false; 00477 break; 00478 } 00479 case (29): 00480 { 00481 // Kanan + kotak 00482 ReloadOn = !ReloadOn; 00483 isReload = true; 00484 break; 00485 } 00486 case (30): 00487 { 00488 // Kiri + kotak 00489 ReloadOn = !ReloadOn; 00490 isReload = true; 00491 break; 00492 } 00493 case (13) : 00494 { 00495 // Serong kanan maju 00496 motorDpn.speed(-serong); 00497 motorL.speed(-serong); 00498 motorBlk.speed(serong); 00499 motorR.speed(serong); 00500 break; 00501 } 00502 case (14) : 00503 { 00504 // Serong kiri maju 00505 motorDpn.speed(serong); 00506 motorR.speed(serong); 00507 motorBlk.speed(-serong); 00508 motorL.speed(-serong); 00509 break; 00510 } 00511 case (15) : 00512 { 00513 // Serong kanan mundur 00514 motorDpn.speed(-serong); 00515 motorR.speed(-serong); 00516 motorBlk.speed(serong); 00517 motorL.speed(serong); 00518 break; 00519 } 00520 case (16) : 00521 { 00522 // Serong kiri mundur 00523 motorDpn.speed(serong); 00524 motorL.speed(serong); 00525 motorBlk.speed(-serong); 00526 motorR.speed(-serong); 00527 break; 00528 } 00529 case (3) : 00530 { 00531 // Kanan 00532 aksel++; 00533 if (aksel>300) 00534 tuneAksel = 0.9; 00535 00536 motorDpn.speed(-tuneAksel); 00537 motorBlk.speed(tuneAksel); 00538 motorR.brake(1); 00539 motorL.brake(1); 00540 break; 00541 } 00542 case (4) : 00543 { 00544 // Kiri 00545 aksel++; 00546 if (aksel>300) 00547 tuneAksel = 0.9; 00548 00549 motorDpn.speed(tuneAksel); 00550 motorBlk.speed(-tuneAksel); 00551 motorR.brake(1); 00552 motorL.brake(1); 00553 break; 00554 } 00555 case (8) : 00556 { 00557 // Maju 00558 aksel++; 00559 if (aksel>300) 00560 tuneAksel = 0.9; 00561 00562 motorR.speed(tuneAksel); 00563 motorL.speed(-tuneAksel); 00564 motorDpn.brake(1); 00565 motorBlk.brake(1); 00566 break; 00567 } 00568 case (9) : 00569 { 00570 // Mundur 00571 aksel++; 00572 if (aksel>300) 00573 tuneAksel = 0.9; 00574 00575 motorR.speed(-tuneAksel); 00576 motorL.speed(tuneAksel); 00577 motorDpn.brake(1); 00578 motorBlk.brake(1); 00579 break; 00580 } 00581 case (5) : 00582 { 00583 // Aktifkan motor atas 00584 isLauncher = !isLauncher; 00585 break; 00586 } 00587 case (6) : 00588 { 00589 // Target Pulse PID ++ Motor Depan 00590 target_rpm2 = target_rpm2+1.0; 00591 target_rpm = target_rpm+1.0; 00592 init_rpm(); 00593 break; 00594 } 00595 case (7) : 00596 { 00597 // Target Pulse PID -- Motor Depan 00598 target_rpm2 = target_rpm2-1.0; 00599 target_rpm = target_rpm-1.0; 00600 init_rpm(); 00601 break; 00602 } 00603 case (10) : 00604 { 00605 // Pneumatik 00606 if (ready) 00607 { 00608 pneumatik = 0; 00609 previousMillis3 = millis(); 00610 flag_Pneu = true; 00611 ready = false; 00612 00613 } 00614 break; 00615 } 00616 case (11) : 00617 { 00618 // Power Screw Up 00619 ReloadOn = !ReloadOn; 00620 isReload = false; 00621 break; 00622 } 00623 case (31) : 00624 { 00625 // start 00626 target_rpm2 = 22; 00627 target_rpm = 22; 00628 init_rpm(); 00629 break; 00630 } 00631 case (32) : 00632 { 00633 // select 00634 target_rpm2 = 10; 00635 target_rpm = 10; 00636 init_rpm(); 00637 break; 00638 } 00639 case (33) : 00640 { 00641 // R3 00642 target_rpm2 = 17; 00643 target_rpm = 17; 00644 init_rpm(); 00645 break; 00646 } 00647 case (12) : 00648 { 00649 // Power Screw Down 00650 ReloadOn = !ReloadOn; 00651 isReload = true; 00652 break; 00653 } 00654 default : 00655 { 00656 tuneAksel = 0.6; 00657 aksel = 0; 00658 motorDpn.brake(1); 00659 motorBlk.brake(1); 00660 motorR.brake(1); 00661 motorL.brake(1); 00662 } 00663 } // End Switch 00664 } 00665 00666 void reloader() 00667 { 00668 if(ReloadOn){ 00669 if(isReload){ 00670 powerScrew.speed(pwmPowerDown); 00671 if((!limitBawah)||(!limitBawah1)){ 00672 isReload = false; 00673 ReloadOn = false; 00674 } 00675 } 00676 else if(!limitTengah){ 00677 isReload = true; 00678 } 00679 else if((jarak_ping > 6.5) && !flag_Pneu){ 00680 powerScrew.speed(pwmPowerUp); 00681 ready = false; 00682 } 00683 else if((jarak_ping < 6.0) && !flag_Pneu) { 00684 powerScrew.speed(-0.85); 00685 ready = false; 00686 } 00687 else{ 00688 powerScrew.brake(1); 00689 ready = true; 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 Depan 00706 if (currentMillis-previousMillis>=12.5) 00707 { 00708 rpm = (float)encLauncherBlk.getPulses(); 00709 current_error = target_rpm - rpm; 00710 sum_error = sum_error + current_error*12.5; 00711 p = current_error*kpA; 00712 d = (current_error-last_error)*kdA/12.5; 00713 i = sum_error*kiA; 00714 speed = p + d + i; 00715 //init_speed(); 00716 if(speed > maxSpeed){ 00717 launcherBlk.speed(maxSpeed); 00718 } 00719 else if ( speed < minSpeed){ 00720 launcherBlk.speed(minSpeed); 00721 } 00722 else { 00723 launcherBlk.speed(speed); 00724 } 00725 last_error = current_error; 00726 encLauncherBlk.reset(); 00727 //pc.printf("%.04lf\n",rpm); 00728 previousMillis = currentMillis; 00729 } 00730 if (currentMillis2-previousMillis2>=12.5) 00731 { 00732 rpm2 = (float)encLauncherDpn.getPulses(); 00733 current_error2 = target_rpm2 - rpm2; 00734 sum_error2 = sum_error2 + current_error2*12.5; 00735 p2 = current_error2*kpA; 00736 d2 = (current_error2-last_error2)*kdA/12.5; 00737 i2 = sum_error2*kiA; 00738 speed2 = p2 + d2 + i2; 00739 //init_speed(); 00740 if (speed2 > maxSpeed){ 00741 launcherDpn.speed(maxSpeed); 00742 } 00743 else if ( speed < minSpeed){ 00744 launcherDpn.speed(minSpeed); 00745 } 00746 else{ 00747 launcherDpn.speed(speed2); 00748 } 00749 last_error2 = current_error2; 00750 encLauncherDpn.reset(); 00751 previousMillis2 = currentMillis2; 00752 } 00753 pc.printf("%.2f\t%.2f\n",rpm,rpm2); 00754 } 00755 else 00756 { 00757 launcherDpn.brake(1); 00758 launcherBlk.brake(1); 00759 sum_error = 0; 00760 sum_error2 = 0; 00761 current_error = 0; 00762 current_error2 = 0; 00763 last_error = 0; 00764 last_error2 = 0; 00765 } 00766 } 00767 00768 /*********************************************************/ 00769 /* Main Function */ 00770 /*********************************************************/ 00771 00772 int main (void) 00773 { 00774 // Set baud rate - 115200 00775 joystick.setup(); 00776 pc.baud(115200); 00777 wait_ms(1000); 00778 00779 // initializing encoder 00780 pneumatik =1; 00781 00782 setCenter(); 00783 00784 wait_ms(500); 00785 00786 //initializing PING 00787 pingAtas.Send(); 00788 00789 pc.printf("ready...."); 00790 startMillis(); 00791 while(1) 00792 { 00793 // interupsi pembacaan PING setiap 30 ms 00794 if(millis() - previousMillis4 >= 5){ //30 00795 jarak_ping = (float)pingAtas.Read_cm()/2; 00796 00797 pingAtas.Send(); 00798 previousMillis4 = millis(); 00799 } 00800 00801 // Interrupt Serial 00802 joystick.idle(); 00803 if(joystick.readable()) 00804 { 00805 // Panggil fungsi pembacaan joystik 00806 joystick.baca_data(); 00807 // Panggil fungsi pengolahan data joystik 00808 joystick.olah_data(); 00809 // Masuk ke case joystick 00810 case_joy = case_joystick(); 00811 //pc.printf("%d\n",case_joy); 00812 aktuator(); 00813 launcher(); 00814 reloader(); 00815 if ((millis()-previousMillis3 >= 320)&&(flag_Pneu)){ 00816 pneumatik = 1; 00817 flag_Pneu = false; 00818 } 00819 } 00820 else 00821 { 00822 joystick.idle(); 00823 } 00824 00825 if(millis() - previousMillis5 >= 400) 00826 { 00827 display.write(0,((int) rpm2) / 10); 00828 display.write(1,((int)rpm2) % 10); 00829 display.write(2, (int)target_rpm2 / 10); 00830 display.write(3, (int)target_rpm2 % 10); 00831 display.setColon(true); 00832 00833 previousMillis5 = millis(); 00834 } 00835 } 00836 }
Generated on Sat Jul 16 2022 13:37:08 by
1.7.2