KRAI 2017 / Mbed 2 deprecated Joystick_ManualBaseBaruV2_PIDlauncher

Rancha mark_II

Dependencies:   Motor PID Joystick_OrdoV5 mbed millis

Fork of Joystick_ManualBaseBaruV2_enc by KRAI 2017

main.cpp@20:54dc93e7b016, 2017-01-28 (annotated)

Committer:
MarchioKevin
Date:
Sat Jan 28 04:33:39 2017 +0000
Revision:
20:54dc93e7b016
Parent:
19:38f148ce00f0
Child:
21:da2f3d04468f
Mode encoder sumbu X: Perpindahan; Bersih-bersih kodinga wkwk

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