KRAI 2017 / Mbed 2 deprecated DagonFly__RoadToJapan_17Mei

dear all

Dependencies:   DigitDisplay Motor PID Ping mbed millis

Fork of DagonFly__RoadToJapan_15Mei_A by KRAI 2017

main.cpp@13:8ab42383a2ca, 2016-12-09 (annotated)

Committer:
rahmadirizki18
Date:
Fri Dec 09 10:27:56 2016 +0000
Revision:
13:8ab42383a2ca
Parent:
12:e07c59c28c29
Child:
14:6d389e99981c
tambahan fungsi r2 click dan l2 click

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 /****************************************************************************/
rahmadirizki18 6:68293bed71ea 20 /* */
rahmadirizki18 6:68293bed71ea 21 /* Joystick */
rahmadirizki18 6:68293bed71ea 22 /* kanan => posisi target x ditambah 0.01 */
rahmadirizki18 6:68293bed71ea 23 /* kiri => posisi target x dikurang 0.01 */
rahmadirizki18 6:68293bed71ea 24 /* atas => posisi target y ditambah 0.01 */
rahmadirizki18 6:68293bed71ea 25 /* bawah => posisi target y dikurang 0.01 */
rahmadirizki18 6:68293bed71ea 26 /* */
rahmadirizki18 6:68293bed71ea 27 /* Tombol silang => Kembali keposisi Awal */
rahmadirizki18 6:68293bed71ea 28 /* Tombol segitiga => Aktif motor Launcher */
rahmadirizki18 13:8ab42383a2ca 29 /* Tombol lingkaran=> Aktif servo Launcher */
rahmadirizki18 13:8ab42383a2ca 30 /* Tombol L3 => PWM Launcher dikurangin */
rahmadirizki18 13:8ab42383a2ca 31 /* Tombol R3 => PWM Launcher ditambahin */
rahmadirizki18 13:8ab42383a2ca 32 /* */
rahmadirizki18 13:8ab42383a2ca 33 /* */
rahmadirizki18 6:68293bed71ea 34 /* */
rahmadirizki18 6:68293bed71ea 35 /****************************************************************************/
rahmadirizki18 6:68293bed71ea 36
fanny868 0:9072e932503c 37
fanny868 0:9072e932503c 38 #include "mbed.h"
fanny868 0:9072e932503c 39 #include "JoystickPS3.h"
fanny868 0:9072e932503c 40 #include "Motor.h"
rahmadirizki18 6:68293bed71ea 41 #include "Servo.h"
fanny868 0:9072e932503c 42
fanny868 0:9072e932503c 43 //#define koefperlambatan 0.8
rahmadirizki18 5:3aa203218306 44 #include "encoderKRAI.h"
rahmadirizki18 5:3aa203218306 45
rahmadirizki18 5:3aa203218306 46 #define pi 22/7
rahmadirizki18 5:3aa203218306 47 #define diaEncoder 0.058
rahmadirizki18 5:3aa203218306 48 #define PPR 1000
rahmadirizki18 5:3aa203218306 49 #define diaRobot 0.64
Joshua23 9:5a50782510fb 50 #define pinservo1 PC_8
rahmadirizki18 7:d138c56dab20 51 //PC 9
Joshua23 9:5a50782510fb 52 #define pinservo2 PC_9
rahmadirizki18 5:3aa203218306 53
rahmadirizki18 5:3aa203218306 54 float K_enc = pi*diaEncoder;
rahmadirizki18 5:3aa203218306 55 float K_robot = pi*diaRobot;
rahmadirizki18 5:3aa203218306 56
rahmadirizki18 5:3aa203218306 57 float speed1=0.6;
rahmadirizki18 5:3aa203218306 58 float speed2=0.6;
rahmadirizki18 5:3aa203218306 59 float speed3=0.6;
rahmadirizki18 5:3aa203218306 60 float speed4=0.6;
franshendri 12:e07c59c28c29 61 float speedB=0.43 ;
franshendri 12:e07c59c28c29 62 float speedL=0.4;
rahmadirizki18 5:3aa203218306 63
rahmadirizki18 5:3aa203218306 64 float KpX=0.5, KpY=0.5, Kp_tetha=0.03;
rahmadirizki18 6:68293bed71ea 65
rahmadirizki18 6:68293bed71ea 66 Timer waktu;
rahmadirizki18 5:3aa203218306 67 //float jarakGlobalY;
rahmadirizki18 5:3aa203218306 68
rahmadirizki18 5:3aa203218306 69 // Deklarasi variabel PID
rahmadirizki18 5:3aa203218306 70 //PID PID(0.992,0.000,0.81,0.001); //(P,I,D, time sampling)
rahmadirizki18 5:3aa203218306 71
rahmadirizki18 5:3aa203218306 72 // Deklarasi variabel encoder
rahmadirizki18 6:68293bed71ea 73 encoderKRAI encoderDepan( PB_14, PB_13, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
rahmadirizki18 5:3aa203218306 74 encoderKRAI encoderBelakang( PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING);
rahmadirizki18 5:3aa203218306 75 encoderKRAI encoderKanan( PD_2, PC_12, 720, encoderKRAI::X2_ENCODING);
rahmadirizki18 5:3aa203218306 76 encoderKRAI encoderKiri( PC_11, PC_10, 2000, encoderKRAI::X2_ENCODING);
fanny868 0:9072e932503c 77
fanny868 0:9072e932503c 78 // Deklarasi variabel motor
franshendri 10:f0f0dc3904e0 79 Motor motor1(PA_8, PB_0 , PC_15); // pwm, fwd, rev
franshendri 10:f0f0dc3904e0 80 Motor motor2(PA_11, PA_6 ,PA_5); // pwm, fwd, rev
franshendri 10:f0f0dc3904e0 81 Motor motor3(PA_9, PC_2 , PC_3); // pwm, fwd, rev
franshendri 10:f0f0dc3904e0 82 Motor motor4(PA_10, PB_5 ,PB_4); // pwm, fwd, rev
rahmadirizki18 5:3aa203218306 83
rahmadirizki18 6:68293bed71ea 84 //Motor Atas
franshendri 10:f0f0dc3904e0 85 Motor motorld(PB_8, PB_1 , PA_13); // pwm, fwd, rev
franshendri 10:f0f0dc3904e0 86 Motor motorlb(PB_9, PA_12, PC_5 ); // pwm, fwd, rev
rahmadirizki18 6:68293bed71ea 87
rahmadirizki18 6:68293bed71ea 88 //Servo Atas
franshendri 10:f0f0dc3904e0 89 Servo servoS(PC_9);
franshendri 10:f0f0dc3904e0 90 Servo servoB(PC_7);
rahmadirizki18 6:68293bed71ea 91
rahmadirizki18 5:3aa203218306 92 // Deklarasi variabel posisi robot
rahmadirizki18 5:3aa203218306 93 //robotPos posisi();
rahmadirizki18 5:3aa203218306 94
rahmadirizki18 5:3aa203218306 95 // Inisialisasi Pin TX-RX Joystik dan PC
rahmadirizki18 5:3aa203218306 96 //Serial pc(PA_0,PA_1);
rahmadirizki18 5:3aa203218306 97 //Serial pc(USBTX,USBRX);
rahmadirizki18 5:3aa203218306 98 // Deklarasi Variabel Global
rahmadirizki18 5:3aa203218306 99 /*
rahmadirizki18 5:3aa203218306 100 * posX dan posY berdasarkan arah robot
rahmadirizki18 5:3aa203218306 101 * encoder Depan & Belakang sejajar sumbu Y
rahmadirizki18 5:3aa203218306 102 * encoder Kanan & Kiri sejajar sumbu X
rahmadirizki18 5:3aa203218306 103 */
rahmadirizki18 5:3aa203218306 104 float jarak, posX, posY;
rahmadirizki18 5:3aa203218306 105 //float keliling = pi*diameterRoda;
rahmadirizki18 5:3aa203218306 106
rahmadirizki18 5:3aa203218306 107 void detect_encoder(float speed);
rahmadirizki18 5:3aa203218306 108 void setCenter();
rahmadirizki18 5:3aa203218306 109 float getY();
rahmadirizki18 5:3aa203218306 110 float getX();
rahmadirizki18 5:3aa203218306 111 float getTetha();
rahmadirizki18 5:3aa203218306 112
fanny868 0:9072e932503c 113 // Inisialisasi Pin TX-RX Joystik dan PC
rahmadirizki18 3:1287fccc11be 114 joysticknucleo joystick(PA_0,PA_1);
rahmadirizki18 3:1287fccc11be 115 Serial pc(USBTX,USBRX);
fanny868 0:9072e932503c 116
rahmadirizki18 5:3aa203218306 117 // Posisi target
rahmadirizki18 5:3aa203218306 118 float XT, YT, Tetha;
rahmadirizki18 5:3aa203218306 119
rahmadirizki18 5:3aa203218306 120 //encoder variable
rahmadirizki18 5:3aa203218306 121 float errX, errY, errT, Vt, Vx, Vy;
rahmadirizki18 5:3aa203218306 122 float V1, V2, V3, V4;
rahmadirizki18 5:3aa203218306 123
fanny868 0:9072e932503c 124 //bool perlambatan=0;
fanny868 0:9072e932503c 125 char case_ger;
rahmadirizki18 3:1287fccc11be 126 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 127 bool stop = true;
rahmadirizki18 6:68293bed71ea 128 bool Launcher = false,ServoGo = false;
rahmadirizki18 4:483c07cc22e1 129 float jLX,jLY;
rahmadirizki18 5:3aa203218306 130 double vcurr;
rahmadirizki18 4:483c07cc22e1 131 float x,y; // untuk analoghat kiri
rahmadirizki18 3:1287fccc11be 132 float errorx=0,errory=0;
rahmadirizki18 3:1287fccc11be 133
rahmadirizki18 4:483c07cc22e1 134 // Fungsi mapping 0-255 ke -128 sampai 127
rahmadirizki18 4:483c07cc22e1 135 float mapping(float a,float error)
rahmadirizki18 3:1287fccc11be 136 {
rahmadirizki18 4:483c07cc22e1 137 float hasil,b;
rahmadirizki18 4:483c07cc22e1 138 b = (float)((a-128)/128);
rahmadirizki18 4:483c07cc22e1 139 if (b>(error - 0.2) && b<(error + 0.2))
rahmadirizki18 4:483c07cc22e1 140 {
rahmadirizki18 4:483c07cc22e1 141 hasil = 0;
rahmadirizki18 4:483c07cc22e1 142 } else {
rahmadirizki18 4:483c07cc22e1 143 hasil = b;
rahmadirizki18 4:483c07cc22e1 144 }
rahmadirizki18 3:1287fccc11be 145 return (hasil);
rahmadirizki18 3:1287fccc11be 146 }
fanny868 0:9072e932503c 147
rahmadirizki18 4:483c07cc22e1 148 // Kalibrasi tombol analog kiri
rahmadirizki18 4:483c07cc22e1 149 void kalibrasi()
rahmadirizki18 4:483c07cc22e1 150 {
rahmadirizki18 4:483c07cc22e1 151 errorx = (jLX - 128)/128;
rahmadirizki18 4:483c07cc22e1 152 errory = (jLY - 128)/128;
rahmadirizki18 4:483c07cc22e1 153
rahmadirizki18 4:483c07cc22e1 154 }
rahmadirizki18 4:483c07cc22e1 155
rahmadirizki18 3:1287fccc11be 156 // simpan data analog
rahmadirizki18 3:1287fccc11be 157 void baca_analog()
rahmadirizki18 3:1287fccc11be 158 {
rahmadirizki18 3:1287fccc11be 159 jLX = joystick.LX;
rahmadirizki18 3:1287fccc11be 160 jLY = joystick.LY;
rahmadirizki18 3:1287fccc11be 161
rahmadirizki18 3:1287fccc11be 162 // Pembacaan nilai Y terbalik
rahmadirizki18 4:483c07cc22e1 163 x = mapping(jLX,errorx);
rahmadirizki18 4:483c07cc22e1 164 y = -mapping(jLY,errory);
rahmadirizki18 3:1287fccc11be 165 }
fanny868 0:9072e932503c 166
rahmadirizki18 3:1287fccc11be 167 // Gerak dari Motor base
fanny868 0:9072e932503c 168 int case_gerak()
fanny868 0:9072e932503c 169 {
fanny868 0:9072e932503c 170 int casegerak;
rahmadirizki18 3:1287fccc11be 171 baca_analog();
rahmadirizki18 3:1287fccc11be 172 if (!joystick.L1 && joystick.R1) {
fanny868 0:9072e932503c 173 // Pivot Kanan
fanny868 0:9072e932503c 174 casegerak = 1;
fanny868 0:9072e932503c 175 } else if (!joystick.R1 && joystick.L1) {
fanny868 0:9072e932503c 176 // Pivot Kiri
fanny868 0:9072e932503c 177 casegerak = 2;
rahmadirizki18 3:1287fccc11be 178 } else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
fanny868 0:9072e932503c 179 // Maju
rahmadirizki18 3:1287fccc11be 180 casegerak = 3;
rahmadirizki18 3:1287fccc11be 181 } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
fanny868 0:9072e932503c 182 // Mundur
fanny868 0:9072e932503c 183 casegerak = 4;
rahmadirizki18 3:1287fccc11be 184 } else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan)) {
fanny868 0:9072e932503c 185 // Serong Atas Kanan
fanny868 0:9072e932503c 186 casegerak = 5;
rahmadirizki18 3:1287fccc11be 187 } else if((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan)) {
fanny868 0:9072e932503c 188 // Serong Bawah Kanan
fanny868 0:9072e932503c 189 casegerak = 6;
rahmadirizki18 3:1287fccc11be 190 } else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)) {
fanny868 0:9072e932503c 191 // Serong Atas Kiri
fanny868 0:9072e932503c 192 casegerak = 7;
rahmadirizki18 3:1287fccc11be 193 } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)) {
fanny868 0:9072e932503c 194 // Serong Bawah Kiri
fanny868 0:9072e932503c 195 casegerak = 8;
rahmadirizki18 3:1287fccc11be 196 } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
fanny868 0:9072e932503c 197 // Kanan
fanny868 0:9072e932503c 198 casegerak = 9;
rahmadirizki18 3:1287fccc11be 199 } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
fanny868 0:9072e932503c 200 // Kiri
rahmadirizki18 3:1287fccc11be 201 casegerak = 10;
rahmadirizki18 3:1287fccc11be 202 } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
rahmadirizki18 3:1287fccc11be 203 // case gerak analog on off
rahmadirizki18 3:1287fccc11be 204 if (analog){
rahmadirizki18 3:1287fccc11be 205 casegerak = 11;
rahmadirizki18 3:1287fccc11be 206 } else {
rahmadirizki18 3:1287fccc11be 207 casegerak = 12;
rahmadirizki18 3:1287fccc11be 208 }
rahmadirizki18 3:1287fccc11be 209 }
fanny868 0:9072e932503c 210 return(casegerak);
fanny868 0:9072e932503c 211 }
fanny868 0:9072e932503c 212
fanny868 0:9072e932503c 213
fanny868 0:9072e932503c 214
fanny868 0:9072e932503c 215 /**
fanny868 0:9072e932503c 216
rahmadirizki18 3:1287fccc11be 217 ** Case 1 : Pivot kanan
fanny868 0:9072e932503c 218 ** Case 2 : Pivot Kiri
fanny868 0:9072e932503c 219 ** Case 3 : Maju
fanny868 0:9072e932503c 220 ** Case 4 : Mundur
fanny868 0:9072e932503c 221 ** Case 5 : Serong Atas Kanan
fanny868 0:9072e932503c 222 ** Case 6 : Serong Bawah Kanan
fanny868 0:9072e932503c 223 ** Case 7 : Serong Atas Kiri
fanny868 0:9072e932503c 224 ** Case 8 : Serong Bawah Kiri
fanny868 0:9072e932503c 225 ** Case 9 : Kanan
fanny868 0:9072e932503c 226 ** Case 10 : Kiri
rahmadirizki18 3:1287fccc11be 227 ** Case 11 : Analog
rahmadirizki18 3:1287fccc11be 228 ** Case 11 : break
fanny868 0:9072e932503c 229
fanny868 0:9072e932503c 230 **/
rahmadirizki18 3:1287fccc11be 231
rahmadirizki18 5:3aa203218306 232
rahmadirizki18 3:1287fccc11be 233 // Pergerakan dari base
fanny868 0:9072e932503c 234 void aktuator()
fanny868 0:9072e932503c 235 {
rahmadirizki18 6:68293bed71ea 236 //Servo
rahmadirizki18 7:d138c56dab20 237 if (ServoGo){
franshendri 12:e07c59c28c29 238 servoS.position(20);
franshendri 10:f0f0dc3904e0 239 wait_ms(500);
franshendri 12:e07c59c28c29 240 servoS.position(-28);
Joshua23 8:0711dea61312 241 wait_ms(500);
franshendri 12:e07c59c28c29 242 servoS.position(20);
Joshua23 8:0711dea61312 243 wait_ms(500);
franshendri 12:e07c59c28c29 244 for (int i = -0; i<=70; i++){
Joshua23 8:0711dea61312 245 servoB.position(i);
Joshua23 8:0711dea61312 246 wait_ms(10);
Joshua23 8:0711dea61312 247 }
Joshua23 8:0711dea61312 248 wait_ms(500);
Joshua23 8:0711dea61312 249 servoB.position(0);
rahmadirizki18 7:d138c56dab20 250 ServoGo = false;
rahmadirizki18 6:68293bed71ea 251
rahmadirizki18 6:68293bed71ea 252 }else{
franshendri 12:e07c59c28c29 253 servoS.position(20);
rahmadirizki18 6:68293bed71ea 254 servoB.position(0);
fanny868 0:9072e932503c 255
rahmadirizki18 7:d138c56dab20 256 }
rahmadirizki18 6:68293bed71ea 257
rahmadirizki18 6:68293bed71ea 258 // Motor Atas
rahmadirizki18 6:68293bed71ea 259 if (Launcher) {
rahmadirizki18 7:d138c56dab20 260 motorld.speed(speedL);
franshendri 12:e07c59c28c29 261 motorlb.speed(speedB);
rahmadirizki18 6:68293bed71ea 262 }else{
rahmadirizki18 6:68293bed71ea 263 motorld.speed(0);
rahmadirizki18 6:68293bed71ea 264 motorlb.speed(0);
rahmadirizki18 6:68293bed71ea 265 }
rahmadirizki18 6:68293bed71ea 266
rahmadirizki18 6:68293bed71ea 267 // MOTOR Bawah
fanny868 0:9072e932503c 268 switch (case_ger)
fanny868 0:9072e932503c 269 {
rahmadirizki18 3:1287fccc11be 270 case (1):
fanny868 0:9072e932503c 271 {
rahmadirizki18 5:3aa203218306 272 Tetha = Tetha - 0.5;
fanny868 0:9072e932503c 273 pivka=true;
rahmadirizki18 3:1287fccc11be 274 maju=mundur=analog=kiri=kanan=saka=saki=sbka=sbki=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 275
rahmadirizki18 5:3aa203218306 276 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 277
rahmadirizki18 5:3aa203218306 278
fanny868 0:9072e932503c 279 break;
fanny868 0:9072e932503c 280 }
fanny868 0:9072e932503c 281 case (2):
fanny868 0:9072e932503c 282 {
rahmadirizki18 5:3aa203218306 283 Tetha = Tetha + 0.5;
fanny868 0:9072e932503c 284
fanny868 0:9072e932503c 285 pivki=true;
rahmadirizki18 3:1287fccc11be 286 maju=mundur=kiri=analog=kanan=saka=saki=sbka=sbki=pivka=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 287
rahmadirizki18 5:3aa203218306 288 pc.printf("pivKi Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
fanny868 0:9072e932503c 289
rahmadirizki18 5:3aa203218306 290
fanny868 0:9072e932503c 291 break;
fanny868 0:9072e932503c 292 }
fanny868 0:9072e932503c 293 case (3):
fanny868 0:9072e932503c 294 {
franshendri 10:f0f0dc3904e0 295 YT = YT + 0.1;
fanny868 0:9072e932503c 296
fanny868 0:9072e932503c 297 maju=true;
rahmadirizki18 3:1287fccc11be 298 mundur=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 299
rahmadirizki18 5:3aa203218306 300 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 301
fanny868 0:9072e932503c 302 break;
fanny868 0:9072e932503c 303 }
fanny868 0:9072e932503c 304 case (4):
fanny868 0:9072e932503c 305 {
franshendri 10:f0f0dc3904e0 306 YT = YT - 0.1;
fanny868 0:9072e932503c 307
fanny868 0:9072e932503c 308 mundur=true;
rahmadirizki18 3:1287fccc11be 309 maju=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 310
rahmadirizki18 5:3aa203218306 311 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 312
fanny868 0:9072e932503c 313
fanny868 0:9072e932503c 314 break;
fanny868 0:9072e932503c 315 }
fanny868 0:9072e932503c 316 case (5) :
fanny868 0:9072e932503c 317 {
franshendri 10:f0f0dc3904e0 318 XT = XT + 0.1;
franshendri 10:f0f0dc3904e0 319 YT = YT + 0.1;
fanny868 0:9072e932503c 320
fanny868 0:9072e932503c 321 saka=true;
rahmadirizki18 3:1287fccc11be 322 maju=mundur=kiri=kanan=sbka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 323
rahmadirizki18 5:3aa203218306 324 pc.printf("saka Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
fanny868 0:9072e932503c 325
fanny868 0:9072e932503c 326
fanny868 0:9072e932503c 327 break;
fanny868 0:9072e932503c 328 }
fanny868 0:9072e932503c 329 case (6) :
fanny868 0:9072e932503c 330 {
franshendri 10:f0f0dc3904e0 331 XT = XT + 0.1;
franshendri 10:f0f0dc3904e0 332 YT = YT - 0.1;
rahmadirizki18 5:3aa203218306 333
fanny868 0:9072e932503c 334
fanny868 0:9072e932503c 335 sbka=true;
rahmadirizki18 3:1287fccc11be 336 maju=mundur=kiri=kanan=saka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 337
rahmadirizki18 5:3aa203218306 338 pc.printf("sbka Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
fanny868 0:9072e932503c 339
fanny868 0:9072e932503c 340
fanny868 0:9072e932503c 341 break;
fanny868 0:9072e932503c 342 }
fanny868 0:9072e932503c 343 case (7) :
fanny868 0:9072e932503c 344 {
franshendri 10:f0f0dc3904e0 345 XT = XT - 0.1;
franshendri 10:f0f0dc3904e0 346 YT = YT + 0.1;
rahmadirizki18 5:3aa203218306 347
fanny868 0:9072e932503c 348
fanny868 0:9072e932503c 349 saki=true;
rahmadirizki18 3:1287fccc11be 350 maju=kiri=kanan=saka=mundur=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 351
rahmadirizki18 5:3aa203218306 352 pc.printf("saki Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
fanny868 0:9072e932503c 353
fanny868 0:9072e932503c 354
fanny868 0:9072e932503c 355 break;
fanny868 0:9072e932503c 356 }
fanny868 0:9072e932503c 357 case (8) :
fanny868 0:9072e932503c 358 {
franshendri 10:f0f0dc3904e0 359 XT = XT - 0.1;
franshendri 10:f0f0dc3904e0 360 YT = YT - 0.1;
rahmadirizki18 5:3aa203218306 361
rahmadirizki18 5:3aa203218306 362 pc.printf("sbki Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
fanny868 0:9072e932503c 363
fanny868 0:9072e932503c 364
fanny868 0:9072e932503c 365 break;
fanny868 0:9072e932503c 366 }
fanny868 0:9072e932503c 367 case (9) :
fanny868 0:9072e932503c 368 {
franshendri 10:f0f0dc3904e0 369 XT = XT + 0.1;
fanny868 0:9072e932503c 370 kanan=true;
rahmadirizki18 3:1287fccc11be 371 maju=kiri=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 372
rahmadirizki18 5:3aa203218306 373 pc.printf("Kanan Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
fanny868 0:9072e932503c 374
fanny868 0:9072e932503c 375 break;
fanny868 0:9072e932503c 376 }
fanny868 0:9072e932503c 377 case (10) :
fanny868 0:9072e932503c 378 {
franshendri 10:f0f0dc3904e0 379 XT = XT - 0.1;
fanny868 0:9072e932503c 380
fanny868 0:9072e932503c 381 kiri=true;
rahmadirizki18 3:1287fccc11be 382 maju=kanan=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 383
rahmadirizki18 5:3aa203218306 384 pc.printf("Kiri Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
fanny868 0:9072e932503c 385
fanny868 0:9072e932503c 386
fanny868 0:9072e932503c 387 break;
fanny868 0:9072e932503c 388 }
rahmadirizki18 3:1287fccc11be 389 case (11):
rahmadirizki18 3:1287fccc11be 390 {
rahmadirizki18 4:483c07cc22e1 391
franshendri 10:f0f0dc3904e0 392 XT = XT + 0.1*x;
franshendri 10:f0f0dc3904e0 393 YT = YT + 0.1*y;
rahmadirizki18 3:1287fccc11be 394
rahmadirizki18 3:1287fccc11be 395 analog=true;
rahmadirizki18 3:1287fccc11be 396 maju=mundur=kiri=kanan=saka=saki=sbka=sbki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
rahmadirizki18 3:1287fccc11be 397
rahmadirizki18 5:3aa203218306 398 pc.printf("analog Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
rahmadirizki18 3:1287fccc11be 399
rahmadirizki18 3:1287fccc11be 400 break;
rahmadirizki18 3:1287fccc11be 401 }
rahmadirizki18 3:1287fccc11be 402 default :
fanny868 0:9072e932503c 403 {
rahmadirizki18 5:3aa203218306 404
rahmadirizki18 3:1287fccc11be 405 maju=mundur=kiri=kanan=saka=saki=sbka=sbki=analog=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 406 stop = true;
fanny868 0:9072e932503c 407
fanny868 0:9072e932503c 408 //s1 = 0;s2 =0; s3 =0; s4 =0;
fanny868 0:9072e932503c 409
rahmadirizki18 5:3aa203218306 410 // pc.printf("Stop V1=%.2f V2=%.2f V3=%.2f V4=%.2f\n",V1,V2,V3,V4);
rahmadirizki18 3:1287fccc11be 411 }
rahmadirizki18 5:3aa203218306 412 }
rahmadirizki18 5:3aa203218306 413 }
rahmadirizki18 5:3aa203218306 414
rahmadirizki18 5:3aa203218306 415 //Begin Encoder
rahmadirizki18 5:3aa203218306 416
rahmadirizki18 5:3aa203218306 417 void setCenter()
rahmadirizki18 5:3aa203218306 418 {
rahmadirizki18 5:3aa203218306 419 encoderDepan.reset();
rahmadirizki18 5:3aa203218306 420 encoderBelakang.reset();
rahmadirizki18 5:3aa203218306 421 encoderKanan.reset();
rahmadirizki18 5:3aa203218306 422 encoderKiri.reset();
rahmadirizki18 5:3aa203218306 423 }
rahmadirizki18 5:3aa203218306 424
rahmadirizki18 5:3aa203218306 425 float getX()
rahmadirizki18 5:3aa203218306 426 {
rahmadirizki18 5:3aa203218306 427 float jarakEncDpn, jarakEncBlk;
rahmadirizki18 5:3aa203218306 428 jarakEncDpn = (encoderDepan.getPulses())/(float)(2000.0)*K_enc;
rahmadirizki18 5:3aa203218306 429 jarakEncBlk = (encoderBelakang.getPulses())/(float)(2000.0)*K_enc;
rahmadirizki18 5:3aa203218306 430 return (jarakEncDpn-jarakEncBlk)/2;
rahmadirizki18 5:3aa203218306 431 }
rahmadirizki18 5:3aa203218306 432
rahmadirizki18 5:3aa203218306 433 float getY()
rahmadirizki18 5:3aa203218306 434 {
rahmadirizki18 5:3aa203218306 435 float jarakEncKir, jarakEncKan;
rahmadirizki18 5:3aa203218306 436 jarakEncKir = (encoderKiri.getPulses())/(float)(2000.0)*K_enc;
rahmadirizki18 5:3aa203218306 437 jarakEncKan = (encoderKanan.getPulses())/(float)(720.0)*K_enc;
rahmadirizki18 5:3aa203218306 438 return (jarakEncKir-jarakEncKan)/2;
rahmadirizki18 5:3aa203218306 439 }
rahmadirizki18 5:3aa203218306 440
rahmadirizki18 5:3aa203218306 441 float getTetha()
rahmadirizki18 5:3aa203218306 442 {
rahmadirizki18 5:3aa203218306 443 float busurDpn, busurBlk, busurKir, busurKan;
rahmadirizki18 5:3aa203218306 444 busurDpn = ((encoderDepan.getPulses())/(float)(2000.0)*K_enc)/K_robot*360.0;
rahmadirizki18 5:3aa203218306 445 busurBlk = ((encoderBelakang.getPulses())/(float)(2000.0)*K_enc)/K_robot*360.0;
rahmadirizki18 5:3aa203218306 446 busurKir = ((encoderKiri.getPulses())/(float)(2000.0)*K_enc)/K_robot*360.0;
rahmadirizki18 5:3aa203218306 447 busurKan = ((encoderKanan.getPulses())/(float)(720.0)*K_enc)/K_robot*360.0;
rahmadirizki18 5:3aa203218306 448
rahmadirizki18 5:3aa203218306 449 return -(busurDpn+busurBlk+busurKir+busurKan)/4;
fanny868 0:9072e932503c 450 }
fanny868 0:9072e932503c 451
rahmadirizki18 5:3aa203218306 452 void gotoXYT(float xa, float ya, float Ta)
rahmadirizki18 5:3aa203218306 453 {
rahmadirizki18 5:3aa203218306 454
rahmadirizki18 5:3aa203218306 455 errX = xa-getX();
rahmadirizki18 5:3aa203218306 456 Vx = KpX*errX;
rahmadirizki18 5:3aa203218306 457
rahmadirizki18 5:3aa203218306 458 errY = ya-getY();
rahmadirizki18 5:3aa203218306 459 Vy = KpY*errY;
rahmadirizki18 5:3aa203218306 460
rahmadirizki18 5:3aa203218306 461 errT = Ta-getTetha();
rahmadirizki18 5:3aa203218306 462 Vt = Kp_tetha*errT;
rahmadirizki18 5:3aa203218306 463
rahmadirizki18 5:3aa203218306 464 V1 = Vx+Vy-Vt;
rahmadirizki18 5:3aa203218306 465 V2 = Vx-Vy-Vt;
rahmadirizki18 5:3aa203218306 466 V3 = -Vx-Vy-Vt;
rahmadirizki18 5:3aa203218306 467 V4 = -Vx+Vy-Vt;
rahmadirizki18 5:3aa203218306 468
rahmadirizki18 5:3aa203218306 469 if (V1>speed1)
rahmadirizki18 5:3aa203218306 470 { V1 = speed1; }
rahmadirizki18 5:3aa203218306 471 else if (V1<-speed1)
rahmadirizki18 5:3aa203218306 472 { V1 = -speed1; }
rahmadirizki18 5:3aa203218306 473
rahmadirizki18 5:3aa203218306 474 if (V2>speed2)
rahmadirizki18 5:3aa203218306 475 { V2 = speed2; }
rahmadirizki18 5:3aa203218306 476 else if (V2<-speed2)
rahmadirizki18 5:3aa203218306 477 { V2 = -speed2; }
rahmadirizki18 5:3aa203218306 478
rahmadirizki18 5:3aa203218306 479 if (V3>speed3)
rahmadirizki18 5:3aa203218306 480 { V3 = speed3; }
rahmadirizki18 5:3aa203218306 481 else if (V3<-speed3)
rahmadirizki18 5:3aa203218306 482 { V3 = -speed3; }
rahmadirizki18 5:3aa203218306 483
rahmadirizki18 5:3aa203218306 484 if (V4>speed4)
rahmadirizki18 5:3aa203218306 485 { V4 = speed4; }
rahmadirizki18 5:3aa203218306 486 else if (V4<-speed4)
rahmadirizki18 5:3aa203218306 487 { V4 = -speed4; }
rahmadirizki18 5:3aa203218306 488
rahmadirizki18 5:3aa203218306 489 if (((errX > 0.05) || (errX<-0.05)) || ((errY > 0.05) || (errY<-0.05)) || ((errT > 0.05) || (errT<-0.05)))
rahmadirizki18 5:3aa203218306 490 {
rahmadirizki18 5:3aa203218306 491 motor1.speed(V1);
rahmadirizki18 5:3aa203218306 492 motor2.speed(V2);
rahmadirizki18 5:3aa203218306 493 motor3.speed(V3);
rahmadirizki18 5:3aa203218306 494 motor4.speed(V4);
rahmadirizki18 5:3aa203218306 495 // pc.printf("V1=%.2f \ ", V1);
rahmadirizki18 5:3aa203218306 496 }
rahmadirizki18 5:3aa203218306 497 else
rahmadirizki18 5:3aa203218306 498 {
rahmadirizki18 5:3aa203218306 499 motor1.brake(1);
rahmadirizki18 5:3aa203218306 500 motor2.brake(1);
rahmadirizki18 5:3aa203218306 501 motor3.brake(1);
rahmadirizki18 5:3aa203218306 502 motor4.brake(1);
rahmadirizki18 5:3aa203218306 503 //_ms(1000);
rahmadirizki18 5:3aa203218306 504 }
rahmadirizki18 5:3aa203218306 505 //pc.printf("%f\t%f\t%f ", errX*100,errY*100,errT);
rahmadirizki18 5:3aa203218306 506 // wait_ms(10);
rahmadirizki18 5:3aa203218306 507
rahmadirizki18 5:3aa203218306 508 }
rahmadirizki18 5:3aa203218306 509 //End Encoder
rahmadirizki18 5:3aa203218306 510
rahmadirizki18 7:d138c56dab20 511 void speedLauncher()
rahmadirizki18 7:d138c56dab20 512 {
franshendri 12:e07c59c28c29 513 if (joystick.R3_click and speedL < 0.8){
franshendri 12:e07c59c28c29 514 speedL = speedL + 0.01;}
rahmadirizki18 7:d138c56dab20 515 if (joystick.L3_click and speedL > 0.1){
franshendri 12:e07c59c28c29 516 speedL = speedL - 0.01;}
rahmadirizki18 13:8ab42383a2ca 517 if (joystick.R2_click and speedB < 0.8 ){
rahmadirizki18 13:8ab42383a2ca 518 speedB = speedB + 0.01;}
rahmadirizki18 13:8ab42383a2ca 519 if (joystick.L2_click and speedB > 0.1 ){
rahmadirizki18 13:8ab42383a2ca 520 speedB = speedB - 0.01;}
rahmadirizki18 13:8ab42383a2ca 521 pc.printf("Pwm depan = %.3f\t Pwm belakang = %.3f\n", speedL, speedB);
rahmadirizki18 7:d138c56dab20 522 }
franshendri 12:e07c59c28c29 523
franshendri 12:e07c59c28c29 524
franshendri 12:e07c59c28c29 525
fanny868 0:9072e932503c 526 int main (void)
fanny868 0:9072e932503c 527 {
fanny868 0:9072e932503c 528 // Set baud rate - 115200
fanny868 0:9072e932503c 529 joystick.setup();
rahmadirizki18 3:1287fccc11be 530 pc.baud(115200);
rahmadirizki18 6:68293bed71ea 531 wait_ms(1000);
rahmadirizki18 5:3aa203218306 532 setCenter();
rahmadirizki18 5:3aa203218306 533 wait_ms(500);
rahmadirizki18 5:3aa203218306 534
rahmadirizki18 5:3aa203218306 535 //Posisi Awal
rahmadirizki18 5:3aa203218306 536 XT = 0;
rahmadirizki18 5:3aa203218306 537 YT = 0;
rahmadirizki18 5:3aa203218306 538 Tetha = 0;
rahmadirizki18 3:1287fccc11be 539 pc.printf("Ready...\n");
rahmadirizki18 4:483c07cc22e1 540 kalibrasi();
rahmadirizki18 6:68293bed71ea 541 waktu.start();
fanny868 0:9072e932503c 542 while(1)
fanny868 0:9072e932503c 543 {
fanny868 0:9072e932503c 544 // Interrupt Serial
fanny868 0:9072e932503c 545 joystick.idle();
fanny868 0:9072e932503c 546 if(joystick.readable() ) {
fanny868 0:9072e932503c 547 // Panggil fungsi pembacaan joystik
fanny868 0:9072e932503c 548 joystick.baca_data();
fanny868 0:9072e932503c 549 // Panggil fungsi pengolahan data joystik
fanny868 0:9072e932503c 550 joystick.olah_data();
franshendri 12:e07c59c28c29 551 //pc.printf("%3d %3d\n\r",joystick.R2, joystick.L2);
fanny868 0:9072e932503c 552 //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 553 case_ger = case_gerak();
rahmadirizki18 3:1287fccc11be 554 aktuator();
rahmadirizki18 6:68293bed71ea 555
franshendri 12:e07c59c28c29 556 //pc.printf("bacaS = %.2f\tbacaB = %.2f\n",servoS.read(), servoB.read());
rahmadirizki18 6:68293bed71ea 557
rahmadirizki18 3:1287fccc11be 558 //kalibrasi
rahmadirizki18 4:483c07cc22e1 559 if (joystick.START){
rahmadirizki18 4:483c07cc22e1 560 kalibrasi();}
rahmadirizki18 4:483c07cc22e1 561 // analog switch mode
rahmadirizki18 6:68293bed71ea 562 if (joystick.SELECT_click) {analog = !analog;}
rahmadirizki18 6:68293bed71ea 563 if (joystick.segitiga_click) {Launcher = !Launcher;}
rahmadirizki18 6:68293bed71ea 564 if (joystick.lingkaran_click){
rahmadirizki18 6:68293bed71ea 565 ServoGo = true;
rahmadirizki18 6:68293bed71ea 566 waktu.reset();
rahmadirizki18 6:68293bed71ea 567 }
rahmadirizki18 5:3aa203218306 568 if (joystick.silang) {
rahmadirizki18 5:3aa203218306 569 XT = 0;
rahmadirizki18 5:3aa203218306 570 YT = 0;
rahmadirizki18 5:3aa203218306 571 Tetha = 0;
rahmadirizki18 5:3aa203218306 572 pc.printf("x..\n");
rahmadirizki18 5:3aa203218306 573 }
rahmadirizki18 7:d138c56dab20 574 speedLauncher();
franshendri 12:e07c59c28c29 575
fanny868 0:9072e932503c 576 } else {
fanny868 0:9072e932503c 577 joystick.idle();
fanny868 0:9072e932503c 578
fanny868 0:9072e932503c 579 }
rahmadirizki18 5:3aa203218306 580 gotoXYT(XT,YT,Tetha);
rahmadirizki18 5:3aa203218306 581
fanny868 0:9072e932503c 582 }
fanny868 0:9072e932503c 583 }