KRAI 2017 / Mbed 2 deprecated MainProgram_BaseBaru_17feb

Update 18 Februari 2017, PID laucnher dan Base sudah diperbarui

Dependencies:   Motor PID Joystick_OrdoV5 mbed millis

Fork of MainProgram_BaseBaru_otomatis-reloader by KRAI 2017

main.cpp@17:e4229d77a5ab, 2017-01-27 (annotated)

Committer:
rahmadirizki18
Date:
Fri Jan 27 14:42:17 2017 +0000
Revision:
17:e4229d77a5ab
Parent:
16:90119f03c5d1
Child:
18:1da121ddb7c1
tambahan mode manual dan semiautomatis, belum dioptimasi

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 17:e4229d77a5ab 110 bool launcher = false, servoGo = false, manual = true, caseTadi = false, caseSekarang = false;
fanny868 0:9072e932503c 111
calmantara186 16:90119f03c5d1 112 /***********************************************/
calmantara186 16:90119f03c5d1 113 /* Deklarasi Fungsi dan Procedure */
calmantara186 16:90119f03c5d1 114 /***********************************************/
calmantara186 16:90119f03c5d1 115 int case_gerak(){
calmantara186 16:90119f03c5d1 116 /*****************************************************
calmantara186 16:90119f03c5d1 117 ** Gerak Motor Base
calmantara186 16:90119f03c5d1 118 ** Case 1 : Pivot kanan
calmantara186 16:90119f03c5d1 119 ** Case 2 : Pivot Kiri
calmantara186 16:90119f03c5d1 120 ** Case 3 : Maju
calmantara186 16:90119f03c5d1 121 ** Case 4 : Mundur
calmantara186 16:90119f03c5d1 122 ** Case 5 : Serong Atas Kanan
calmantara186 16:90119f03c5d1 123 ** Case 6 : Serong Bawah Kanan
calmantara186 16:90119f03c5d1 124 ** Case 7 : Serong Atas Kiri
calmantara186 16:90119f03c5d1 125 ** Case 8 : Serong Bawah Kiri
calmantara186 16:90119f03c5d1 126 ** Case 9 : Kanan
calmantara186 16:90119f03c5d1 127 ** Case 10 : Kiri
calmantara186 16:90119f03c5d1 128 ** Case 12 : break
calmantara186 16:90119f03c5d1 129 ****************************************************/
fanny868 0:9072e932503c 130 int casegerak;
calmantara186 16:90119f03c5d1 131 if (!joystick.L1 && joystick.R1) {
fanny868 0:9072e932503c 132 // Pivot Kanan
fanny868 0:9072e932503c 133 casegerak = 1;
calmantara186 16:90119f03c5d1 134 }
calmantara186 16:90119f03c5d1 135 else if (!joystick.R1 && joystick.L1) {
fanny868 0:9072e932503c 136 // Pivot Kiri
fanny868 0:9072e932503c 137 casegerak = 2;
calmantara186 16:90119f03c5d1 138 }
calmantara186 16:90119f03c5d1 139 else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
fanny868 0:9072e932503c 140 // Maju
rahmadirizki18 3:1287fccc11be 141 casegerak = 3;
calmantara186 16:90119f03c5d1 142 }
calmantara186 16:90119f03c5d1 143 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
fanny868 0:9072e932503c 144 // Mundur
fanny868 0:9072e932503c 145 casegerak = 4;
calmantara186 16:90119f03c5d1 146 }
calmantara186 16:90119f03c5d1 147 else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan)) {
fanny868 0:9072e932503c 148 // Serong Atas Kanan
fanny868 0:9072e932503c 149 casegerak = 5;
calmantara186 16:90119f03c5d1 150 }
calmantara186 16:90119f03c5d1 151 else if((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan)) {
fanny868 0:9072e932503c 152 // Serong Bawah Kanan
fanny868 0:9072e932503c 153 casegerak = 6;
calmantara186 16:90119f03c5d1 154 }
calmantara186 16:90119f03c5d1 155 else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)) {
fanny868 0:9072e932503c 156 // Serong Atas Kiri
fanny868 0:9072e932503c 157 casegerak = 7;
calmantara186 16:90119f03c5d1 158 }
calmantara186 16:90119f03c5d1 159 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)) {
fanny868 0:9072e932503c 160 // Serong Bawah Kiri
fanny868 0:9072e932503c 161 casegerak = 8;
calmantara186 16:90119f03c5d1 162 }
calmantara186 16:90119f03c5d1 163 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
fanny868 0:9072e932503c 164 // Kanan
fanny868 0:9072e932503c 165 casegerak = 9;
rahmadirizki18 17:e4229d77a5ab 166 caseSekarang = true;
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 17:e4229d77a5ab 170 casegerak = 10;
rahmadirizki18 17:e4229d77a5ab 171 caseSekarang = true;
calmantara186 16:90119f03c5d1 172 }
calmantara186 16:90119f03c5d1 173 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
calmantara186 16:90119f03c5d1 174 casegerak = 12;
calmantara186 16:90119f03c5d1 175 }
fanny868 0:9072e932503c 176 return(casegerak);
fanny868 0:9072e932503c 177 }
fanny868 0:9072e932503c 178
calmantara186 16:90119f03c5d1 179 void aktuator(){
calmantara186 16:90119f03c5d1 180 /* Fungsi untuk menggerakkan servo */
calmantara186 16:90119f03c5d1 181 // Servo
calmantara186 16:90119f03c5d1 182 if (servoGo){
franshendri 12:e07c59c28c29 183 servoS.position(20);
franshendri 10:f0f0dc3904e0 184 wait_ms(500);
franshendri 12:e07c59c28c29 185 servoS.position(-28);
Joshua23 8:0711dea61312 186 wait_ms(500);
franshendri 12:e07c59c28c29 187 servoS.position(20);
Joshua23 8:0711dea61312 188 wait_ms(500);
franshendri 12:e07c59c28c29 189 for (int i = -0; i<=70; i++){
Joshua23 8:0711dea61312 190 servoB.position(i);
Joshua23 8:0711dea61312 191 wait_ms(10);
Joshua23 8:0711dea61312 192 }
Joshua23 8:0711dea61312 193 wait_ms(500);
Joshua23 8:0711dea61312 194 servoB.position(0);
calmantara186 16:90119f03c5d1 195 servoGo = false;
calmantara186 16:90119f03c5d1 196 }
calmantara186 16:90119f03c5d1 197 else{
franshendri 12:e07c59c28c29 198 servoS.position(20);
rahmadirizki18 6:68293bed71ea 199 servoB.position(0);
rahmadirizki18 7:d138c56dab20 200 }
rahmadirizki18 6:68293bed71ea 201
rahmadirizki18 6:68293bed71ea 202 // Motor Atas
calmantara186 16:90119f03c5d1 203 if (launcher) {
rahmadirizki18 7:d138c56dab20 204 motorld.speed(speedL);
franshendri 12:e07c59c28c29 205 motorlb.speed(speedB);
rahmadirizki18 6:68293bed71ea 206 }else{
rahmadirizki18 6:68293bed71ea 207 motorld.speed(0);
rahmadirizki18 6:68293bed71ea 208 motorlb.speed(0);
rahmadirizki18 6:68293bed71ea 209 }
rahmadirizki18 6:68293bed71ea 210
rahmadirizki18 6:68293bed71ea 211 // MOTOR Bawah
rahmadirizki18 17:e4229d77a5ab 212 if (manual) {
rahmadirizki18 17:e4229d77a5ab 213 // Mode Manual
rahmadirizki18 17:e4229d77a5ab 214 switch (case_ger) {
rahmadirizki18 17:e4229d77a5ab 215 case (1): {
rahmadirizki18 17:e4229d77a5ab 216 // Pivot Kanan
rahmadirizki18 17:e4229d77a5ab 217 motor1.speed(-PIVOT);
rahmadirizki18 17:e4229d77a5ab 218 motor2.speed(-PIVOT);
rahmadirizki18 17:e4229d77a5ab 219 motor3.speed(-PIVOT);
rahmadirizki18 17:e4229d77a5ab 220 motor4.speed(-PIVOT);
fanny868 0:9072e932503c 221 break;
rahmadirizki18 17:e4229d77a5ab 222 }
rahmadirizki18 17:e4229d77a5ab 223 case (2): {
rahmadirizki18 17:e4229d77a5ab 224 // Pivot Kiri
rahmadirizki18 17:e4229d77a5ab 225 motor1.speed(PIVOT);
rahmadirizki18 17:e4229d77a5ab 226 motor2.speed(PIVOT);
rahmadirizki18 17:e4229d77a5ab 227 motor3.speed(PIVOT);
rahmadirizki18 17:e4229d77a5ab 228 motor4.speed(PIVOT);
rahmadirizki18 17:e4229d77a5ab 229 break;
rahmadirizki18 17:e4229d77a5ab 230 }
rahmadirizki18 17:e4229d77a5ab 231 case (3): {
rahmadirizki18 17:e4229d77a5ab 232 // Maju
rahmadirizki18 17:e4229d77a5ab 233 motor1.speed(-VMAX);
rahmadirizki18 17:e4229d77a5ab 234 motor2.speed(VMAX);
rahmadirizki18 17:e4229d77a5ab 235 motor3.speed(VMAX);
rahmadirizki18 17:e4229d77a5ab 236 motor4.speed(-VMAX);
fanny868 0:9072e932503c 237 break;
fanny868 0:9072e932503c 238 }
rahmadirizki18 17:e4229d77a5ab 239 case (4): {
rahmadirizki18 17:e4229d77a5ab 240 // Mundur
rahmadirizki18 17:e4229d77a5ab 241 motor1.speed(VMAX);
rahmadirizki18 17:e4229d77a5ab 242 motor2.speed(-VMAX);
rahmadirizki18 17:e4229d77a5ab 243 motor3.speed(-VMAX);
rahmadirizki18 17:e4229d77a5ab 244 motor4.speed(VMAX);
rahmadirizki18 17:e4229d77a5ab 245 break;
rahmadirizki18 17:e4229d77a5ab 246 }
rahmadirizki18 17:e4229d77a5ab 247 case (5) : {
rahmadirizki18 17:e4229d77a5ab 248 // Samping Atas Kanan
rahmadirizki18 17:e4229d77a5ab 249 motor1.speed(-SAMPING);
rahmadirizki18 17:e4229d77a5ab 250 motor2.brake(1);
rahmadirizki18 17:e4229d77a5ab 251 motor3.speed(SAMPING);
rahmadirizki18 17:e4229d77a5ab 252 motor4.brake(1);
fanny868 0:9072e932503c 253 break;
fanny868 0:9072e932503c 254 }
rahmadirizki18 17:e4229d77a5ab 255 case (6) : {
rahmadirizki18 17:e4229d77a5ab 256 // Samping Bawah Kanan
rahmadirizki18 17:e4229d77a5ab 257 motor1.brake(1);
rahmadirizki18 17:e4229d77a5ab 258 motor2.speed(-SAMPING);
rahmadirizki18 17:e4229d77a5ab 259 motor3.brake(1);
rahmadirizki18 17:e4229d77a5ab 260 motor4.speed(SAMPING);
fanny868 0:9072e932503c 261 break;
fanny868 0:9072e932503c 262 }
rahmadirizki18 17:e4229d77a5ab 263 case (7) : {
rahmadirizki18 17:e4229d77a5ab 264 // Samping Atas Kiri
rahmadirizki18 17:e4229d77a5ab 265 motor1.brake(1);
rahmadirizki18 17:e4229d77a5ab 266 motor2.speed(SAMPING);
rahmadirizki18 17:e4229d77a5ab 267 motor3.brake(1);
rahmadirizki18 17:e4229d77a5ab 268 motor4.speed(-SAMPING);
rahmadirizki18 17:e4229d77a5ab 269 break;
rahmadirizki18 17:e4229d77a5ab 270 }
rahmadirizki18 17:e4229d77a5ab 271 case (8) : {
rahmadirizki18 17:e4229d77a5ab 272 // Samping Bawah Kiri
rahmadirizki18 17:e4229d77a5ab 273 motor1.speed(SAMPING);
rahmadirizki18 17:e4229d77a5ab 274 motor2.brake(1);
rahmadirizki18 17:e4229d77a5ab 275 motor3.speed(-SAMPING);
rahmadirizki18 17:e4229d77a5ab 276 motor4.brake(1);
fanny868 0:9072e932503c 277 break;
fanny868 0:9072e932503c 278 }
rahmadirizki18 17:e4229d77a5ab 279 case (9) : {
rahmadirizki18 17:e4229d77a5ab 280 // Kanan
rahmadirizki18 17:e4229d77a5ab 281 motor1.speed(-VMAX);
rahmadirizki18 17:e4229d77a5ab 282 motor2.speed(-VMAX);
rahmadirizki18 17:e4229d77a5ab 283 motor3.speed(VMAX);
rahmadirizki18 17:e4229d77a5ab 284 motor4.speed(VMAX);
fanny868 0:9072e932503c 285 break;
fanny868 0:9072e932503c 286 }
rahmadirizki18 17:e4229d77a5ab 287 case (10) : {
rahmadirizki18 17:e4229d77a5ab 288 // Kiri
rahmadirizki18 17:e4229d77a5ab 289 motor1.speed(VMAX);
rahmadirizki18 17:e4229d77a5ab 290 motor2.speed(VMAX);
rahmadirizki18 17:e4229d77a5ab 291 motor3.speed(-VMAX);
rahmadirizki18 17:e4229d77a5ab 292 motor4.speed(-VMAX);
fanny868 0:9072e932503c 293 break;
rahmadirizki18 17:e4229d77a5ab 294 }
rahmadirizki18 17:e4229d77a5ab 295 default : {
rahmadirizki18 17:e4229d77a5ab 296 motor1.brake(1);
rahmadirizki18 17:e4229d77a5ab 297 motor2.brake(1);
rahmadirizki18 17:e4229d77a5ab 298 motor3.brake(1);
rahmadirizki18 17:e4229d77a5ab 299 motor4.brake(1);
rahmadirizki18 17:e4229d77a5ab 300 }
rahmadirizki18 17:e4229d77a5ab 301 } // End Switch
rahmadirizki18 17:e4229d77a5ab 302 } else {
rahmadirizki18 17:e4229d77a5ab 303 //Mode Encoder
rahmadirizki18 17:e4229d77a5ab 304 switch (case_ger) {
rahmadirizki18 17:e4229d77a5ab 305 case (1):{
rahmadirizki18 17:e4229d77a5ab 306 Tetha = Tetha - 0.05;
rahmadirizki18 17:e4229d77a5ab 307 break;
rahmadirizki18 17:e4229d77a5ab 308 }
rahmadirizki18 17:e4229d77a5ab 309 case (2):{
rahmadirizki18 17:e4229d77a5ab 310 Tetha = Tetha + 0.05;
rahmadirizki18 17:e4229d77a5ab 311 break;
rahmadirizki18 17:e4229d77a5ab 312 }
rahmadirizki18 17:e4229d77a5ab 313 case (3):{
rahmadirizki18 17:e4229d77a5ab 314 YT = YT + 0.01;
rahmadirizki18 17:e4229d77a5ab 315 break;
rahmadirizki18 17:e4229d77a5ab 316 }
rahmadirizki18 17:e4229d77a5ab 317 case (4):{
rahmadirizki18 17:e4229d77a5ab 318 YT = YT - 0.01;
rahmadirizki18 17:e4229d77a5ab 319 break;
rahmadirizki18 17:e4229d77a5ab 320 }
rahmadirizki18 17:e4229d77a5ab 321 case (5) :{
rahmadirizki18 17:e4229d77a5ab 322 XT = XT + 0.01;
rahmadirizki18 17:e4229d77a5ab 323 YT = YT + 0.01;
rahmadirizki18 17:e4229d77a5ab 324 break;
rahmadirizki18 17:e4229d77a5ab 325 }
rahmadirizki18 17:e4229d77a5ab 326 case (6) :{
rahmadirizki18 17:e4229d77a5ab 327 XT = XT + 0.01;
rahmadirizki18 17:e4229d77a5ab 328 YT = YT - 0.01;
rahmadirizki18 17:e4229d77a5ab 329 break;
rahmadirizki18 17:e4229d77a5ab 330 }
rahmadirizki18 17:e4229d77a5ab 331 case (7) :{
rahmadirizki18 17:e4229d77a5ab 332 XT = XT - 0.01;
rahmadirizki18 17:e4229d77a5ab 333 YT = YT + 0.01;
rahmadirizki18 17:e4229d77a5ab 334 break;
rahmadirizki18 17:e4229d77a5ab 335 }
rahmadirizki18 17:e4229d77a5ab 336 case (8) :{
rahmadirizki18 17:e4229d77a5ab 337 XT = XT - 0.01;
rahmadirizki18 17:e4229d77a5ab 338 YT = YT - 0.01;
rahmadirizki18 17:e4229d77a5ab 339 break;
rahmadirizki18 17:e4229d77a5ab 340 }
rahmadirizki18 17:e4229d77a5ab 341 case (9) :{
rahmadirizki18 17:e4229d77a5ab 342 // Kanan
rahmadirizki18 17:e4229d77a5ab 343 if (caseSekarang != caseTadi) XT = XT + PERPINDAHAN;
rahmadirizki18 17:e4229d77a5ab 344 caseTadi = caseSekarang;
rahmadirizki18 17:e4229d77a5ab 345
rahmadirizki18 3:1287fccc11be 346 break;
rahmadirizki18 17:e4229d77a5ab 347 }
rahmadirizki18 17:e4229d77a5ab 348 case (10) :{
rahmadirizki18 17:e4229d77a5ab 349 // Kiri
rahmadirizki18 17:e4229d77a5ab 350 if (caseSekarang!= caseTadi) XT = XT - PERPINDAHAN;
rahmadirizki18 17:e4229d77a5ab 351 caseTadi = caseSekarang;
rahmadirizki18 17:e4229d77a5ab 352 break;
rahmadirizki18 17:e4229d77a5ab 353 }
rahmadirizki18 17:e4229d77a5ab 354 default :{}
rahmadirizki18 17:e4229d77a5ab 355 } //end of switch
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 }