KRTMI 2019
Kodingan KRAI 2017
Dependencies: mbed DigitDisplay PID Motor Ping millis
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)//kanan \ Omniwheel */ 00012 /* | | */ 00013 /* | 3 (kiri)//depan 4 (kanan) | Roda Kiri Kanan: */ 00014 /* | | Omniwheel */ 00015 /* \ 1 (Depan)//kiri / */ 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.5; //0.17 // PWM Pivot Kanan, Pivot Kiri 00085 float tuneDpn = 1.0; //1.0 // Tunning PWM motor Depan 00086 float tuneBlk = 1.0; //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_0,PA_1); 00135 Serial pc(USBTX,USBRX,115200); 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_6, PC_14, PC_13); 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(-rasio*PIVOT-t_new); 00352 motorBlk.speed(-rasio*PIVOT-t_new); 00353 ////////////////MotorR.speed(-rasio*PIVOT-t_new); 00354 motorL.speed(-PIVOT); 00355 break; 00356 } 00357 case (2): 00358 { 00359 // Pivot Kiri 00360 motorDpn.speed(rasio*PIVOT+t_new); 00361 motorBlk.speed(rasio*PIVOT+t_new); 00362 ////////////////MotorR.speed(rasio*PIVOT+t_new); 00363 motorL.speed(PIVOT); 00364 break; 00365 } 00366 case (17): 00367 { 00368 // Kanan + Rotasi Kanan 00369 motorDpn.speed(-PIVOT); 00370 motorBlk.speed(-rasio*PIVOT-t_new); 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(rasio*PIVOT+t_new); 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(1); 00549 motorDpn.brake(-tuneAksel); 00550 motorBlk.speed(tuneAksel); 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(1); 00562 motorDpn.brake(tuneAksel); 00563 motorBlk.brake(-tuneAksel); 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 previousMillis6 = millis(); 00598 00599 } 00600 break; 00601 } 00602 case (11) : 00603 { 00604 // Power Screw Up 00605 ReloadOn = !ReloadOn; 00606 isReload = false; 00607 break; 00608 } 00609 case (31) : 00610 { 00611 // start 00612 target_rpm2 = 24; 00613 target_rpm = 24; 00614 init_rpm(); 00615 break; 00616 } 00617 case (32) : 00618 { 00619 // select 00620 target_rpm2 = 11; 00621 target_rpm = 11; 00622 init_rpm(); 00623 break; 00624 } 00625 case (33) : 00626 { 00627 // R3 00628 target_rpm2 = 17; 00629 target_rpm = 17; 00630 init_rpm(); 00631 break; 00632 } 00633 case (12) : 00634 { 00635 // Power Screw Down 00636 ReloadOn = !ReloadOn; 00637 isReload = true; 00638 break; 00639 } 00640 case (34) :{ 00641 pneu_paku = !pneu_paku; 00642 wait_ms(50); 00643 if (pneu_paku == 1){ 00644 PIVOT = 0.17; 00645 }else{ 00646 PIVOT = 0.8; 00647 } 00648 } 00649 default : 00650 { 00651 tuneAksel = 0.6; 00652 aksel = 0; 00653 motorDpn.brake(1); 00654 motorBlk.brake(1); 00655 //////////////MotorR.brake(1); 00656 motorL.brake(1); 00657 } 00658 } // End Switch 00659 } 00660 00661 /*void reloader() 00662 { 00663 if(ReloadOn){ 00664 if(isReload){ 00665 powerScrew.speed(pwmPowerDown); 00666 pc.printf("%.2f\n", jarak_ping); 00667 if((!limitBawah)||(!limitBawah1)){ 00668 isReload = false; 00669 ReloadOn = false; 00670 } 00671 } 00672 else if(!limitTengah){ 00673 isReload = true; 00674 } 00675 else if(!flag_Pneu){ 00676 //pc.printf("%.2f\n", ping_pwm); 00677 if (millis()-previousMillis6>700) 00678 { 00679 ping_current_error = (double) (ping_target-jarak_ping); 00680 00681 ping_sum_error += ping_current_error*ping_Ts; 00682 ping_pwm = (double) ping_Kp*ping_current_error + ping_Kd*(ping_current_error-ping_previous_error1)/ping_Ts; 00683 00684 if (ping_pwm>1) ping_pwm=1; 00685 if (ping_pwm>0.049 && ping_pwm<0.5) ping_pwm = 0.5; 00686 if (ping_pwm<-0.049 && ping_pwm>-0.3) ping_pwm = -0.3; 00687 if (ping_pwm<-1) ping_pwm=-1; 00688 00689 powerScrew.speed(ping_pwm); 00690 00691 ping_previous_error1 = ping_current_error; 00692 } 00693 00694 } 00695 if ((jarak_ping>(ping_target-2))&&(jarak_ping<(ping_target+2))){ 00696 ready = true; 00697 }else{ 00698 ready = false; 00699 } 00700 } 00701 else{ 00702 powerScrew.brake(1); 00703 } 00704 } 00705 00706 00707 void launcher() 00708 { 00709 if (isLauncher) 00710 { 00711 currentMillis = millis(); 00712 currentMillis2 = millis(); 00713 00714 // PID Launcher Belakang 00715 if (currentMillis-previousMillis>=Ts) 00716 { 00717 rpm = (float)encLauncherBlk.getPulses(); 00718 current_error1 = target_rpm - rpm; 00719 a1 = kpA1 + kiA1*Ts/2 + kdA1/Ts; 00720 b1 = -kpA1 + kiA1*Ts/2 - 2*kdA1/Ts; 00721 c1 = kdA1/Ts; 00722 speed = previous_speed1 + a1*current_error1 + b1*previous_error1_1 + c1*previous_error1_2; 00723 //init_speed(); 00724 if(speed > maxSpeed){ 00725 launcherBlk.speed(maxSpeed); 00726 } 00727 else if ( speed < minSpeed){ 00728 launcherBlk.speed(minSpeed); 00729 } 00730 else { 00731 launcherBlk.speed(speed); 00732 } 00733 previous_speed1 = speed; 00734 previous_error1_2 = previous_error1_1; 00735 previous_error1_1 = current_error1; 00736 encLauncherBlk.reset(); 00737 previousMillis = currentMillis; 00738 00739 } 00740 // PID Launcher Depan 00741 if (currentMillis2-previousMillis2>=Ts) 00742 { 00743 rpm2 = (float)encLauncherDpn.getPulses(); 00744 current_error2 = target_rpm2 - rpm2; 00745 a2 = kpA2 + kiA2*Ts/2 + kdA2/Ts; 00746 b2 = -kpA2 + kiA2*Ts/2 - 2*kdA2/Ts; 00747 c2 = kdA2/Ts; 00748 speed2 = previous_speed2 + a2*current_error2 + b2*previous_error2_1 + c2*previous_error2_2; 00749 //init_speed(); 00750 if (speed2 > maxSpeed){ 00751 launcherDpn.speed(maxSpeed); 00752 } 00753 else if ( speed2 < minSpeed){ 00754 launcherDpn.speed(minSpeed); 00755 } 00756 else{ 00757 launcherDpn.speed(speed2); 00758 } 00759 previous_speed2 = speed2; 00760 previous_error2_2 = previous_error2_1; 00761 previous_error2_1 = current_error2; 00762 encLauncherDpn.reset(); 00763 previousMillis2 = currentMillis2; 00764 } 00765 //pc.printf("%.2f\t%.2f\n",rpm,rpm2); 00766 } 00767 else 00768 { 00769 launcherDpn.brake(1); 00770 launcherBlk.brake(1); 00771 00772 previous_error1_1 = 0; 00773 previous_error1_2 = 0; 00774 previous_error2_1 = 0; 00775 previous_error2_2 = 0; 00776 previous_speed1 = 0; 00777 previous_speed2 = 0; 00778 } 00779 } 00780 */ 00781 /*********************************************************/ 00782 /* Main Function */ 00783 /*********************************************************/ 00784 00785 int main (void) 00786 { 00787 // Set baud rate - 115200 00788 joystick.setup(); 00789 pc.baud(115200); 00790 wait_ms(1000); 00791 00792 // initializing encoder 00793 pneumatik =1; 00794 00795 wait_ms(500); 00796 00797 //initializing PING 00798 pingAtas.Send(); 00799 00800 pc.printf("ready...."); 00801 startMillis(); 00802 while(1) 00803 { 00804 // interupsi pembacaan PING setiap 30 ms 00805 if(millis() - previousMillis4 >= 10){ //30 00806 jarak_ping = (float)pingAtas.Read_cm(); 00807 00808 pingAtas.Send(); 00809 previousMillis4 = millis(); 00810 } 00811 00812 // Interrupt Serial 00813 joystick.idle(); 00814 if(joystick.readable()) 00815 { 00816 // Panggil fungsi pembacaan joystik 00817 joystick.baca_data(); 00818 // Panggil fungsi pengolahan data joystik 00819 joystick.olah_data(); 00820 // Masuk ke case joystick 00821 case_joy = case_joystick(); 00822 //pc.printf("%d\n",case_joy); 00823 aktuator(); 00824 //launcher(); 00825 //reloader(); 00826 if ((millis()-previousMillis3 >= 230)&&(flag_Pneu)){ 00827 pneumatik = 1; 00828 flag_Pneu = false; 00829 //wait_ms(1000); 00830 } 00831 } 00832 else 00833 { 00834 joystick.idle(); 00835 } 00836 00837 if(millis() - previousMillis5 >= 400) 00838 { 00839 display.write(0,((int) rpm2) / 10); 00840 display.write(1,((int)rpm2) % 10); 00841 display.write(2, (int)target_rpm2 / 10); 00842 display.write(3, (int)target_rpm2 % 10); 00843 display.setColon(true); 00844 00845 previousMillis5 = millis(); 00846 } 00847 } 00848 }
Generated on Sat Jul 16 2022 13:40:32 by
1.7.2