KRAI 2017 / Mbed 2 deprecated MainProgram_BaseBaru_fix_omni_25Mar

di BSCA

Dependencies:   DigitDisplay Motor PID Ping mbed millis

Fork of MainProgram_BaseBaru_fix_omni12Feb by KRAI 2017

main.cpp@18:1da121ddb7c1, 2017-01-27 (annotated)

Committer:
rahmadirizki18
Date:
Fri Jan 27 16:20:44 2017 +0000
Revision:
18:1da121ddb7c1
Parent:
17:e4229d77a5ab
Child:
19:38f148ce00f0
yoi

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 */
calmantara186 16:90119f03c5d1 31 /* Tombol R3 => PWM Launcher ditambahin */
rahmadirizki18 13:8ab42383a2ca 32 /* */
calmantara186 16:90119f03c5d1 33 /* Bismillahirahmanirrahim */
calmantara186 16:90119f03c5d1 34 /* Jagalah Kebersihan kodingan */
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"
rahmadirizki18 5:3aa203218306 42 #include "encoderKRAI.h"
rahmadirizki18 5:3aa203218306 43
calmantara186 16:90119f03c5d1 44 /***********************************************/
calmantara186 16:90119f03c5d1 45 /* Konstanta dan Variabel */
calmantara186 16:90119f03c5d1 46 /***********************************************/
calmantara186 16:90119f03c5d1 47 #define PI 3.14159265
calmantara186 16:90119f03c5d1 48 #define D_ENCODER 0.058
calmantara186 16:90119f03c5d1 49 #define D_ROBOT 0.64
rahmadirizki18 17:e4229d77a5ab 50 #define VMAX 0.3 // Maju, Mundur, Kiri Kanan
rahmadirizki18 17:e4229d77a5ab 51 #define SAMPING 0.3 // Saka, Saki, Sbka, Sbki
rahmadirizki18 17:e4229d77a5ab 52 #define PIVOT 0.4 // Pivka, Pivki
rahmadirizki18 17:e4229d77a5ab 53 #define PERPINDAHAN 1 // Perpindahan ke kanan dan kiri
rahmadirizki18 5:3aa203218306 54
calmantara186 16:90119f03c5d1 55 float k_enc = PI*D_ENCODER;
calmantara186 16:90119f03c5d1 56 float k_robot = PI*D_ROBOT;
rahmadirizki18 5:3aa203218306 57
calmantara186 16:90119f03c5d1 58 float speed1 =0.6;
calmantara186 16:90119f03c5d1 59 float speed2 =0.6;
calmantara186 16:90119f03c5d1 60 float speed3 =0.6;
calmantara186 16:90119f03c5d1 61 float speed4 =0.6;
calmantara186 16:90119f03c5d1 62 float speedB =0.43;
calmantara186 16:90119f03c5d1 63 float speedL =0.4;
rahmadirizki18 5:3aa203218306 64
calmantara186 16:90119f03c5d1 65 float kpX=0.5, kpY=0.5, kp_tetha=0.03;
calmantara186 16:90119f03c5d1 66
calmantara186 16:90119f03c5d1 67 /* Deklarasi encoder */
calmantara186 16:90119f03c5d1 68 encoderKRAI encoderDepan( PB_13, PB_14, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
calmantara186 16:90119f03c5d1 69 encoderKRAI encoderBelakang( PC_11, PC_10, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
calmantara186 16:90119f03c5d1 70 encoderKRAI encoderKanan( PC_12, PD_2, 720, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
calmantara186 16:90119f03c5d1 71 encoderKRAI encoderKiri( PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
rahmadirizki18 15:98f0d56b14f0 72
calmantara186 16:90119f03c5d1 73 /* Deklarasi Motor Base */
calmantara186 16:90119f03c5d1 74 Motor motor1(PB_7, PA_14 , PA_15); // pwm, fwd, rev
calmantara186 16:90119f03c5d1 75 Motor motor2(PB_8, PA_13 ,PB_0); // pwm, fwd, rev
calmantara186 16:90119f03c5d1 76 Motor motor3(PB_9, PA_12 ,PC_5); // pwm, fwd, rev
calmantara186 16:90119f03c5d1 77 Motor motor4(PB_6, PB_1 ,PB_12); // pwm, fwd, rev
fanny868 0:9072e932503c 78
calmantara186 16:90119f03c5d1 79 /* Deklarasi Motor Launcher */
calmantara186 16:90119f03c5d1 80 Motor motorld(PA_8, PC_1 , PC_2); // pwm, fwd, rev
calmantara186 16:90119f03c5d1 81 Motor motorlb(PA_9, PA_4, PC_15 ); // pwm, fwd, rev
rahmadirizki18 5:3aa203218306 82
calmantara186 16:90119f03c5d1 83 /* Deklarasi Servo Launcher */
rahmadirizki18 14:6d389e99981c 84 Servo servoS(PB_2);
rahmadirizki18 14:6d389e99981c 85 Servo servoB(PA_5);
rahmadirizki18 6:68293bed71ea 86
calmantara186 16:90119f03c5d1 87 /**
calmantara186 16:90119f03c5d1 88 * posX dan posY berdasarkan arah robot
calmantara186 16:90119f03c5d1 89 * encoder Depan & Belakang sejajar sumbu Y
calmantara186 16:90119f03c5d1 90 * encoder Kanan & Kiri sejajar sumbu X
calmantara186 16:90119f03c5d1 91 **/
rahmadirizki18 5:3aa203218306 92
calmantara186 16:90119f03c5d1 93 /* Variabel Encoder */
calmantara186 16:90119f03c5d1 94 float jarak, posX, posY; //
calmantara186 16:90119f03c5d1 95 float XT, YT, Tetha; // Jarak Target Robot
calmantara186 16:90119f03c5d1 96 float errX, errY, errT, Vt, Vx, Vy; // Variabel yang didapatkan encoder
calmantara186 16:90119f03c5d1 97 float v1, v2, v3, v4; // Variabel kecepatan motor dari encoder
rahmadirizki18 5:3aa203218306 98
calmantara186 16:90119f03c5d1 99 /* Fungsi dan Procedur Encoder */
calmantara186 16:90119f03c5d1 100 void setCenter(); // Fungsi reset agar robot di tengah
calmantara186 16:90119f03c5d1 101 float getY(); // FUngsi mendapatkan jarak Y
calmantara186 16:90119f03c5d1 102 float getX(); // FUngsi mendapatkan jarak X
calmantara186 16:90119f03c5d1 103 float getTetha(); // FUngsi mendapatkan jarak Tetha
calmantara186 16:90119f03c5d1 104
calmantara186 16:90119f03c5d1 105 /* Inisialisasi Pin TX-RX Joystik dan PC */
rahmadirizki18 3:1287fccc11be 106 joysticknucleo joystick(PA_0,PA_1);
fanny868 0:9072e932503c 107
calmantara186 16:90119f03c5d1 108 /* Variabel Stick */
fanny868 0:9072e932503c 109 char case_ger;
rahmadirizki18 18:1da121ddb7c1 110 bool launcher = false, servoGo = false, manual = true;
rahmadirizki18 18:1da121ddb7c1 111 int caseSebelum;
fanny868 0:9072e932503c 112
calmantara186 16:90119f03c5d1 113 /***********************************************/
calmantara186 16:90119f03c5d1 114 /* Deklarasi Fungsi dan Procedure */
calmantara186 16:90119f03c5d1 115 /***********************************************/
calmantara186 16:90119f03c5d1 116 int case_gerak(){
calmantara186 16:90119f03c5d1 117 /*****************************************************
calmantara186 16:90119f03c5d1 118 ** Gerak Motor Base
calmantara186 16:90119f03c5d1 119 ** Case 1 : Pivot kanan
calmantara186 16:90119f03c5d1 120 ** Case 2 : Pivot Kiri
calmantara186 16:90119f03c5d1 121 ** Case 3 : Maju
calmantara186 16:90119f03c5d1 122 ** Case 4 : Mundur
calmantara186 16:90119f03c5d1 123 ** Case 5 : Serong Atas Kanan
calmantara186 16:90119f03c5d1 124 ** Case 6 : Serong Bawah Kanan
calmantara186 16:90119f03c5d1 125 ** Case 7 : Serong Atas Kiri
calmantara186 16:90119f03c5d1 126 ** Case 8 : Serong Bawah Kiri
calmantara186 16:90119f03c5d1 127 ** Case 9 : Kanan
calmantara186 16:90119f03c5d1 128 ** Case 10 : Kiri
calmantara186 16:90119f03c5d1 129 ** Case 12 : break
calmantara186 16:90119f03c5d1 130 ****************************************************/
fanny868 0:9072e932503c 131 int casegerak;
calmantara186 16:90119f03c5d1 132 if (!joystick.L1 && joystick.R1) {
fanny868 0:9072e932503c 133 // Pivot Kanan
fanny868 0:9072e932503c 134 casegerak = 1;
calmantara186 16:90119f03c5d1 135 }
calmantara186 16:90119f03c5d1 136 else if (!joystick.R1 && joystick.L1) {
fanny868 0:9072e932503c 137 // Pivot Kiri
fanny868 0:9072e932503c 138 casegerak = 2;
calmantara186 16:90119f03c5d1 139 }
calmantara186 16:90119f03c5d1 140 else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
fanny868 0:9072e932503c 141 // Maju
rahmadirizki18 3:1287fccc11be 142 casegerak = 3;
calmantara186 16:90119f03c5d1 143 }
calmantara186 16:90119f03c5d1 144 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
fanny868 0:9072e932503c 145 // Mundur
fanny868 0:9072e932503c 146 casegerak = 4;
calmantara186 16:90119f03c5d1 147 }
calmantara186 16:90119f03c5d1 148 else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan)) {
fanny868 0:9072e932503c 149 // Serong Atas Kanan
fanny868 0:9072e932503c 150 casegerak = 5;
calmantara186 16:90119f03c5d1 151 }
calmantara186 16:90119f03c5d1 152 else if((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan)) {
fanny868 0:9072e932503c 153 // Serong Bawah Kanan
fanny868 0:9072e932503c 154 casegerak = 6;
calmantara186 16:90119f03c5d1 155 }
calmantara186 16:90119f03c5d1 156 else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)) {
fanny868 0:9072e932503c 157 // Serong Atas Kiri
fanny868 0:9072e932503c 158 casegerak = 7;
calmantara186 16:90119f03c5d1 159 }
calmantara186 16:90119f03c5d1 160 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)) {
fanny868 0:9072e932503c 161 // Serong Bawah Kiri
fanny868 0:9072e932503c 162 casegerak = 8;
calmantara186 16:90119f03c5d1 163 }
calmantara186 16:90119f03c5d1 164 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
fanny868 0:9072e932503c 165 // Kanan
fanny868 0:9072e932503c 166 casegerak = 9;
calmantara186 16:90119f03c5d1 167 }
calmantara186 16:90119f03c5d1 168 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
fanny868 0:9072e932503c 169 // Kiri
rahmadirizki18 18:1da121ddb7c1 170 casegerak = 10;
calmantara186 16:90119f03c5d1 171 }
calmantara186 16:90119f03c5d1 172 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
calmantara186 16:90119f03c5d1 173 casegerak = 12;
calmantara186 16:90119f03c5d1 174 }
fanny868 0:9072e932503c 175 return(casegerak);
fanny868 0:9072e932503c 176 }
fanny868 0:9072e932503c 177
calmantara186 16:90119f03c5d1 178 void aktuator(){
calmantara186 16:90119f03c5d1 179 /* Fungsi untuk menggerakkan servo */
calmantara186 16:90119f03c5d1 180 // Servo
calmantara186 16:90119f03c5d1 181 if (servoGo){
franshendri 12:e07c59c28c29 182 servoS.position(20);
franshendri 10:f0f0dc3904e0 183 wait_ms(500);
franshendri 12:e07c59c28c29 184 servoS.position(-28);
Joshua23 8:0711dea61312 185 wait_ms(500);
franshendri 12:e07c59c28c29 186 servoS.position(20);
Joshua23 8:0711dea61312 187 wait_ms(500);
franshendri 12:e07c59c28c29 188 for (int i = -0; i<=70; i++){
Joshua23 8:0711dea61312 189 servoB.position(i);
Joshua23 8:0711dea61312 190 wait_ms(10);
Joshua23 8:0711dea61312 191 }
Joshua23 8:0711dea61312 192 wait_ms(500);
Joshua23 8:0711dea61312 193 servoB.position(0);
calmantara186 16:90119f03c5d1 194 servoGo = false;
calmantara186 16:90119f03c5d1 195 }
calmantara186 16:90119f03c5d1 196 else{
franshendri 12:e07c59c28c29 197 servoS.position(20);
rahmadirizki18 6:68293bed71ea 198 servoB.position(0);
rahmadirizki18 7:d138c56dab20 199 }
rahmadirizki18 6:68293bed71ea 200
rahmadirizki18 6:68293bed71ea 201 // Motor Atas
calmantara186 16:90119f03c5d1 202 if (launcher) {
rahmadirizki18 7:d138c56dab20 203 motorld.speed(speedL);
franshendri 12:e07c59c28c29 204 motorlb.speed(speedB);
rahmadirizki18 6:68293bed71ea 205 }else{
rahmadirizki18 6:68293bed71ea 206 motorld.speed(0);
rahmadirizki18 6:68293bed71ea 207 motorlb.speed(0);
rahmadirizki18 6:68293bed71ea 208 }
rahmadirizki18 6:68293bed71ea 209
rahmadirizki18 6:68293bed71ea 210 // MOTOR Bawah
rahmadirizki18 17:e4229d77a5ab 211 if (manual) {
rahmadirizki18 17:e4229d77a5ab 212 // Mode Manual
rahmadirizki18 17:e4229d77a5ab 213 switch (case_ger) {
rahmadirizki18 17:e4229d77a5ab 214 case (1): {
rahmadirizki18 17:e4229d77a5ab 215 // Pivot Kanan
rahmadirizki18 17:e4229d77a5ab 216 motor1.speed(-PIVOT);
rahmadirizki18 17:e4229d77a5ab 217 motor2.speed(-PIVOT);
rahmadirizki18 17:e4229d77a5ab 218 motor3.speed(-PIVOT);
rahmadirizki18 17:e4229d77a5ab 219 motor4.speed(-PIVOT);
fanny868 0:9072e932503c 220 break;
rahmadirizki18 17:e4229d77a5ab 221 }
rahmadirizki18 17:e4229d77a5ab 222 case (2): {
rahmadirizki18 17:e4229d77a5ab 223 // Pivot Kiri
rahmadirizki18 17:e4229d77a5ab 224 motor1.speed(PIVOT);
rahmadirizki18 17:e4229d77a5ab 225 motor2.speed(PIVOT);
rahmadirizki18 17:e4229d77a5ab 226 motor3.speed(PIVOT);
rahmadirizki18 17:e4229d77a5ab 227 motor4.speed(PIVOT);
rahmadirizki18 17:e4229d77a5ab 228 break;
rahmadirizki18 17:e4229d77a5ab 229 }
rahmadirizki18 17:e4229d77a5ab 230 case (3): {
rahmadirizki18 17:e4229d77a5ab 231 // Maju
rahmadirizki18 17:e4229d77a5ab 232 motor1.speed(-VMAX);
rahmadirizki18 17:e4229d77a5ab 233 motor2.speed(VMAX);
rahmadirizki18 17:e4229d77a5ab 234 motor3.speed(VMAX);
rahmadirizki18 17:e4229d77a5ab 235 motor4.speed(-VMAX);
fanny868 0:9072e932503c 236 break;
fanny868 0:9072e932503c 237 }
rahmadirizki18 17:e4229d77a5ab 238 case (4): {
rahmadirizki18 17:e4229d77a5ab 239 // Mundur
rahmadirizki18 17:e4229d77a5ab 240 motor1.speed(VMAX);
rahmadirizki18 17:e4229d77a5ab 241 motor2.speed(-VMAX);
rahmadirizki18 17:e4229d77a5ab 242 motor3.speed(-VMAX);
rahmadirizki18 17:e4229d77a5ab 243 motor4.speed(VMAX);
rahmadirizki18 17:e4229d77a5ab 244 break;
rahmadirizki18 17:e4229d77a5ab 245 }
rahmadirizki18 17:e4229d77a5ab 246 case (5) : {
rahmadirizki18 17:e4229d77a5ab 247 // Samping Atas Kanan
rahmadirizki18 17:e4229d77a5ab 248 motor1.speed(-SAMPING);
rahmadirizki18 17:e4229d77a5ab 249 motor2.brake(1);
rahmadirizki18 17:e4229d77a5ab 250 motor3.speed(SAMPING);
rahmadirizki18 17:e4229d77a5ab 251 motor4.brake(1);
fanny868 0:9072e932503c 252 break;
fanny868 0:9072e932503c 253 }
rahmadirizki18 17:e4229d77a5ab 254 case (6) : {
rahmadirizki18 17:e4229d77a5ab 255 // Samping Bawah Kanan
rahmadirizki18 17:e4229d77a5ab 256 motor1.brake(1);
rahmadirizki18 17:e4229d77a5ab 257 motor2.speed(-SAMPING);
rahmadirizki18 17:e4229d77a5ab 258 motor3.brake(1);
rahmadirizki18 17:e4229d77a5ab 259 motor4.speed(SAMPING);
fanny868 0:9072e932503c 260 break;
fanny868 0:9072e932503c 261 }
rahmadirizki18 17:e4229d77a5ab 262 case (7) : {
rahmadirizki18 17:e4229d77a5ab 263 // Samping Atas Kiri
rahmadirizki18 17:e4229d77a5ab 264 motor1.brake(1);
rahmadirizki18 17:e4229d77a5ab 265 motor2.speed(SAMPING);
rahmadirizki18 17:e4229d77a5ab 266 motor3.brake(1);
rahmadirizki18 17:e4229d77a5ab 267 motor4.speed(-SAMPING);
rahmadirizki18 17:e4229d77a5ab 268 break;
rahmadirizki18 17:e4229d77a5ab 269 }
rahmadirizki18 17:e4229d77a5ab 270 case (8) : {
rahmadirizki18 17:e4229d77a5ab 271 // Samping Bawah Kiri
rahmadirizki18 17:e4229d77a5ab 272 motor1.speed(SAMPING);
rahmadirizki18 17:e4229d77a5ab 273 motor2.brake(1);
rahmadirizki18 17:e4229d77a5ab 274 motor3.speed(-SAMPING);
rahmadirizki18 17:e4229d77a5ab 275 motor4.brake(1);
fanny868 0:9072e932503c 276 break;
fanny868 0:9072e932503c 277 }
rahmadirizki18 17:e4229d77a5ab 278 case (9) : {
rahmadirizki18 17:e4229d77a5ab 279 // Kanan
rahmadirizki18 17:e4229d77a5ab 280 motor1.speed(-VMAX);
rahmadirizki18 17:e4229d77a5ab 281 motor2.speed(-VMAX);
rahmadirizki18 17:e4229d77a5ab 282 motor3.speed(VMAX);
rahmadirizki18 17:e4229d77a5ab 283 motor4.speed(VMAX);
fanny868 0:9072e932503c 284 break;
fanny868 0:9072e932503c 285 }
rahmadirizki18 17:e4229d77a5ab 286 case (10) : {
rahmadirizki18 17:e4229d77a5ab 287 // Kiri
rahmadirizki18 17:e4229d77a5ab 288 motor1.speed(VMAX);
rahmadirizki18 17:e4229d77a5ab 289 motor2.speed(VMAX);
rahmadirizki18 17:e4229d77a5ab 290 motor3.speed(-VMAX);
rahmadirizki18 17:e4229d77a5ab 291 motor4.speed(-VMAX);
fanny868 0:9072e932503c 292 break;
rahmadirizki18 17:e4229d77a5ab 293 }
rahmadirizki18 17:e4229d77a5ab 294 default : {
rahmadirizki18 17:e4229d77a5ab 295 motor1.brake(1);
rahmadirizki18 17:e4229d77a5ab 296 motor2.brake(1);
rahmadirizki18 17:e4229d77a5ab 297 motor3.brake(1);
rahmadirizki18 17:e4229d77a5ab 298 motor4.brake(1);
rahmadirizki18 17:e4229d77a5ab 299 }
rahmadirizki18 17:e4229d77a5ab 300 } // End Switch
rahmadirizki18 17:e4229d77a5ab 301 } else {
rahmadirizki18 17:e4229d77a5ab 302 //Mode Encoder
rahmadirizki18 17:e4229d77a5ab 303 switch (case_ger) {
rahmadirizki18 17:e4229d77a5ab 304 case (1):{
rahmadirizki18 17:e4229d77a5ab 305 Tetha = Tetha - 0.05;
rahmadirizki18 17:e4229d77a5ab 306 break;
rahmadirizki18 17:e4229d77a5ab 307 }
rahmadirizki18 17:e4229d77a5ab 308 case (2):{
rahmadirizki18 17:e4229d77a5ab 309 Tetha = Tetha + 0.05;
rahmadirizki18 17:e4229d77a5ab 310 break;
rahmadirizki18 17:e4229d77a5ab 311 }
rahmadirizki18 17:e4229d77a5ab 312 case (3):{
rahmadirizki18 17:e4229d77a5ab 313 YT = YT + 0.01;
rahmadirizki18 17:e4229d77a5ab 314 break;
rahmadirizki18 17:e4229d77a5ab 315 }
rahmadirizki18 17:e4229d77a5ab 316 case (4):{
rahmadirizki18 17:e4229d77a5ab 317 YT = YT - 0.01;
rahmadirizki18 17:e4229d77a5ab 318 break;
rahmadirizki18 17:e4229d77a5ab 319 }
rahmadirizki18 17:e4229d77a5ab 320 case (5) :{
rahmadirizki18 17:e4229d77a5ab 321 XT = XT + 0.01;
rahmadirizki18 17:e4229d77a5ab 322 YT = YT + 0.01;
rahmadirizki18 17:e4229d77a5ab 323 break;
rahmadirizki18 17:e4229d77a5ab 324 }
rahmadirizki18 17:e4229d77a5ab 325 case (6) :{
rahmadirizki18 17:e4229d77a5ab 326 XT = XT + 0.01;
rahmadirizki18 17:e4229d77a5ab 327 YT = YT - 0.01;
rahmadirizki18 17:e4229d77a5ab 328 break;
rahmadirizki18 17:e4229d77a5ab 329 }
rahmadirizki18 17:e4229d77a5ab 330 case (7) :{
rahmadirizki18 17:e4229d77a5ab 331 XT = XT - 0.01;
rahmadirizki18 17:e4229d77a5ab 332 YT = YT + 0.01;
rahmadirizki18 17:e4229d77a5ab 333 break;
rahmadirizki18 17:e4229d77a5ab 334 }
rahmadirizki18 17:e4229d77a5ab 335 case (8) :{
rahmadirizki18 17:e4229d77a5ab 336 XT = XT - 0.01;
rahmadirizki18 17:e4229d77a5ab 337 YT = YT - 0.01;
rahmadirizki18 17:e4229d77a5ab 338 break;
rahmadirizki18 17:e4229d77a5ab 339 }
rahmadirizki18 17:e4229d77a5ab 340 case (9) :{
rahmadirizki18 17:e4229d77a5ab 341 // Kanan
rahmadirizki18 18:1da121ddb7c1 342 if (case_ger == caseSebelum) XT = XT + PERPINDAHAN;
rahmadirizki18 17:e4229d77a5ab 343
rahmadirizki18 18:1da121ddb7c1 344
rahmadirizki18 3:1287fccc11be 345 break;
rahmadirizki18 17:e4229d77a5ab 346 }
rahmadirizki18 17:e4229d77a5ab 347 case (10) :{
rahmadirizki18 17:e4229d77a5ab 348 // Kiri
rahmadirizki18 18:1da121ddb7c1 349 if (case_ger == caseSebelum) XT = XT - PERPINDAHAN;
rahmadirizki18 18:1da121ddb7c1 350
rahmadirizki18 17:e4229d77a5ab 351 break;
rahmadirizki18 17:e4229d77a5ab 352 }
rahmadirizki18 17:e4229d77a5ab 353 default :{}
rahmadirizki18 17:e4229d77a5ab 354 } //end of switch
rahmadirizki18 18:1da121ddb7c1 355 caseSebelum = case_ger;
rahmadirizki18 17:e4229d77a5ab 356 }
rahmadirizki18 5:3aa203218306 357 }
rahmadirizki18 5:3aa203218306 358
calmantara186 16:90119f03c5d1 359 void setCenter(){
calmantara186 16:90119f03c5d1 360 /* Fungsi untuk menentukan center dari robot */
rahmadirizki18 5:3aa203218306 361 encoderDepan.reset();
rahmadirizki18 5:3aa203218306 362 encoderBelakang.reset();
rahmadirizki18 5:3aa203218306 363 encoderKanan.reset();
rahmadirizki18 5:3aa203218306 364 encoderKiri.reset();
rahmadirizki18 5:3aa203218306 365 }
rahmadirizki18 5:3aa203218306 366
calmantara186 16:90119f03c5d1 367 float getX(){
calmantara186 16:90119f03c5d1 368 /* Fungsi untuk mendapatkan jarak X */
rahmadirizki18 5:3aa203218306 369 float jarakEncDpn, jarakEncBlk;
calmantara186 16:90119f03c5d1 370 jarakEncDpn = (encoderDepan.getPulses())/(float)(2000.0)*k_enc;
calmantara186 16:90119f03c5d1 371 jarakEncBlk = (encoderBelakang.getPulses())/(float)(2000.0)*k_enc;
rahmadirizki18 5:3aa203218306 372 return (jarakEncDpn-jarakEncBlk)/2;
rahmadirizki18 5:3aa203218306 373 }
rahmadirizki18 5:3aa203218306 374
calmantara186 16:90119f03c5d1 375 float getY(){
calmantara186 16:90119f03c5d1 376 /* Fungsi untuk mendapatkan jarak Y */
rahmadirizki18 5:3aa203218306 377 float jarakEncKir, jarakEncKan;
calmantara186 16:90119f03c5d1 378 jarakEncKir = (encoderKiri.getPulses())/(float)(2000.0)*k_enc;
calmantara186 16:90119f03c5d1 379 jarakEncKan = (encoderKanan.getPulses())/(float)(720.0)*k_enc;
rahmadirizki18 5:3aa203218306 380 return (jarakEncKir-jarakEncKan)/2;
rahmadirizki18 5:3aa203218306 381 }
rahmadirizki18 5:3aa203218306 382
calmantara186 16:90119f03c5d1 383 float getTetha(){
calmantara186 16:90119f03c5d1 384 /* Fungsi untuk mendapatkan nilai tetha */
rahmadirizki18 5:3aa203218306 385 float busurDpn, busurBlk, busurKir, busurKan;
calmantara186 16:90119f03c5d1 386 busurDpn = ((encoderDepan.getPulses())/(float)(2000.0)*k_enc)/k_robot*360.0;
calmantara186 16:90119f03c5d1 387 busurBlk = ((encoderBelakang.getPulses())/(float)(2000.0)*k_enc)/k_robot*360.0;
calmantara186 16:90119f03c5d1 388 busurKir = ((encoderKiri.getPulses())/(float)(2000.0)*k_enc)/k_robot*360.0;
calmantara186 16:90119f03c5d1 389 busurKan = ((encoderKanan.getPulses())/(float)(720.0)*k_enc)/k_robot*360.0;
rahmadirizki18 5:3aa203218306 390
rahmadirizki18 5:3aa203218306 391 return -(busurDpn+busurBlk+busurKir+busurKan)/4;
fanny868 0:9072e932503c 392 }
fanny868 0:9072e932503c 393
calmantara186 16:90119f03c5d1 394 void gotoXYT(float xa, float ya, float Ta){
calmantara186 16:90119f03c5d1 395 /* Fungsi untuk bergerat ke target */
calmantara186 16:90119f03c5d1 396 errX = xa-getX();
calmantara186 16:90119f03c5d1 397 Vx = kpX*errX;
calmantara186 16:90119f03c5d1 398
calmantara186 16:90119f03c5d1 399 errY = ya-getY();
calmantara186 16:90119f03c5d1 400 Vy = kpY*errY;
calmantara186 16:90119f03c5d1 401
calmantara186 16:90119f03c5d1 402 errT = Ta-getTetha();
calmantara186 16:90119f03c5d1 403 Vt = kp_tetha*errT;
calmantara186 16:90119f03c5d1 404
calmantara186 16:90119f03c5d1 405 v1 = Vx+Vy-Vt;
calmantara186 16:90119f03c5d1 406 v2 = Vx-Vy-Vt;
calmantara186 16:90119f03c5d1 407 v3 = -Vx-Vy-Vt;
calmantara186 16:90119f03c5d1 408 v4 = -Vx+Vy-Vt;
calmantara186 16:90119f03c5d1 409
calmantara186 16:90119f03c5d1 410 if (v1>speed1)
calmantara186 16:90119f03c5d1 411 { v1 = speed1; }
calmantara186 16:90119f03c5d1 412 else if (v1<-speed1)
calmantara186 16:90119f03c5d1 413 { v1 = -speed1; }
calmantara186 16:90119f03c5d1 414
calmantara186 16:90119f03c5d1 415 if (v2>speed2)
calmantara186 16:90119f03c5d1 416 { v2 = speed2; }
calmantara186 16:90119f03c5d1 417 else if (v2<-speed2)
calmantara186 16:90119f03c5d1 418 { v2 = -speed2; }
calmantara186 16:90119f03c5d1 419
calmantara186 16:90119f03c5d1 420 if (v3>speed3)
calmantara186 16:90119f03c5d1 421 { v3 = speed3; }
calmantara186 16:90119f03c5d1 422 else if (v3<-speed3)
calmantara186 16:90119f03c5d1 423 { v3 = -speed3; }
calmantara186 16:90119f03c5d1 424
calmantara186 16:90119f03c5d1 425 if (v4>speed4)
calmantara186 16:90119f03c5d1 426 { v4 = speed4; }
calmantara186 16:90119f03c5d1 427 else if (v4<-speed4)
calmantara186 16:90119f03c5d1 428 { v4 = -speed4; }
calmantara186 16:90119f03c5d1 429
calmantara186 16:90119f03c5d1 430 if (((errX > 0.05) || (errX<-0.05)) || ((errY > 0.05) || (errY<-0.05)) || ((errT > 0.05) || (errT<-0.05))){
calmantara186 16:90119f03c5d1 431 motor1.speed(v1);
calmantara186 16:90119f03c5d1 432 motor2.speed(v2);
calmantara186 16:90119f03c5d1 433 motor3.speed(v3);
calmantara186 16:90119f03c5d1 434 motor4.speed(v4);
calmantara186 16:90119f03c5d1 435 }
calmantara186 16:90119f03c5d1 436 else{
calmantara186 16:90119f03c5d1 437 motor1.brake(1);
calmantara186 16:90119f03c5d1 438 motor2.brake(1);
calmantara186 16:90119f03c5d1 439 motor3.brake(1);
calmantara186 16:90119f03c5d1 440 motor4.brake(1);
calmantara186 16:90119f03c5d1 441 }
calmantara186 16:90119f03c5d1 442 }
rahmadirizki18 5:3aa203218306 443
calmantara186 16:90119f03c5d1 444 void speedLauncher(){
calmantara186 16:90119f03c5d1 445 /* Fungsi untuk speed launcher */
franshendri 12:e07c59c28c29 446 if (joystick.R3_click and speedL < 0.8){
calmantara186 16:90119f03c5d1 447 speedL = speedL + 0.01;
calmantara186 16:90119f03c5d1 448 }
rahmadirizki18 7:d138c56dab20 449 if (joystick.L3_click and speedL > 0.1){
calmantara186 16:90119f03c5d1 450 speedL = speedL - 0.01;
calmantara186 16:90119f03c5d1 451 }
rahmadirizki18 13:8ab42383a2ca 452 if (joystick.R2_click and speedB < 0.8 ){
calmantara186 16:90119f03c5d1 453 speedB = speedB + 0.01;
calmantara186 16:90119f03c5d1 454 }
rahmadirizki18 13:8ab42383a2ca 455 if (joystick.L2_click and speedB > 0.1 ){
calmantara186 16:90119f03c5d1 456 speedB = speedB - 0.01;
calmantara186 16:90119f03c5d1 457 }
rahmadirizki18 7:d138c56dab20 458 }
franshendri 12:e07c59c28c29 459
calmantara186 16:90119f03c5d1 460 /***********************************************/
calmantara186 16:90119f03c5d1 461 /* Main Function */
calmantara186 16:90119f03c5d1 462 /***********************************************/
calmantara186 16:90119f03c5d1 463
calmantara186 16:90119f03c5d1 464 int main (void){
calmantara186 16:90119f03c5d1 465 /* Set baud rate - 115200 */
fanny868 0:9072e932503c 466 joystick.setup();
rahmadirizki18 6:68293bed71ea 467 wait_ms(1000);
rahmadirizki18 5:3aa203218306 468 setCenter();
rahmadirizki18 5:3aa203218306 469 wait_ms(500);
rahmadirizki18 5:3aa203218306 470
calmantara186 16:90119f03c5d1 471 /* Posisi Awal */
rahmadirizki18 5:3aa203218306 472 XT = 0;
rahmadirizki18 5:3aa203218306 473 YT = 0;
rahmadirizki18 5:3aa203218306 474 Tetha = 0;
calmantara186 16:90119f03c5d1 475
calmantara186 16:90119f03c5d1 476 /* Untuk mendapatkan serial dari Arduino */
fanny868 0:9072e932503c 477 while(1)
fanny868 0:9072e932503c 478 {
fanny868 0:9072e932503c 479 // Interrupt Serial
calmantara186 16:90119f03c5d1 480 joystick.idle();
calmantara186 16:90119f03c5d1 481
calmantara186 16:90119f03c5d1 482 if(joystick.readable() ) {
fanny868 0:9072e932503c 483 // Panggil fungsi pembacaan joystik
fanny868 0:9072e932503c 484 joystick.baca_data();
calmantara186 16:90119f03c5d1 485
fanny868 0:9072e932503c 486 // Panggil fungsi pengolahan data joystik
fanny868 0:9072e932503c 487 joystick.olah_data();
calmantara186 16:90119f03c5d1 488
calmantara186 16:90119f03c5d1 489 // Masuk ke case gerak
fanny868 0:9072e932503c 490 case_ger = case_gerak();
rahmadirizki18 3:1287fccc11be 491 aktuator();
calmantara186 16:90119f03c5d1 492
rahmadirizki18 17:e4229d77a5ab 493 if (joystick.segitiga_click) launcher = !launcher;
rahmadirizki18 17:e4229d77a5ab 494 if (joystick.lingkaran_click) servoGo = true;
rahmadirizki18 17:e4229d77a5ab 495 if (joystick.SELECT_click) manual = !manual;
rahmadirizki18 5:3aa203218306 496 if (joystick.silang) {
rahmadirizki18 5:3aa203218306 497 XT = 0;
rahmadirizki18 5:3aa203218306 498 YT = 0;
rahmadirizki18 5:3aa203218306 499 Tetha = 0;
calmantara186 16:90119f03c5d1 500 }
rahmadirizki18 7:d138c56dab20 501 speedLauncher();
franshendri 12:e07c59c28c29 502
calmantara186 16:90119f03c5d1 503 }
calmantara186 16:90119f03c5d1 504 else {
calmantara186 16:90119f03c5d1 505 joystick.idle();
fanny868 0:9072e932503c 506 }
rahmadirizki18 5:3aa203218306 507 gotoXYT(XT,YT,Tetha);
rahmadirizki18 5:3aa203218306 508
fanny868 0:9072e932503c 509 }
fanny868 0:9072e932503c 510 }