KRAI 2017
this is disco time
Dependencies: DigitDisplay Motor PID Ping mbed millis
Fork of
DagonFly__RoadToJapan_17Mei
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.1293, kdA2=1.0070, kiA2=0.0002986; 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 = 17.0, target_rpm2 = 17.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 = 15; // ping lama 00078 double ping_target = 14; // ping baru 00079 double ping_current_error, ping_previous_error1 = 0, ping_sum_error=0; 00080 double ping_Kp = -0.3747, ping_Kd = -0.049, ping_Ts=10; 00081 double ping_pwm, ping_previous_pwm = 0; 00082 00083 // Variable Bawah 00084 float PIVOT = 0.17; // PWM Pivot Kanan, Pivot Kiri 00085 float tuneDpn = 1.0; // Tunning PWM motor Depan 00086 float tuneBlk = 1.0; // Tunning PWM motor belakang 00087 float tuneAksel = 0.6; 00088 int aksel = 0; 00089 float tuneAkselBlk = 0.4; 00090 float tuneR = 1.0; 00091 float tuneL = 1.0; 00092 float serong = 0.4; 00093 float rasio = 1.4545; 00094 float t_new = 0.1; 00095 00096 /* variable tunning */ 00097 float tunning_L; 00098 float tunning_R; 00099 float tunning_Dpn; 00100 float tunning_Blk; 00101 00102 /* Deklarasi Variable Millis */ 00103 static volatile uint32_t previousMillis = 0; // PID launcher 00104 static volatile uint32_t currentMillis; 00105 static volatile uint32_t previousMillis2 = 0; // PID launcher 00106 static volatile uint32_t currentMillis2; 00107 static volatile uint32_t previousMillis3 = 0; // Pneumatik 00108 static volatile uint32_t previousMillis4 = 0; // Ping 00109 static volatile uint32_t previousMillis5 = 0; // Display 00110 static volatile uint32_t previousMillis6 = 0; // pneu 00111 00112 /* Variabel Stick */ 00113 //Logic untuk masuk aktuator 00114 int case_joy; 00115 bool isLauncher = false; 00116 bool isReload = false; 00117 bool ReloadOn = false; 00118 bool flag_Pneu = false; 00119 bool flag_paku = false; 00120 00121 bool ready = false; 00122 00123 /*****************************************************/ 00124 /* Definisi Prosedur, Fungsi dan Setting Pinout */ 00125 /*****************************************************/ 00126 00127 /* Fungsi dan Procedur Encoder */ 00128 void init_speed(); // 00129 void aktuator(); // Pergerakan aktuator berdasarkan case joystick 00130 int case_joystick(); // Mendapatkan case dari joystick 00131 //void speedKalibrasiMotor(); // Pertambahan target RPM motor atas melalui joystick 00132 00133 /* Inisialisasi Pin TX-RX Joystik dan PC */ 00134 joysticknucleo joystick(PA_9,PA_10); //v 00135 Serial pc(PA_2,PA_3); //v 00136 00137 /* Deklarasi Encoder Launcher */ 00138 encoderKRAI encLauncherDpn( PA_8, PC_9, 28, encoderKRAI::X4_ENCODING); //v 00139 encoderKRAI encLauncherBlk( PC_14, PC_13, 28, encoderKRAI::X4_ENCODING); //v 00140 00141 // encoder base 00142 encoderKRAI encBaseR( PC_15, PF_0, 28, encoderKRAI::X4_ENCODING); //v 00143 encoderKRAI encBaseL( PF_1, PF_2, 28, encoderKRAI::X4_ENCODING); //v 00144 encoderKRAI encBaseDpn( PF_5, PF_6, 28, encoderKRAI::X4_ENCODING); //v 00145 encoderKRAI encBaseBlk( PF_3, PF_4, 28, encoderKRAI::X4_ENCODING); //v 00146 00147 // encoder elevator 00148 encoderKRAI encElevator( PD_7, PG_10, 28, encoderKRAI::X4_ENCODING); //v 00149 00150 /* Deklarasi Motor Base */ 00151 Motor motorDpn(PB_3, PC_2, PC_3); //v 00152 Motor motorBlk(PA_5, PC_1, PC_0); //v 00153 Motor motorL (PB_11, PE_7, PE_8);//v 00154 Motor motorR (PB_10, PG_0, PF_15); //v 00155 00156 /* Deklarasi Motor Launcher */ 00157 Motor launcherDpn(PA_7,PB_2,PC_5); //v 00158 Motor launcherBlk(PA_6, PC_4, PA_4); // v 00159 Motor powerScrew(PB_1, PF_13, PF_12); //v 00160 00161 Motor Reloader(PD_12, PG_1, PE_9); // v 00162 Motor Elevator(PB_0, PF_14, PF_11); //v 00163 00164 /* Deklarasi Penumatik Launcher */ 00165 DigitalOut pneumatik(PA_15, PullUp); //v 00166 DigitalOut pneu_paku(PC_10, PullUp); //v 00167 00168 /*Dekalrasi Limit Switch */ 00169 DigitalIn limitTengah(PD_5, PullUp); //v 00170 DigitalIn limitBawah(PD_4, PullUp); //v 00171 DigitalIn limitBawah1(PD_3, PullUp); //v 00172 DigitalIn limitAtas1(PD_2, PullUp); //v tambahan 00173 00174 00175 /*deklarasi PING ultrasonic*/ 00176 Ping pingAtas(PC_11); //v 00177 00178 /*Deklarasi Display*/ 00179 DigitDisplay display (PB_6, PB_5); //v 00180 00181 /****************************************************/ 00182 /* Deklarasi Fungsi dan Procedure */ 00183 /****************************************************/ 00184 int case_joystick() 00185 { 00186 //---------------------------------------------------// 00187 // Gerak Motor Base // 00188 // Case 1 : Pivot kanan // 00189 // Case 2 : Pivot Kiri // 00190 // Case 3 : Kanan // 00191 // Case 4 : Kiri // 00192 // Case 5 : Break // 00193 //---------------------------------------------------// 00194 00195 int caseJoystick; 00196 if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { 00197 // Pivot Kanan 00198 caseJoystick = 1; 00199 } 00200 else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { 00201 // Pivot Kiri 00202 caseJoystick = 2; 00203 } 00204 else if ((joystick.START_click)&&(!joystick.SELECT_click)&&(!joystick.R3_click)) { 00205 // tambah rpm dengan nilai tertentu 00206 caseJoystick = 31; 00207 } 00208 else if ((!joystick.START_click)&&(joystick.SELECT_click)&&(!joystick.R3_click)) { 00209 // kurangi rpm dengan nilai tertentu 00210 caseJoystick = 32; 00211 } 00212 else if ((!joystick.START_click)&&(!joystick.SELECT_click)&&(joystick.R3_click)) { 00213 // kurangi rpm dengan nilai tertentu 00214 caseJoystick = 33; 00215 } 00216 else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) { 00217 // Kanan + Rotasi kanan 00218 caseJoystick = 17; 00219 } 00220 else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) { 00221 // Kanan + Rotasi kiri 00222 caseJoystick = 18; 00223 } 00224 else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { 00225 // Kiri + Rotasi kanan 00226 caseJoystick = 19; 00227 } 00228 else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { 00229 // Kanan + Rotasi kiri 00230 caseJoystick = 20; 00231 } 00232 else if ((joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { 00233 // Maju + Rotasi kanan 00234 caseJoystick = 21; 00235 } 00236 else if ((!joystick.R1)&&(joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { 00237 // Maju + Rotasi kiri 00238 caseJoystick = 22; 00239 } 00240 else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { 00241 // Mundur + Rotasi kanan 00242 caseJoystick = 23; 00243 } 00244 else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { 00245 // Mundur + Rotasi kiri 00246 caseJoystick = 24; 00247 } 00248 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.segitiga_click)) { 00249 // Kanan + segitiga 00250 caseJoystick = 25; 00251 } 00252 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.segitiga_click)) { 00253 // Kiri + segitiga 00254 caseJoystick = 26; 00255 } 00256 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.lingkaran_click)) { 00257 // Kanan + lingkaran 00258 caseJoystick = 27; 00259 } 00260 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.lingkaran_click)) { 00261 // Kiri + lingkaran 00262 caseJoystick = 28; 00263 } 00264 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.kotak_click)) { 00265 // Kanan + kotak 00266 caseJoystick = 29; 00267 } 00268 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.kotak_click)) { 00269 // Kiri + kotak 00270 caseJoystick = 30; 00271 } 00272 else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) { 00273 // Serong kanan maju 00274 caseJoystick = 13; 00275 } 00276 else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { 00277 // Serong kiri maju 00278 caseJoystick = 14; 00279 } 00280 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) { 00281 // Serong kanan mundur 00282 caseJoystick = 15; 00283 } 00284 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { 00285 // Serong kiri mundur 00286 caseJoystick = 16; 00287 } 00288 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) { 00289 // Kanan 00290 caseJoystick = 3; 00291 } 00292 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { 00293 // Kiri 00294 caseJoystick = 4; 00295 } 00296 else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { 00297 // Atas -- Maju 00298 caseJoystick = 8; 00299 } 00300 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { 00301 // Bawah -- Mundur 00302 caseJoystick = 9; 00303 } 00304 else if (joystick.segitiga_click){ 00305 // Motor Launcher 00306 caseJoystick = 5; 00307 } 00308 else if (joystick.R2_click){ 00309 // Target Pulse PID ++ Motor Depan 00310 caseJoystick = 6; 00311 } 00312 else if (joystick.L2_click){ 00313 // Target Pulse PID -- Motor 00314 caseJoystick = 7; 00315 } 00316 else if (joystick.silang_click){ 00317 // Pnemuatik ON 00318 caseJoystick = 10; 00319 } 00320 else if ((joystick.lingkaran_click)&&(!joystick.kotak_click)) { 00321 // Power Screw Up 00322 caseJoystick = 11; 00323 } 00324 else if ((joystick.kotak_click)&&(!joystick.lingkaran_click)) { 00325 // Power Screw Down 00326 caseJoystick = 12; 00327 } 00328 else if (joystick.L3){ 00329 // Paku Bumi ON/OFF 00330 caseJoystick = 34; 00331 } 00332 else 00333 { 00334 tuneAksel = 0.6; 00335 aksel = 0; 00336 } 00337 00338 return(caseJoystick); 00339 } 00340 00341 00342 void init_rpm (){ 00343 if (target_rpm>maxRPM-2){ 00344 target_rpm = maxRPM-2; 00345 } 00346 if (target_rpm<minRPM){ 00347 target_rpm = minRPM; 00348 } 00349 if (target_rpm2>maxRPM){ 00350 target_rpm2 = maxRPM; 00351 } 00352 if (target_rpm2<minRPM+2){ 00353 target_rpm2 = minRPM+2; 00354 } 00355 } 00356 00357 void aktuator() 00358 { 00359 switch (case_joy) { 00360 case (1): 00361 { 00362 // Pivot Kanan 00363 motorDpn.speed(-PIVOT); 00364 motorBlk.speed(-PIVOT); 00365 motorR.speed(-rasio*PIVOT-t_new); 00366 motorL.speed(-rasio*PIVOT-t_new); 00367 break; 00368 } 00369 case (2): 00370 { 00371 // Pivot Kiri 00372 motorDpn.speed(PIVOT); 00373 motorBlk.speed(PIVOT); 00374 motorR.speed(rasio*PIVOT+t_new); 00375 motorL.speed(rasio*PIVOT+t_new); 00376 break; 00377 } 00378 case (17): 00379 { 00380 // Kanan + Rotasi Kanan 00381 motorDpn.speed(-PIVOT); 00382 motorBlk.speed(-PIVOT); 00383 motorR.speed(-rasio*PIVOT-t_new); 00384 motorL.speed(-rasio*PIVOT-t_new); 00385 break; 00386 } 00387 case (18): 00388 { 00389 // Kanan + Rotasi Kiri 00390 motorDpn.speed(PIVOT); 00391 motorBlk.speed(PIVOT); 00392 motorR.speed(rasio*PIVOT+t_new); 00393 motorL.speed(rasio*PIVOT+t_new); 00394 break; 00395 } 00396 case (19): 00397 { 00398 // Kiri + Rotasi Kanan 00399 motorDpn.speed(-PIVOT); 00400 motorBlk.speed(-PIVOT); 00401 motorR.speed(-rasio*PIVOT-t_new); 00402 motorL.speed(-rasio*PIVOT-t_new); 00403 break; 00404 } 00405 case (20): 00406 { 00407 // Kiri + Rotasi Kiri 00408 motorDpn.speed(PIVOT); 00409 motorBlk.speed(PIVOT); 00410 motorR.speed(rasio*PIVOT+t_new); 00411 motorL.speed(rasio*PIVOT+t_new); 00412 break; 00413 } 00414 case (21): 00415 { 00416 // Maju + Rotasi Kanan 00417 motorDpn.speed(-PIVOT); 00418 motorBlk.speed(-PIVOT); 00419 motorR.speed(-rasio*PIVOT-t_new); 00420 motorL.speed(-rasio*PIVOT-t_new); 00421 break; 00422 } 00423 case (22): 00424 { 00425 // Maju + Rotasi Kiri 00426 motorDpn.speed(PIVOT); 00427 motorBlk.speed(PIVOT); 00428 motorR.speed(rasio*PIVOT+t_new); 00429 motorL.speed(rasio*PIVOT+t_new); 00430 break; 00431 } 00432 case (23): 00433 { 00434 // Mundur + Rotasi Kanan 00435 motorDpn.speed(-PIVOT); 00436 motorBlk.speed(-PIVOT); 00437 motorR.speed(-rasio*PIVOT-t_new); 00438 motorL.speed(-rasio*PIVOT-t_new); 00439 break; 00440 } 00441 case (24): 00442 { 00443 // Mundur + Rotasi Kiri 00444 motorDpn.speed(PIVOT); 00445 motorBlk.speed(PIVOT); 00446 motorR.speed(rasio*PIVOT+t_new); 00447 motorL.speed(rasio*PIVOT+t_new); 00448 break; 00449 } 00450 case (25): 00451 { 00452 // Kanan + segitiga 00453 isLauncher = !isLauncher; 00454 break; 00455 } 00456 case (26): 00457 { 00458 // Kiri + segitiga 00459 isLauncher = !isLauncher; 00460 break; 00461 } 00462 case (27): 00463 { 00464 // Kanan + lingkaran 00465 ReloadOn = !ReloadOn; 00466 isReload = false; 00467 break; 00468 } 00469 case (28): 00470 { 00471 // Kiri + lingkaran 00472 ReloadOn = !ReloadOn; 00473 isReload = false; 00474 break; 00475 } 00476 case (29): 00477 { 00478 // Kanan + kotak 00479 ReloadOn = !ReloadOn; 00480 isReload = true; 00481 break; 00482 } 00483 case (30): 00484 { 00485 // Kiri + kotak 00486 ReloadOn = !ReloadOn; 00487 isReload = true; 00488 break; 00489 } 00490 case (13) : 00491 { 00492 // Serong kanan maju 00493 motorDpn.speed(-serong); 00494 motorL.speed(-serong-t_new); 00495 motorBlk.speed(serong); 00496 motorR.speed(serong+t_new); 00497 break; 00498 } 00499 case (14) : 00500 { 00501 // Serong kiri maju 00502 motorDpn.speed(serong); 00503 motorR.speed(serong+t_new); 00504 motorBlk.speed(-serong); 00505 motorL.speed(-serong-t_new); 00506 break; 00507 } 00508 case (15) : 00509 { 00510 // Serong kanan mundur 00511 motorDpn.speed(-serong); 00512 motorR.speed(-serong-t_new); 00513 motorBlk.speed(serong); 00514 motorL.speed(serong+t_new); 00515 break; 00516 } 00517 case (16) : 00518 { 00519 // Serong kiri mundur 00520 motorDpn.speed(serong); 00521 motorL.speed(serong+t_new); 00522 motorBlk.speed(-serong); 00523 motorR.speed(-serong-t_new); 00524 break; 00525 } 00526 case (3) : 00527 { 00528 // Kanan 00529 aksel++; 00530 if (aksel>300) 00531 tuneAksel = 0.9; 00532 00533 motorDpn.speed(-tuneAksel); 00534 motorBlk.speed(tuneAksel); 00535 motorR.brake(1); 00536 motorL.brake(1); 00537 break; 00538 } 00539 case (4) : 00540 { 00541 // Kiri 00542 aksel++; 00543 if (aksel>300) 00544 tuneAksel = 0.9; 00545 00546 motorDpn.speed(tuneAksel); 00547 motorBlk.speed(-tuneAksel); 00548 motorR.brake(1); 00549 motorL.brake(1); 00550 break; 00551 } 00552 case (8) : 00553 { 00554 // Maju 00555 aksel++; 00556 if (aksel>300) 00557 tuneAksel = 0.9; 00558 00559 motorR.speed(tuneAksel); 00560 motorL.speed(-tuneAksel); 00561 motorDpn.brake(1); 00562 motorBlk.brake(1); 00563 break; 00564 } 00565 case (9) : 00566 { 00567 // Mundur 00568 aksel++; 00569 if (aksel>300) 00570 tuneAksel = 0.9; 00571 00572 motorR.speed(-tuneAksel); 00573 motorL.speed(tuneAksel); 00574 motorDpn.brake(1); 00575 motorBlk.brake(1); 00576 break; 00577 } 00578 case (5) : 00579 { 00580 // Aktifkan motor atas 00581 isLauncher = !isLauncher; 00582 break; 00583 } 00584 case (6) : 00585 { 00586 // Target Pulse PID ++ Motor Depan 00587 target_rpm2 = target_rpm2+1.0; 00588 target_rpm = target_rpm+1.0; 00589 init_rpm(); 00590 break; 00591 } 00592 case (7) : 00593 { 00594 // Target Pulse PID -- Motor Depan 00595 target_rpm2 = target_rpm2-1.0; 00596 target_rpm = target_rpm-1.0; 00597 init_rpm(); 00598 break; 00599 } 00600 case (10) : 00601 { 00602 // Pneumatik 00603 if (ready) 00604 { 00605 pneumatik = 0; 00606 previousMillis3 = millis(); 00607 flag_Pneu = true; 00608 ready = false; 00609 previousMillis6 = millis(); 00610 00611 } 00612 break; 00613 } 00614 case (11) : 00615 { 00616 // Power Screw Up 00617 ReloadOn = !ReloadOn; 00618 isReload = false; 00619 break; 00620 } 00621 case (31) : 00622 { 00623 // start 00624 target_rpm2 = 24; 00625 target_rpm = 24; 00626 init_rpm(); 00627 break; 00628 } 00629 case (32) : 00630 { 00631 // select 00632 target_rpm2 = 11; 00633 target_rpm = 11; 00634 init_rpm(); 00635 break; 00636 } 00637 case (33) : 00638 { 00639 // R3 00640 target_rpm2 = 17; 00641 target_rpm = 17; 00642 init_rpm(); 00643 break; 00644 } 00645 case (12) : 00646 { 00647 // Power Screw Down 00648 ReloadOn = !ReloadOn; 00649 isReload = true; 00650 break; 00651 } 00652 case (34) :{ 00653 pneu_paku = !pneu_paku; 00654 wait_ms(50); 00655 if (pneu_paku == 1){ 00656 PIVOT = 0.17; 00657 }else{ 00658 PIVOT = 0.8; 00659 } 00660 } 00661 default : 00662 { 00663 tuneAksel = 0.6; 00664 aksel = 0; 00665 motorDpn.brake(1); 00666 motorBlk.brake(1); 00667 motorR.brake(1); 00668 motorL.brake(1); 00669 } 00670 } // End Switch 00671 } 00672 00673 void reloader() 00674 { 00675 if(ReloadOn){ 00676 if(isReload){ 00677 powerScrew.speed(pwmPowerDown); 00678 pc.printf("%.2f\n", jarak_ping); 00679 if((!limitBawah)||(!limitBawah1)){ 00680 isReload = false; 00681 ReloadOn = false; 00682 } 00683 } 00684 else if(!limitTengah){ 00685 isReload = true; 00686 } 00687 else if(!flag_Pneu){ 00688 //pc.printf("%.2f\n", ping_pwm); 00689 if (millis()-previousMillis6>700) 00690 { 00691 ping_current_error = (double) (ping_target-jarak_ping); 00692 00693 ping_sum_error += ping_current_error*ping_Ts; 00694 ping_pwm = (double) ping_Kp*ping_current_error + ping_Kd*(ping_current_error-ping_previous_error1)/ping_Ts; 00695 00696 if (ping_pwm>1) ping_pwm=1; 00697 if (ping_pwm>0.049 && ping_pwm<0.5) ping_pwm = 0.5; 00698 if (ping_pwm<-0.049 && ping_pwm>-0.3) ping_pwm = -0.3; 00699 if (ping_pwm<-1) ping_pwm=-1; 00700 00701 powerScrew.speed(ping_pwm); 00702 00703 ping_previous_error1 = ping_current_error; 00704 } 00705 00706 } 00707 if ((jarak_ping>(ping_target-2))&&(jarak_ping<(ping_target+2))){ 00708 ready = true; 00709 }else{ 00710 ready = false; 00711 } 00712 } 00713 else{ 00714 powerScrew.brake(1); 00715 } 00716 } 00717 00718 00719 void launcher() 00720 { 00721 if (isLauncher) 00722 { 00723 currentMillis = millis(); 00724 currentMillis2 = millis(); 00725 00726 // PID Launcher Belakang 00727 if (currentMillis-previousMillis>=Ts) 00728 { 00729 rpm = (float)encLauncherBlk.getPulses(); 00730 current_error1 = target_rpm - rpm; 00731 a1 = kpA1 + kiA1*Ts/2 + kdA1/Ts; 00732 b1 = -kpA1 + kiA1*Ts/2 - 2*kdA1/Ts; 00733 c1 = kdA1/Ts; 00734 speed = previous_speed1 + a1*current_error1 + b1*previous_error1_1 + c1*previous_error1_2; 00735 //init_speed(); 00736 if(speed > maxSpeed){ 00737 launcherBlk.speed(maxSpeed); 00738 } 00739 else if ( speed < minSpeed){ 00740 launcherBlk.speed(minSpeed); 00741 } 00742 else { 00743 launcherBlk.speed(speed); 00744 } 00745 previous_speed1 = speed; 00746 previous_error1_2 = previous_error1_1; 00747 previous_error1_1 = current_error1; 00748 encLauncherBlk.reset(); 00749 previousMillis = currentMillis; 00750 00751 } 00752 // PID Launcher Depan 00753 if (currentMillis2-previousMillis2>=Ts) 00754 { 00755 rpm2 = (float)encLauncherDpn.getPulses(); 00756 current_error2 = target_rpm2 - rpm2; 00757 a2 = kpA2 + kiA2*Ts/2 + kdA2/Ts; 00758 b2 = -kpA2 + kiA2*Ts/2 - 2*kdA2/Ts; 00759 c2 = kdA2/Ts; 00760 speed2 = previous_speed2 + a2*current_error2 + b2*previous_error2_1 + c2*previous_error2_2; 00761 //init_speed(); 00762 if (speed2 > maxSpeed){ 00763 launcherDpn.speed(maxSpeed); 00764 } 00765 else if ( speed2 < minSpeed){ 00766 launcherDpn.speed(minSpeed); 00767 } 00768 else{ 00769 launcherDpn.speed(speed2); 00770 } 00771 previous_speed2 = speed2; 00772 previous_error2_2 = previous_error2_1; 00773 previous_error2_1 = current_error2; 00774 encLauncherDpn.reset(); 00775 previousMillis2 = currentMillis2; 00776 } 00777 //pc.printf("%.2f\t%.2f\n",rpm,rpm2); 00778 } 00779 else 00780 { 00781 launcherDpn.brake(1); 00782 launcherBlk.brake(1); 00783 00784 previous_error1_1 = 0; 00785 previous_error1_2 = 0; 00786 previous_error2_1 = 0; 00787 previous_error2_2 = 0; 00788 previous_speed1 = 0; 00789 previous_speed2 = 0; 00790 } 00791 } 00792 00793 /*********************************************************/ 00794 /* Main Function */ 00795 /*********************************************************/ 00796 00797 int main (void) 00798 { 00799 // Set baud rate - 115200 00800 joystick.setup(); 00801 pc.baud(115200); 00802 wait_ms(1000); 00803 00804 // initializing encoder 00805 pneumatik =1; 00806 00807 wait_ms(500); 00808 00809 //initializing PING 00810 pingAtas.Send(); 00811 00812 pc.printf("ready...."); 00813 startMillis(); 00814 while(1) 00815 { 00816 // interupsi pembacaan PING setiap 30 ms 00817 if(millis() - previousMillis4 >= 10){ //30 00818 jarak_ping = (float)pingAtas.Read_cm(); 00819 00820 pingAtas.Send(); 00821 previousMillis4 = millis(); 00822 } 00823 00824 // Interrupt Serial 00825 joystick.idle(); 00826 if(joystick.readable()) 00827 { 00828 // Panggil fungsi pembacaan joystik 00829 joystick.baca_data(); 00830 // Panggil fungsi pengolahan data joystik 00831 joystick.olah_data(); 00832 // Masuk ke case joystick 00833 case_joy = case_joystick(); 00834 //pc.printf("%d\n",case_joy); 00835 aktuator(); 00836 launcher(); 00837 reloader(); 00838 if ((millis()-previousMillis3 >= 230)&&(flag_Pneu)){ 00839 pneumatik = 1; 00840 flag_Pneu = false; 00841 //wait_ms(1000); 00842 } 00843 } 00844 else 00845 { 00846 joystick.idle(); 00847 } 00848 00849 if(millis() - previousMillis5 >= 400) 00850 { 00851 display.write(0,((int) rpm2) / 10); 00852 display.write(1,((int)rpm2) % 10); 00853 display.write(2, (int)target_rpm2 / 10); 00854 display.write(3, (int)target_rpm2 % 10); 00855 display.setColon(true); 00856 00857 previousMillis5 = millis(); 00858 } 00859 } 00860 }
Generated on Thu Jul 14 2022 11:58:13 by
1.7.2