KRAI 2017
/
Joystick_OrdoV4
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: Motor PID Joystic_OrdoV3 mbed
Fork of
Joystick_OrdoV4
by 
Joshua Gunawan
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 /****************************************************************************/ 00002 /* PROGRAM UNTUK PID CLOSED LOOP */ 00003 /* */ 00004 /* - Digunakan encoder autonics */ 00005 /* - Konfigurasi Motor dan Encoder sbb : */ 00006 /* _________________ */ 00007 /* | DEPAN | */ 00008 /* | 1. e .2 | Angka ==> Motor */ 00009 /* | ` ` | e ==> Encoder */ 00010 /* | e e | */ 00011 /* | . . | */ 00012 /* | 4` e `3 | */ 00013 /* |________________| */ 00014 /* */ 00015 /* SETTINGS (WAJIB!) : */ 00016 /* 1. Settings Pin Encoder, Resolusi, dan Tipe encoding di omniPos.h */ 00017 /* 2. Deklarasi penggunaan library omniPos pada bagian deklarasi encoder */ 00018 /* */ 00019 /****************************************************************************/ 00020 /* */ 00021 /* Joystick */ 00022 /* kanan => posisi target x ditambah 0.01 */ 00023 /* kiri => posisi target x dikurang 0.01 */ 00024 /* atas => posisi target y ditambah 0.01 */ 00025 /* bawah => posisi target y dikurang 0.01 */ 00026 /* */ 00027 /* Tombol silang => Kembali keposisi Awal */ 00028 /* Tombol segitiga => Aktif motor Launcher */ 00029 /* Tombol kotak => Aktif servo Launcher */ 00030 /* */ 00031 /****************************************************************************/ 00032 00033 00034 #include "mbed.h" 00035 #include "JoystickPS3.h" 00036 #include "Motor.h" 00037 #include "Servo.h" 00038 00039 //#define koefperlambatan 0.8 00040 #include "encoderKRAI.h" 00041 00042 #define pi 22/7 00043 #define diaEncoder 0.058 00044 #define PPR 1000 00045 #define diaRobot 0.64 00046 #define pinservo1 PC_8 00047 //PC 9 00048 #define pinservo2 PC_9 00049 00050 float K_enc = pi*diaEncoder; 00051 float K_robot = pi*diaRobot; 00052 00053 float speed1=0.6; 00054 float speed2=0.6; 00055 float speed3=0.6; 00056 float speed4=0.6; 00057 float speedL=0.2; 00058 00059 float KpX=0.5, KpY=0.5, Kp_tetha=0.03; 00060 00061 Timer waktu; 00062 //float jarakGlobalY; 00063 00064 // Deklarasi variabel PID 00065 //PID PID(0.992,0.000,0.81,0.001); //(P,I,D, time sampling) 00066 00067 // Deklarasi variabel encoder 00068 encoderKRAI encoderDepan( PB_14, PB_13, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan 00069 encoderKRAI encoderBelakang( PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING); 00070 encoderKRAI encoderKanan( PD_2, PC_12, 720, encoderKRAI::X2_ENCODING); 00071 encoderKRAI encoderKiri( PC_11, PC_10, 2000, encoderKRAI::X2_ENCODING); 00072 00073 // Deklarasi variabel motor 00074 Motor motor1(PB_8, PB_1 , PA_13); // pwm, fwd, rev 00075 Motor motor2(PB_9, PA_12, PC_5); // pwm, fwd, rev 00076 Motor motor3(PB_6, PA_7 , PB_12); // pwm, fwd, rev 00077 Motor motor4(PB_7, PA_14 ,PA_15); // pwm, fwd, rev 00078 00079 //Motor Atas 00080 Motor motorld(PA_8, PB_0 ,PC_15); // pwm, fwd, rev 00081 Motor motorlb(PA_11, PA_5 ,PA_6); // pwm, fwd, rev 00082 00083 //Servo Atas 00084 Servo servoS(pinservo1); 00085 Servo servoB(pinservo2); 00086 00087 // Deklarasi variabel posisi robot 00088 //robotPos posisi(); 00089 00090 // Inisialisasi Pin TX-RX Joystik dan PC 00091 //Serial pc(PA_0,PA_1); 00092 //Serial pc(USBTX,USBRX); 00093 // Deklarasi Variabel Global 00094 /* 00095 * posX dan posY berdasarkan arah robot 00096 * encoder Depan & Belakang sejajar sumbu Y 00097 * encoder Kanan & Kiri sejajar sumbu X 00098 */ 00099 float jarak, posX, posY; 00100 //float keliling = pi*diameterRoda; 00101 00102 void detect_encoder(float speed); 00103 void setCenter(); 00104 float getY(); 00105 float getX(); 00106 float getTetha(); 00107 00108 00109 00110 00111 // Inisialisasi Pin TX-RX Joystik dan PC 00112 joysticknucleo joystick(PA_0,PA_1); 00113 Serial pc(USBTX,USBRX); 00114 00115 // Posisi target 00116 float XT, YT, Tetha; 00117 00118 //encoder variable 00119 float errX, errY, errT, Vt, Vx, Vy; 00120 float V1, V2, V3, V4; 00121 00122 //bool perlambatan=0; 00123 char case_ger; 00124 bool maju=false,mundur=false,pivka=false,pivki=false,kiri=false,kanan=false,saka=false,saki=false,sbka=false,sbki=false,cw1=false,ccw1=false,cw2=false,ccw2=false,cw3=false,ccw3=false,analog=false; 00125 bool stop = true; 00126 bool Launcher = false,ServoGo = false; 00127 float jLX,jLY; 00128 double vcurr; 00129 float x,y; // untuk analoghat kiri 00130 float errorx=0,errory=0; 00131 00132 // Fungsi mapping 0-255 ke -128 sampai 127 00133 float mapping(float a,float error) 00134 { 00135 float hasil,b; 00136 b = (float)((a-128)/128); 00137 if (b>(error - 0.2) && b<(error + 0.2)) 00138 { 00139 hasil = 0; 00140 } else { 00141 hasil = b; 00142 } 00143 return (hasil); 00144 } 00145 00146 // Kalibrasi tombol analog kiri 00147 void kalibrasi() 00148 { 00149 errorx = (jLX - 128)/128; 00150 errory = (jLY - 128)/128; 00151 00152 } 00153 00154 // simpan data analog 00155 void baca_analog() 00156 { 00157 jLX = joystick.LX; 00158 jLY = joystick.LY; 00159 00160 // Pembacaan nilai Y terbalik 00161 x = mapping(jLX,errorx); 00162 y = -mapping(jLY,errory); 00163 } 00164 00165 // Gerak dari Motor base 00166 int case_gerak() 00167 { 00168 int casegerak; 00169 baca_analog(); 00170 if (!joystick.L1 && joystick.R1) { 00171 // Pivot Kanan 00172 casegerak = 1; 00173 } else if (!joystick.R1 && joystick.L1) { 00174 // Pivot Kiri 00175 casegerak = 2; 00176 } else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { 00177 // Maju 00178 casegerak = 3; 00179 } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { 00180 // Mundur 00181 casegerak = 4; 00182 } else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan)) { 00183 // Serong Atas Kanan 00184 casegerak = 5; 00185 } else if((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan)) { 00186 // Serong Bawah Kanan 00187 casegerak = 6; 00188 } else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)) { 00189 // Serong Atas Kiri 00190 casegerak = 7; 00191 } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)) { 00192 // Serong Bawah Kiri 00193 casegerak = 8; 00194 } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) { 00195 // Kanan 00196 casegerak = 9; 00197 } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { 00198 // Kiri 00199 casegerak = 10; 00200 } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { 00201 // case gerak analog on off 00202 if (analog){ 00203 casegerak = 11; 00204 } else { 00205 casegerak = 12; 00206 } 00207 } 00208 return(casegerak); 00209 } 00210 00211 00212 00213 /** 00214 00215 ** Case 1 : Pivot kanan 00216 ** Case 2 : Pivot Kiri 00217 ** Case 3 : Maju 00218 ** Case 4 : Mundur 00219 ** Case 5 : Serong Atas Kanan 00220 ** Case 6 : Serong Bawah Kanan 00221 ** Case 7 : Serong Atas Kiri 00222 ** Case 8 : Serong Bawah Kiri 00223 ** Case 9 : Kanan 00224 ** Case 10 : Kiri 00225 ** Case 11 : Analog 00226 ** Case 11 : break 00227 00228 **/ 00229 00230 00231 // Pergerakan dari base 00232 void aktuator() 00233 { 00234 //Servo 00235 if (ServoGo){ 00236 servoS.position(-90); 00237 wait_ms(500); 00238 servoS.position(10); 00239 wait_ms(500); 00240 for (int i = 0; i<=90; i++){ 00241 servoB.position(i); 00242 wait_ms(10); 00243 } 00244 wait_ms(500); 00245 servoB.position(0); 00246 ServoGo = false; 00247 00248 }else{ 00249 servoS.position(10); 00250 servoB.position(0); 00251 00252 } 00253 00254 // Motor Atas 00255 if (Launcher) { 00256 motorld.speed(speedL); 00257 motorlb.speed(speedL); 00258 }else{ 00259 motorld.speed(0); 00260 motorlb.speed(0); 00261 } 00262 00263 // MOTOR Bawah 00264 switch (case_ger) 00265 { 00266 case (1): 00267 { 00268 Tetha = Tetha - 0.5; 00269 pivka=true; 00270 maju=mundur=analog=kiri=kanan=saka=saki=sbka=sbki=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; 00271 00272 pc.printf("pivKa Xt =%.2f x=%.2f YT=%.2f y=%.2f errx=%.2f erry=%.2f \n",XT,x,YT,y,errX,errY); 00273 00274 00275 break; 00276 } 00277 case (2): 00278 { 00279 Tetha = Tetha + 0.5; 00280 00281 pivki=true; 00282 maju=mundur=kiri=analog=kanan=saka=saki=sbka=sbki=pivka=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; 00283 00284 pc.printf("pivKi Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y); 00285 00286 00287 break; 00288 } 00289 case (3): 00290 { 00291 YT = YT + 0.01; 00292 00293 maju=true; 00294 mundur=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; 00295 00296 pc.printf("maju Xt =%.2f x=%.2f YT=%.2f y=%.2f errx=%.2f erry=%.2f \n",XT,x,YT,y,errX,errY); 00297 00298 break; 00299 } 00300 case (4): 00301 { 00302 YT = YT - 0.01; 00303 00304 mundur=true; 00305 maju=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; 00306 00307 pc.printf("mundur Xt =%.2f x=%.2f YT=%.2f y=%.2f errx=%.2f erry=%.2f \n",XT,x,YT,y,errX,errY); 00308 00309 00310 break; 00311 } 00312 case (5) : 00313 { 00314 XT = XT + 0.01; 00315 YT = YT + 0.01; 00316 00317 saka=true; 00318 maju=mundur=kiri=kanan=sbka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; 00319 00320 pc.printf("saka Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y); 00321 00322 00323 break; 00324 } 00325 case (6) : 00326 { 00327 XT = XT + 0.01; 00328 YT = YT - 0.01; 00329 00330 00331 sbka=true; 00332 maju=mundur=kiri=kanan=saka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; 00333 00334 pc.printf("sbka Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y); 00335 00336 00337 break; 00338 } 00339 case (7) : 00340 { 00341 XT = XT - 0.01; 00342 YT = YT + 0.01; 00343 00344 00345 saki=true; 00346 maju=kiri=kanan=saka=mundur=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; 00347 00348 pc.printf("saki Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y); 00349 00350 00351 break; 00352 } 00353 case (8) : 00354 { 00355 XT = XT - 0.01; 00356 YT = YT - 0.01; 00357 00358 pc.printf("sbki Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y); 00359 00360 00361 break; 00362 } 00363 case (9) : 00364 { 00365 XT = XT + 0.01; 00366 kanan=true; 00367 maju=kiri=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; 00368 00369 pc.printf("Kanan Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y); 00370 00371 break; 00372 } 00373 case (10) : 00374 { 00375 XT = XT - 0.01; 00376 00377 kiri=true; 00378 maju=kanan=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; 00379 00380 pc.printf("Kiri Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y); 00381 00382 00383 break; 00384 } 00385 case (11): 00386 { 00387 00388 XT = XT + 0.01*x; 00389 YT = YT + 0.01*y; 00390 00391 analog=true; 00392 maju=mundur=kiri=kanan=saka=saki=sbka=sbki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; 00393 00394 pc.printf("analog Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y); 00395 00396 break; 00397 } 00398 default : 00399 { 00400 00401 maju=mundur=kiri=kanan=saka=saki=sbka=sbki=analog=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; 00402 stop = true; 00403 00404 //s1 = 0;s2 =0; s3 =0; s4 =0; 00405 00406 // pc.printf("Stop V1=%.2f V2=%.2f V3=%.2f V4=%.2f\n",V1,V2,V3,V4); 00407 } 00408 } 00409 } 00410 00411 //Begin Encoder 00412 00413 void setCenter() 00414 { 00415 encoderDepan.reset(); 00416 encoderBelakang.reset(); 00417 encoderKanan.reset(); 00418 encoderKiri.reset(); 00419 } 00420 00421 float getX() 00422 { 00423 float jarakEncDpn, jarakEncBlk; 00424 jarakEncDpn = (encoderDepan.getPulses())/(float)(2000.0)*K_enc; 00425 jarakEncBlk = (encoderBelakang.getPulses())/(float)(2000.0)*K_enc; 00426 return (jarakEncDpn-jarakEncBlk)/2; 00427 } 00428 00429 float getY() 00430 { 00431 float jarakEncKir, jarakEncKan; 00432 jarakEncKir = (encoderKiri.getPulses())/(float)(2000.0)*K_enc; 00433 jarakEncKan = (encoderKanan.getPulses())/(float)(720.0)*K_enc; 00434 return (jarakEncKir-jarakEncKan)/2; 00435 } 00436 00437 float getTetha() 00438 { 00439 float busurDpn, busurBlk, busurKir, busurKan; 00440 busurDpn = ((encoderDepan.getPulses())/(float)(2000.0)*K_enc)/K_robot*360.0; 00441 busurBlk = ((encoderBelakang.getPulses())/(float)(2000.0)*K_enc)/K_robot*360.0; 00442 busurKir = ((encoderKiri.getPulses())/(float)(2000.0)*K_enc)/K_robot*360.0; 00443 busurKan = ((encoderKanan.getPulses())/(float)(720.0)*K_enc)/K_robot*360.0; 00444 00445 return -(busurDpn+busurBlk+busurKir+busurKan)/4; 00446 } 00447 00448 void gotoXYT(float xa, float ya, float Ta) 00449 { 00450 00451 errX = xa-getX(); 00452 Vx = KpX*errX; 00453 00454 errY = ya-getY(); 00455 Vy = KpY*errY; 00456 00457 errT = Ta-getTetha(); 00458 Vt = Kp_tetha*errT; 00459 00460 V1 = Vx+Vy-Vt; 00461 V2 = Vx-Vy-Vt; 00462 V3 = -Vx-Vy-Vt; 00463 V4 = -Vx+Vy-Vt; 00464 00465 if (V1>speed1) 00466 { V1 = speed1; } 00467 else if (V1<-speed1) 00468 { V1 = -speed1; } 00469 00470 if (V2>speed2) 00471 { V2 = speed2; } 00472 else if (V2<-speed2) 00473 { V2 = -speed2; } 00474 00475 if (V3>speed3) 00476 { V3 = speed3; } 00477 else if (V3<-speed3) 00478 { V3 = -speed3; } 00479 00480 if (V4>speed4) 00481 { V4 = speed4; } 00482 else if (V4<-speed4) 00483 { V4 = -speed4; } 00484 00485 if (((errX > 0.05) || (errX<-0.05)) || ((errY > 0.05) || (errY<-0.05)) || ((errT > 0.05) || (errT<-0.05))) 00486 { 00487 motor1.speed(V1); 00488 motor2.speed(V2); 00489 motor3.speed(V3); 00490 motor4.speed(V4); 00491 // pc.printf("V1=%.2f \ ", V1); 00492 } 00493 else 00494 { 00495 motor1.brake(1); 00496 motor2.brake(1); 00497 motor3.brake(1); 00498 motor4.brake(1); 00499 //_ms(1000); 00500 } 00501 //pc.printf("%f\t%f\t%f ", errX*100,errY*100,errT); 00502 // wait_ms(10); 00503 00504 } 00505 //End Encoder 00506 00507 void speedLauncher() 00508 { 00509 if (joystick.R3_click and speedL < 0.7){ 00510 speedL = speedL + 0.1;} 00511 if (joystick.L3_click and speedL > 0.1){ 00512 speedL = speedL - 0.1;} 00513 00514 } 00515 00516 int main (void) 00517 { 00518 // Set baud rate - 115200 00519 joystick.setup(); 00520 pc.baud(115200); 00521 wait_ms(1000); 00522 setCenter(); 00523 wait_ms(500); 00524 00525 //Posisi Awal 00526 XT = 0; 00527 YT = 0; 00528 Tetha = 0; 00529 pc.printf("Ready...\n"); 00530 kalibrasi(); 00531 servoS.position(90); 00532 servoB.position(0); 00533 waktu.start(); 00534 while(1) 00535 { 00536 // Interrupt Serial 00537 joystick.idle(); 00538 if(joystick.readable() ) { 00539 // Panggil fungsi pembacaan joystik 00540 joystick.baca_data(); 00541 // Panggil fungsi pengolahan data joystik 00542 joystick.olah_data(); 00543 //pc.printf("%2x %2x %2x %2x %3d %3d %3d %3d %3d %3d\n\r",joystick.button, joystick.RL, joystick.button_click, joystick.RL_click, joystick.R2, joystick.L2, joystick.RX, joystick.RY, joystick.LX, joystick.LY); 00544 case_ger = case_gerak(); 00545 aktuator(); 00546 00547 pc.printf("bacaS = %.2f\tbacaB = %.2f\n",servoS.read(), servoB.read()); 00548 00549 //kalibrasi 00550 if (joystick.START){ 00551 kalibrasi();} 00552 // analog switch mode 00553 if (joystick.SELECT_click) {analog = !analog;} 00554 if (joystick.segitiga_click) {Launcher = !Launcher;} 00555 if (joystick.lingkaran_click){ 00556 ServoGo = true; 00557 waktu.reset(); 00558 } 00559 if (joystick.silang) { 00560 XT = 0; 00561 YT = 0; 00562 Tetha = 0; 00563 pc.printf("x..\n"); 00564 } 00565 speedLauncher(); 00566 } else { 00567 joystick.idle(); 00568 00569 } 00570 gotoXYT(XT,YT,Tetha); 00571 00572 } 00573 }
Generated on Mon Jul 18 2022 06:07:58 by
1.7.2