Joshua Gunawan / Mbed 2 deprecated Joystick_OrdoV4

Tambahan codingan servo, Cek pin servo terlebih dahulu

Dependencies:   Motor PID Joystic_OrdoV3 mbed

Fork of Joystick_OrdoV3 by KRAI 2017

main.cpp@5:3aa203218306, 2016-11-22 (annotated)

Committer:
rahmadirizki18
Date:
Tue Nov 22 15:30:31 2016 +0000
Revision:
5:3aa203218306
Parent:
4:483c07cc22e1
Child:
6:68293bed71ea
lintasan masih curve, balik ke posisi awal masih error besar

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rahmadirizki18 5:3aa203218306 1 /****************************************************************************/
rahmadirizki18 5:3aa203218306 2 /* PROGRAM UNTUK PID CLOSED LOOP */
rahmadirizki18 5:3aa203218306 3 /* */
rahmadirizki18 5:3aa203218306 4 /* - Digunakan encoder autonics */
rahmadirizki18 5:3aa203218306 5 /* - Konfigurasi Motor dan Encoder sbb : */
rahmadirizki18 5:3aa203218306 6 /* _________________ */
rahmadirizki18 5:3aa203218306 7 /* | DEPAN | */
rahmadirizki18 5:3aa203218306 8 /* | 1. e .2 | Angka ==> Motor */
rahmadirizki18 5:3aa203218306 9 /* | ` ` | e ==> Encoder */
rahmadirizki18 5:3aa203218306 10 /* | e e | */
rahmadirizki18 5:3aa203218306 11 /* | . . | */
rahmadirizki18 5:3aa203218306 12 /* | 4` e `3 | */
rahmadirizki18 5:3aa203218306 13 /* |________________| */
rahmadirizki18 5:3aa203218306 14 /* */
rahmadirizki18 5:3aa203218306 15 /* SETTINGS (WAJIB!) : */
rahmadirizki18 5:3aa203218306 16 /* 1. Settings Pin Encoder, Resolusi, dan Tipe encoding di omniPos.h */
rahmadirizki18 5:3aa203218306 17 /* 2. Deklarasi penggunaan library omniPos pada bagian deklarasi encoder */
rahmadirizki18 5:3aa203218306 18 /* */
rahmadirizki18 5:3aa203218306 19 /****************************************************************************/
fanny868 0:9072e932503c 20
fanny868 0:9072e932503c 21 #include "mbed.h"
fanny868 0:9072e932503c 22 #include "JoystickPS3.h"
fanny868 0:9072e932503c 23 #include "Motor.h"
fanny868 0:9072e932503c 24
fanny868 0:9072e932503c 25 #define vmax 1
fanny868 0:9072e932503c 26 #define vmaxserong 0.9
fanny868 0:9072e932503c 27 #define vmaxpivot 0.7
rahmadirizki18 4:483c07cc22e1 28 #define vmaxanalog 0.9
fanny868 0:9072e932503c 29 #define ax 0.005
fanny868 0:9072e932503c 30 //#define koefperlambatan 0.8
rahmadirizki18 5:3aa203218306 31 #include "encoderKRAI.h"
rahmadirizki18 5:3aa203218306 32
rahmadirizki18 5:3aa203218306 33 #define pi 22/7
rahmadirizki18 5:3aa203218306 34 #define diaEncoder 0.058
rahmadirizki18 5:3aa203218306 35 #define PPR 1000
rahmadirizki18 5:3aa203218306 36 #define diaRobot 0.64
rahmadirizki18 5:3aa203218306 37
rahmadirizki18 5:3aa203218306 38 float K_enc = pi*diaEncoder;
rahmadirizki18 5:3aa203218306 39 float K_robot = pi*diaRobot;
rahmadirizki18 5:3aa203218306 40
rahmadirizki18 5:3aa203218306 41 float speed1=0.6;
rahmadirizki18 5:3aa203218306 42 float speed2=0.6;
rahmadirizki18 5:3aa203218306 43 float speed3=0.6;
rahmadirizki18 5:3aa203218306 44 float speed4=0.6;
rahmadirizki18 5:3aa203218306 45
rahmadirizki18 5:3aa203218306 46 float KpX=0.5, KpY=0.5, Kp_tetha=0.03;
rahmadirizki18 5:3aa203218306 47 //float jarakGlobalY;
rahmadirizki18 5:3aa203218306 48
rahmadirizki18 5:3aa203218306 49 // Deklarasi variabel PID
rahmadirizki18 5:3aa203218306 50 //PID PID(0.992,0.000,0.81,0.001); //(P,I,D, time sampling)
rahmadirizki18 5:3aa203218306 51
rahmadirizki18 5:3aa203218306 52 // Deklarasi variabel encoder
rahmadirizki18 5:3aa203218306 53 encoderKRAI encoderDepan( D3, D4, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
rahmadirizki18 5:3aa203218306 54 encoderKRAI encoderBelakang( PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING);
rahmadirizki18 5:3aa203218306 55 encoderKRAI encoderKanan( PD_2, PC_12, 720, encoderKRAI::X2_ENCODING);
rahmadirizki18 5:3aa203218306 56 encoderKRAI encoderKiri( PC_11, PC_10, 2000, encoderKRAI::X2_ENCODING);
fanny868 0:9072e932503c 57
fanny868 0:9072e932503c 58 // Deklarasi variabel motor
rahmadirizki18 5:3aa203218306 59 Motor motor1(PB_8, PB_1 , PA_13); // pwm, fwd, rev
rahmadirizki18 5:3aa203218306 60 Motor motor2(PB_9, PA_12, PC_5); // pwm, fwd, rev
rahmadirizki18 5:3aa203218306 61 Motor motor3(PB_6, PA_7 , PB_12); // pwm, fwd, rev
rahmadirizki18 5:3aa203218306 62 Motor motor4(PB_7, PA_14 ,PA_15); // pwm, fwd, rev
rahmadirizki18 5:3aa203218306 63
rahmadirizki18 5:3aa203218306 64 // Deklarasi variabel posisi robot
rahmadirizki18 5:3aa203218306 65 //robotPos posisi();
rahmadirizki18 5:3aa203218306 66
rahmadirizki18 5:3aa203218306 67 // Inisialisasi Pin TX-RX Joystik dan PC
rahmadirizki18 5:3aa203218306 68 //Serial pc(PA_0,PA_1);
rahmadirizki18 5:3aa203218306 69 //Serial pc(USBTX,USBRX);
rahmadirizki18 5:3aa203218306 70 // Deklarasi Variabel Global
rahmadirizki18 5:3aa203218306 71 /*
rahmadirizki18 5:3aa203218306 72 * posX dan posY berdasarkan arah robot
rahmadirizki18 5:3aa203218306 73 * encoder Depan & Belakang sejajar sumbu Y
rahmadirizki18 5:3aa203218306 74 * encoder Kanan & Kiri sejajar sumbu X
rahmadirizki18 5:3aa203218306 75 */
rahmadirizki18 5:3aa203218306 76 float jarak, posX, posY;
rahmadirizki18 5:3aa203218306 77 //float keliling = pi*diameterRoda;
rahmadirizki18 5:3aa203218306 78
rahmadirizki18 5:3aa203218306 79 void detect_encoder(float speed);
rahmadirizki18 5:3aa203218306 80 void setCenter();
rahmadirizki18 5:3aa203218306 81 float getY();
rahmadirizki18 5:3aa203218306 82 float getX();
rahmadirizki18 5:3aa203218306 83 float getTetha();
rahmadirizki18 5:3aa203218306 84
rahmadirizki18 5:3aa203218306 85
rahmadirizki18 5:3aa203218306 86
fanny868 0:9072e932503c 87
fanny868 0:9072e932503c 88 // Inisialisasi Pin TX-RX Joystik dan PC
rahmadirizki18 3:1287fccc11be 89 joysticknucleo joystick(PA_0,PA_1);
rahmadirizki18 3:1287fccc11be 90 Serial pc(USBTX,USBRX);
fanny868 0:9072e932503c 91
rahmadirizki18 5:3aa203218306 92 // Posisi target
rahmadirizki18 5:3aa203218306 93 float XT, YT, Tetha;
rahmadirizki18 5:3aa203218306 94
rahmadirizki18 5:3aa203218306 95 //encoder variable
rahmadirizki18 5:3aa203218306 96 float errX, errY, errT, Vt, Vx, Vy;
rahmadirizki18 5:3aa203218306 97 float V1, V2, V3, V4;
rahmadirizki18 5:3aa203218306 98
fanny868 0:9072e932503c 99 //bool perlambatan=0;
fanny868 0:9072e932503c 100 char case_ger;
rahmadirizki18 3:1287fccc11be 101 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;
fanny868 0:9072e932503c 102 bool stop = true;
rahmadirizki18 4:483c07cc22e1 103 float jLX,jLY;
rahmadirizki18 5:3aa203218306 104 double vcurr;
rahmadirizki18 4:483c07cc22e1 105 float x,y; // untuk analoghat kiri
rahmadirizki18 3:1287fccc11be 106 float errorx=0,errory=0;
rahmadirizki18 3:1287fccc11be 107
rahmadirizki18 4:483c07cc22e1 108 // Fungsi mapping 0-255 ke -128 sampai 127
rahmadirizki18 4:483c07cc22e1 109 float mapping(float a,float error)
rahmadirizki18 3:1287fccc11be 110 {
rahmadirizki18 4:483c07cc22e1 111 float hasil,b;
rahmadirizki18 4:483c07cc22e1 112 b = (float)((a-128)/128);
rahmadirizki18 4:483c07cc22e1 113 if (b>(error - 0.2) && b<(error + 0.2))
rahmadirizki18 4:483c07cc22e1 114 {
rahmadirizki18 4:483c07cc22e1 115 hasil = 0;
rahmadirizki18 4:483c07cc22e1 116 } else {
rahmadirizki18 4:483c07cc22e1 117 hasil = b;
rahmadirizki18 4:483c07cc22e1 118 }
rahmadirizki18 3:1287fccc11be 119 return (hasil);
rahmadirizki18 3:1287fccc11be 120 }
fanny868 0:9072e932503c 121
rahmadirizki18 4:483c07cc22e1 122 // Kalibrasi tombol analog kiri
rahmadirizki18 4:483c07cc22e1 123 void kalibrasi()
rahmadirizki18 4:483c07cc22e1 124 {
rahmadirizki18 4:483c07cc22e1 125 errorx = (jLX - 128)/128;
rahmadirizki18 4:483c07cc22e1 126 errory = (jLY - 128)/128;
rahmadirizki18 4:483c07cc22e1 127
rahmadirizki18 4:483c07cc22e1 128 }
rahmadirizki18 4:483c07cc22e1 129
rahmadirizki18 3:1287fccc11be 130 // simpan data analog
rahmadirizki18 3:1287fccc11be 131 void baca_analog()
rahmadirizki18 3:1287fccc11be 132 {
rahmadirizki18 3:1287fccc11be 133 jLX = joystick.LX;
rahmadirizki18 3:1287fccc11be 134 jLY = joystick.LY;
rahmadirizki18 3:1287fccc11be 135
rahmadirizki18 3:1287fccc11be 136 // Pembacaan nilai Y terbalik
rahmadirizki18 4:483c07cc22e1 137 x = mapping(jLX,errorx);
rahmadirizki18 4:483c07cc22e1 138 y = -mapping(jLY,errory);
rahmadirizki18 3:1287fccc11be 139 }
fanny868 0:9072e932503c 140
rahmadirizki18 3:1287fccc11be 141 // Gerak dari Motor base
fanny868 0:9072e932503c 142 int case_gerak()
fanny868 0:9072e932503c 143 {
fanny868 0:9072e932503c 144 int casegerak;
rahmadirizki18 3:1287fccc11be 145 baca_analog();
rahmadirizki18 3:1287fccc11be 146 if (!joystick.L1 && joystick.R1) {
fanny868 0:9072e932503c 147 // Pivot Kanan
fanny868 0:9072e932503c 148 casegerak = 1;
fanny868 0:9072e932503c 149 } else if (!joystick.R1 && joystick.L1) {
fanny868 0:9072e932503c 150 // Pivot Kiri
fanny868 0:9072e932503c 151 casegerak = 2;
rahmadirizki18 3:1287fccc11be 152 } else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
fanny868 0:9072e932503c 153 // Maju
rahmadirizki18 3:1287fccc11be 154 casegerak = 3;
rahmadirizki18 3:1287fccc11be 155 } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
fanny868 0:9072e932503c 156 // Mundur
fanny868 0:9072e932503c 157 casegerak = 4;
rahmadirizki18 3:1287fccc11be 158 } else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan)) {
fanny868 0:9072e932503c 159 // Serong Atas Kanan
fanny868 0:9072e932503c 160 casegerak = 5;
rahmadirizki18 3:1287fccc11be 161 } else if((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan)) {
fanny868 0:9072e932503c 162 // Serong Bawah Kanan
fanny868 0:9072e932503c 163 casegerak = 6;
rahmadirizki18 3:1287fccc11be 164 } else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)) {
fanny868 0:9072e932503c 165 // Serong Atas Kiri
fanny868 0:9072e932503c 166 casegerak = 7;
rahmadirizki18 3:1287fccc11be 167 } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)) {
fanny868 0:9072e932503c 168 // Serong Bawah Kiri
fanny868 0:9072e932503c 169 casegerak = 8;
rahmadirizki18 3:1287fccc11be 170 } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
fanny868 0:9072e932503c 171 // Kanan
fanny868 0:9072e932503c 172 casegerak = 9;
rahmadirizki18 3:1287fccc11be 173 } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
fanny868 0:9072e932503c 174 // Kiri
rahmadirizki18 3:1287fccc11be 175 casegerak = 10;
rahmadirizki18 3:1287fccc11be 176 } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
rahmadirizki18 3:1287fccc11be 177 // case gerak analog on off
rahmadirizki18 3:1287fccc11be 178 if (analog){
rahmadirizki18 3:1287fccc11be 179 casegerak = 11;
rahmadirizki18 3:1287fccc11be 180 } else {
rahmadirizki18 3:1287fccc11be 181 casegerak = 12;
rahmadirizki18 3:1287fccc11be 182 }
rahmadirizki18 3:1287fccc11be 183 }
fanny868 0:9072e932503c 184 return(casegerak);
fanny868 0:9072e932503c 185 }
fanny868 0:9072e932503c 186
fanny868 0:9072e932503c 187
fanny868 0:9072e932503c 188
fanny868 0:9072e932503c 189 /**
fanny868 0:9072e932503c 190
rahmadirizki18 3:1287fccc11be 191 ** Case 1 : Pivot kanan
fanny868 0:9072e932503c 192 ** Case 2 : Pivot Kiri
fanny868 0:9072e932503c 193 ** Case 3 : Maju
fanny868 0:9072e932503c 194 ** Case 4 : Mundur
fanny868 0:9072e932503c 195 ** Case 5 : Serong Atas Kanan
fanny868 0:9072e932503c 196 ** Case 6 : Serong Bawah Kanan
fanny868 0:9072e932503c 197 ** Case 7 : Serong Atas Kiri
fanny868 0:9072e932503c 198 ** Case 8 : Serong Bawah Kiri
fanny868 0:9072e932503c 199 ** Case 9 : Kanan
fanny868 0:9072e932503c 200 ** Case 10 : Kiri
rahmadirizki18 3:1287fccc11be 201 ** Case 11 : Analog
rahmadirizki18 3:1287fccc11be 202 ** Case 11 : break
fanny868 0:9072e932503c 203
fanny868 0:9072e932503c 204 **/
rahmadirizki18 3:1287fccc11be 205
rahmadirizki18 5:3aa203218306 206
rahmadirizki18 3:1287fccc11be 207 // Pergerakan dari base
fanny868 0:9072e932503c 208 void aktuator()
fanny868 0:9072e932503c 209 {
fanny868 0:9072e932503c 210
fanny868 0:9072e932503c 211 // MOTOR
fanny868 0:9072e932503c 212 switch (case_ger)
fanny868 0:9072e932503c 213 {
rahmadirizki18 3:1287fccc11be 214 case (1):
fanny868 0:9072e932503c 215 {
rahmadirizki18 5:3aa203218306 216 Tetha = Tetha - 0.5;
fanny868 0:9072e932503c 217 pivka=true;
rahmadirizki18 3:1287fccc11be 218 maju=mundur=analog=kiri=kanan=saka=saki=sbka=sbki=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 219
rahmadirizki18 5:3aa203218306 220 pc.printf("pivKa Xt =%.2f x=%.2f YT=%.2f y=%.2f errx=%.2f erry=%.2f \n",XT,x,YT,y,errX,errY);
fanny868 0:9072e932503c 221
rahmadirizki18 5:3aa203218306 222
fanny868 0:9072e932503c 223 break;
fanny868 0:9072e932503c 224 }
fanny868 0:9072e932503c 225 case (2):
fanny868 0:9072e932503c 226 {
rahmadirizki18 5:3aa203218306 227 Tetha = Tetha + 0.5;
fanny868 0:9072e932503c 228
fanny868 0:9072e932503c 229 pivki=true;
rahmadirizki18 3:1287fccc11be 230 maju=mundur=kiri=analog=kanan=saka=saki=sbka=sbki=pivka=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 231
rahmadirizki18 5:3aa203218306 232 pc.printf("pivKi Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
fanny868 0:9072e932503c 233
rahmadirizki18 5:3aa203218306 234
fanny868 0:9072e932503c 235 break;
fanny868 0:9072e932503c 236 }
fanny868 0:9072e932503c 237 case (3):
fanny868 0:9072e932503c 238 {
rahmadirizki18 5:3aa203218306 239 YT = YT + 0.01;
fanny868 0:9072e932503c 240
fanny868 0:9072e932503c 241 maju=true;
rahmadirizki18 3:1287fccc11be 242 mundur=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 243
rahmadirizki18 5:3aa203218306 244 pc.printf("maju Xt =%.2f x=%.2f YT=%.2f y=%.2f errx=%.2f erry=%.2f \n",XT,x,YT,y,errX,errY);
fanny868 0:9072e932503c 245
fanny868 0:9072e932503c 246 break;
fanny868 0:9072e932503c 247 }
fanny868 0:9072e932503c 248 case (4):
fanny868 0:9072e932503c 249 {
rahmadirizki18 5:3aa203218306 250 YT = YT - 0.01;
fanny868 0:9072e932503c 251
fanny868 0:9072e932503c 252 mundur=true;
rahmadirizki18 3:1287fccc11be 253 maju=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 254
rahmadirizki18 5:3aa203218306 255 pc.printf("mundur Xt =%.2f x=%.2f YT=%.2f y=%.2f errx=%.2f erry=%.2f \n",XT,x,YT,y,errX,errY);
fanny868 0:9072e932503c 256
fanny868 0:9072e932503c 257
fanny868 0:9072e932503c 258 break;
fanny868 0:9072e932503c 259 }
fanny868 0:9072e932503c 260 case (5) :
fanny868 0:9072e932503c 261 {
rahmadirizki18 5:3aa203218306 262 XT = XT + 0.01;
rahmadirizki18 5:3aa203218306 263 YT = YT + 0.01;
fanny868 0:9072e932503c 264
fanny868 0:9072e932503c 265 saka=true;
rahmadirizki18 3:1287fccc11be 266 maju=mundur=kiri=kanan=sbka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 267
rahmadirizki18 5:3aa203218306 268 pc.printf("saka Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
fanny868 0:9072e932503c 269
fanny868 0:9072e932503c 270
fanny868 0:9072e932503c 271 break;
fanny868 0:9072e932503c 272 }
fanny868 0:9072e932503c 273 case (6) :
fanny868 0:9072e932503c 274 {
rahmadirizki18 5:3aa203218306 275 XT = XT + 0.01;
rahmadirizki18 5:3aa203218306 276 YT = YT - 0.01;
rahmadirizki18 5:3aa203218306 277
fanny868 0:9072e932503c 278
fanny868 0:9072e932503c 279 sbka=true;
rahmadirizki18 3:1287fccc11be 280 maju=mundur=kiri=kanan=saka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 281
rahmadirizki18 5:3aa203218306 282 pc.printf("sbka Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
fanny868 0:9072e932503c 283
fanny868 0:9072e932503c 284
fanny868 0:9072e932503c 285 break;
fanny868 0:9072e932503c 286 }
fanny868 0:9072e932503c 287 case (7) :
fanny868 0:9072e932503c 288 {
rahmadirizki18 5:3aa203218306 289 XT = XT - 0.01;
rahmadirizki18 5:3aa203218306 290 YT = YT + 0.01;
rahmadirizki18 5:3aa203218306 291
fanny868 0:9072e932503c 292
fanny868 0:9072e932503c 293 saki=true;
rahmadirizki18 3:1287fccc11be 294 maju=kiri=kanan=saka=mundur=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 295
rahmadirizki18 5:3aa203218306 296 pc.printf("saki Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
fanny868 0:9072e932503c 297
fanny868 0:9072e932503c 298
fanny868 0:9072e932503c 299 break;
fanny868 0:9072e932503c 300 }
fanny868 0:9072e932503c 301 case (8) :
fanny868 0:9072e932503c 302 {
rahmadirizki18 5:3aa203218306 303 XT = XT - 0.01;
rahmadirizki18 5:3aa203218306 304 YT = YT - 0.01;
rahmadirizki18 5:3aa203218306 305
rahmadirizki18 5:3aa203218306 306 pc.printf("sbki Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
fanny868 0:9072e932503c 307
fanny868 0:9072e932503c 308
fanny868 0:9072e932503c 309 break;
fanny868 0:9072e932503c 310 }
fanny868 0:9072e932503c 311 case (9) :
fanny868 0:9072e932503c 312 {
rahmadirizki18 5:3aa203218306 313 XT = XT + 0.01;
fanny868 0:9072e932503c 314 kanan=true;
rahmadirizki18 3:1287fccc11be 315 maju=kiri=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 316
rahmadirizki18 5:3aa203218306 317 pc.printf("Kanan Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
fanny868 0:9072e932503c 318
fanny868 0:9072e932503c 319 break;
fanny868 0:9072e932503c 320 }
fanny868 0:9072e932503c 321 case (10) :
fanny868 0:9072e932503c 322 {
rahmadirizki18 5:3aa203218306 323 XT = XT - 0.01;
fanny868 0:9072e932503c 324
fanny868 0:9072e932503c 325 kiri=true;
rahmadirizki18 3:1287fccc11be 326 maju=kanan=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 327
rahmadirizki18 5:3aa203218306 328 pc.printf("Kiri Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
fanny868 0:9072e932503c 329
fanny868 0:9072e932503c 330
fanny868 0:9072e932503c 331 break;
fanny868 0:9072e932503c 332 }
rahmadirizki18 3:1287fccc11be 333 case (11):
rahmadirizki18 3:1287fccc11be 334 {
rahmadirizki18 4:483c07cc22e1 335
rahmadirizki18 5:3aa203218306 336 XT = XT + 0.01*x;
rahmadirizki18 5:3aa203218306 337 YT = YT + 0.01*y;
rahmadirizki18 3:1287fccc11be 338
rahmadirizki18 3:1287fccc11be 339 analog=true;
rahmadirizki18 3:1287fccc11be 340 maju=mundur=kiri=kanan=saka=saki=sbka=sbki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
rahmadirizki18 3:1287fccc11be 341
rahmadirizki18 5:3aa203218306 342 pc.printf("analog Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
rahmadirizki18 3:1287fccc11be 343
rahmadirizki18 3:1287fccc11be 344 break;
rahmadirizki18 3:1287fccc11be 345 }
rahmadirizki18 3:1287fccc11be 346 default :
fanny868 0:9072e932503c 347 {
rahmadirizki18 5:3aa203218306 348
rahmadirizki18 3:1287fccc11be 349 maju=mundur=kiri=kanan=saka=saki=sbka=sbki=analog=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 350 stop = true;
fanny868 0:9072e932503c 351
fanny868 0:9072e932503c 352 //s1 = 0;s2 =0; s3 =0; s4 =0;
fanny868 0:9072e932503c 353
rahmadirizki18 5:3aa203218306 354 // pc.printf("Stop V1=%.2f V2=%.2f V3=%.2f V4=%.2f\n",V1,V2,V3,V4);
rahmadirizki18 3:1287fccc11be 355 }
rahmadirizki18 5:3aa203218306 356 }
rahmadirizki18 5:3aa203218306 357 }
rahmadirizki18 5:3aa203218306 358
rahmadirizki18 5:3aa203218306 359 //Begin Encoder
rahmadirizki18 5:3aa203218306 360
rahmadirizki18 5:3aa203218306 361 void setCenter()
rahmadirizki18 5:3aa203218306 362 {
rahmadirizki18 5:3aa203218306 363 encoderDepan.reset();
rahmadirizki18 5:3aa203218306 364 encoderBelakang.reset();
rahmadirizki18 5:3aa203218306 365 encoderKanan.reset();
rahmadirizki18 5:3aa203218306 366 encoderKiri.reset();
rahmadirizki18 5:3aa203218306 367 }
rahmadirizki18 5:3aa203218306 368
rahmadirizki18 5:3aa203218306 369 float getX()
rahmadirizki18 5:3aa203218306 370 {
rahmadirizki18 5:3aa203218306 371 float jarakEncDpn, jarakEncBlk;
rahmadirizki18 5:3aa203218306 372 jarakEncDpn = (encoderDepan.getPulses())/(float)(2000.0)*K_enc;
rahmadirizki18 5:3aa203218306 373 jarakEncBlk = (encoderBelakang.getPulses())/(float)(2000.0)*K_enc;
rahmadirizki18 5:3aa203218306 374 return (jarakEncDpn-jarakEncBlk)/2;
rahmadirizki18 5:3aa203218306 375 }
rahmadirizki18 5:3aa203218306 376
rahmadirizki18 5:3aa203218306 377 float getY()
rahmadirizki18 5:3aa203218306 378 {
rahmadirizki18 5:3aa203218306 379 float jarakEncKir, jarakEncKan;
rahmadirizki18 5:3aa203218306 380 jarakEncKir = (encoderKiri.getPulses())/(float)(2000.0)*K_enc;
rahmadirizki18 5:3aa203218306 381 jarakEncKan = (encoderKanan.getPulses())/(float)(720.0)*K_enc;
rahmadirizki18 5:3aa203218306 382 return (jarakEncKir-jarakEncKan)/2;
rahmadirizki18 5:3aa203218306 383 }
rahmadirizki18 5:3aa203218306 384
rahmadirizki18 5:3aa203218306 385 float getTetha()
rahmadirizki18 5:3aa203218306 386 {
rahmadirizki18 5:3aa203218306 387 float busurDpn, busurBlk, busurKir, busurKan;
rahmadirizki18 5:3aa203218306 388 busurDpn = ((encoderDepan.getPulses())/(float)(2000.0)*K_enc)/K_robot*360.0;
rahmadirizki18 5:3aa203218306 389 busurBlk = ((encoderBelakang.getPulses())/(float)(2000.0)*K_enc)/K_robot*360.0;
rahmadirizki18 5:3aa203218306 390 busurKir = ((encoderKiri.getPulses())/(float)(2000.0)*K_enc)/K_robot*360.0;
rahmadirizki18 5:3aa203218306 391 busurKan = ((encoderKanan.getPulses())/(float)(720.0)*K_enc)/K_robot*360.0;
rahmadirizki18 5:3aa203218306 392
rahmadirizki18 5:3aa203218306 393 return -(busurDpn+busurBlk+busurKir+busurKan)/4;
fanny868 0:9072e932503c 394 }
fanny868 0:9072e932503c 395
rahmadirizki18 5:3aa203218306 396 void gotoXYT(float xa, float ya, float Ta)
rahmadirizki18 5:3aa203218306 397 {
rahmadirizki18 5:3aa203218306 398
rahmadirizki18 5:3aa203218306 399 errX = xa-getX();
rahmadirizki18 5:3aa203218306 400 Vx = KpX*errX;
rahmadirizki18 5:3aa203218306 401
rahmadirizki18 5:3aa203218306 402 errY = ya-getY();
rahmadirizki18 5:3aa203218306 403 Vy = KpY*errY;
rahmadirizki18 5:3aa203218306 404
rahmadirizki18 5:3aa203218306 405 errT = Ta-getTetha();
rahmadirizki18 5:3aa203218306 406 Vt = Kp_tetha*errT;
rahmadirizki18 5:3aa203218306 407
rahmadirizki18 5:3aa203218306 408 V1 = Vx+Vy-Vt;
rahmadirizki18 5:3aa203218306 409 V2 = Vx-Vy-Vt;
rahmadirizki18 5:3aa203218306 410 V3 = -Vx-Vy-Vt;
rahmadirizki18 5:3aa203218306 411 V4 = -Vx+Vy-Vt;
rahmadirizki18 5:3aa203218306 412
rahmadirizki18 5:3aa203218306 413 if (V1>speed1)
rahmadirizki18 5:3aa203218306 414 { V1 = speed1; }
rahmadirizki18 5:3aa203218306 415 else if (V1<-speed1)
rahmadirizki18 5:3aa203218306 416 { V1 = -speed1; }
rahmadirizki18 5:3aa203218306 417
rahmadirizki18 5:3aa203218306 418 if (V2>speed2)
rahmadirizki18 5:3aa203218306 419 { V2 = speed2; }
rahmadirizki18 5:3aa203218306 420 else if (V2<-speed2)
rahmadirizki18 5:3aa203218306 421 { V2 = -speed2; }
rahmadirizki18 5:3aa203218306 422
rahmadirizki18 5:3aa203218306 423 if (V3>speed3)
rahmadirizki18 5:3aa203218306 424 { V3 = speed3; }
rahmadirizki18 5:3aa203218306 425 else if (V3<-speed3)
rahmadirizki18 5:3aa203218306 426 { V3 = -speed3; }
rahmadirizki18 5:3aa203218306 427
rahmadirizki18 5:3aa203218306 428 if (V4>speed4)
rahmadirizki18 5:3aa203218306 429 { V4 = speed4; }
rahmadirizki18 5:3aa203218306 430 else if (V4<-speed4)
rahmadirizki18 5:3aa203218306 431 { V4 = -speed4; }
rahmadirizki18 5:3aa203218306 432
rahmadirizki18 5:3aa203218306 433 if (((errX > 0.05) || (errX<-0.05)) || ((errY > 0.05) || (errY<-0.05)) || ((errT > 0.05) || (errT<-0.05)))
rahmadirizki18 5:3aa203218306 434 {
rahmadirizki18 5:3aa203218306 435 motor1.speed(V1);
rahmadirizki18 5:3aa203218306 436 motor2.speed(V2);
rahmadirizki18 5:3aa203218306 437 motor3.speed(V3);
rahmadirizki18 5:3aa203218306 438 motor4.speed(V4);
rahmadirizki18 5:3aa203218306 439 // pc.printf("V1=%.2f \ ", V1);
rahmadirizki18 5:3aa203218306 440 }
rahmadirizki18 5:3aa203218306 441 else
rahmadirizki18 5:3aa203218306 442 {
rahmadirizki18 5:3aa203218306 443 motor1.brake(1);
rahmadirizki18 5:3aa203218306 444 motor2.brake(1);
rahmadirizki18 5:3aa203218306 445 motor3.brake(1);
rahmadirizki18 5:3aa203218306 446 motor4.brake(1);
rahmadirizki18 5:3aa203218306 447 //_ms(1000);
rahmadirizki18 5:3aa203218306 448 }
rahmadirizki18 5:3aa203218306 449 //pc.printf("%f\t%f\t%f ", errX*100,errY*100,errT);
rahmadirizki18 5:3aa203218306 450 // wait_ms(10);
rahmadirizki18 5:3aa203218306 451
rahmadirizki18 5:3aa203218306 452 }
rahmadirizki18 5:3aa203218306 453 //End Encoder
rahmadirizki18 5:3aa203218306 454
fanny868 0:9072e932503c 455
fanny868 0:9072e932503c 456 int main (void)
fanny868 0:9072e932503c 457 {
fanny868 0:9072e932503c 458 // Set baud rate - 115200
fanny868 0:9072e932503c 459 joystick.setup();
rahmadirizki18 3:1287fccc11be 460 pc.baud(115200);
rahmadirizki18 5:3aa203218306 461 wait_ms(1000);
rahmadirizki18 5:3aa203218306 462 setCenter();
rahmadirizki18 5:3aa203218306 463 wait_ms(500);
rahmadirizki18 5:3aa203218306 464
rahmadirizki18 5:3aa203218306 465 //Posisi Awal
rahmadirizki18 5:3aa203218306 466 XT = 0;
rahmadirizki18 5:3aa203218306 467 YT = 0;
rahmadirizki18 5:3aa203218306 468 Tetha = 0;
rahmadirizki18 3:1287fccc11be 469 pc.printf("Ready...\n");
rahmadirizki18 4:483c07cc22e1 470 kalibrasi();
rahmadirizki18 3:1287fccc11be 471
fanny868 0:9072e932503c 472 while(1)
fanny868 0:9072e932503c 473 {
fanny868 0:9072e932503c 474 // Interrupt Serial
fanny868 0:9072e932503c 475 joystick.idle();
fanny868 0:9072e932503c 476 if(joystick.readable() ) {
fanny868 0:9072e932503c 477 // Panggil fungsi pembacaan joystik
fanny868 0:9072e932503c 478 joystick.baca_data();
fanny868 0:9072e932503c 479 // Panggil fungsi pengolahan data joystik
fanny868 0:9072e932503c 480 joystick.olah_data();
fanny868 0:9072e932503c 481 //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);
fanny868 0:9072e932503c 482 case_ger = case_gerak();
rahmadirizki18 3:1287fccc11be 483 aktuator();
rahmadirizki18 3:1287fccc11be 484 //kalibrasi
rahmadirizki18 4:483c07cc22e1 485 if (joystick.START){
rahmadirizki18 4:483c07cc22e1 486 kalibrasi();}
rahmadirizki18 4:483c07cc22e1 487 // analog switch mode
rahmadirizki18 4:483c07cc22e1 488 if (joystick.SELECT_click) {analog=!analog;}
rahmadirizki18 4:483c07cc22e1 489 //pc.printf(" X =%.2f Y =%.2f \n ",x,y);
rahmadirizki18 5:3aa203218306 490 if (joystick.silang) {
rahmadirizki18 5:3aa203218306 491 XT = 0;
rahmadirizki18 5:3aa203218306 492 YT = 0;
rahmadirizki18 5:3aa203218306 493 Tetha = 0;
rahmadirizki18 5:3aa203218306 494 pc.printf("x..\n");
rahmadirizki18 5:3aa203218306 495 }
rahmadirizki18 5:3aa203218306 496
rahmadirizki18 5:3aa203218306 497
fanny868 0:9072e932503c 498 } else {
fanny868 0:9072e932503c 499 joystick.idle();
fanny868 0:9072e932503c 500
fanny868 0:9072e932503c 501 }
rahmadirizki18 5:3aa203218306 502 gotoXYT(XT,YT,Tetha);
rahmadirizki18 5:3aa203218306 503
fanny868 0:9072e932503c 504 }
fanny868 0:9072e932503c 505 }