KRAI 2017 / Mbed 2 deprecated MainProgram_BaseBaru_otomatis-reloader

bug : pwm full di launcher

Dependencies:   Motor PID Joystick_OrdoV5 mbed millis

Fork of MainProgram_BaseBaru by KRAI 2017

main.cpp@21:da2f3d04468f, 2017-01-28 (annotated)

Committer:
MarchioKevin
Date:
Sat Jan 28 07:24:07 2017 +0000
Revision:
21:da2f3d04468f
Parent:
20:54dc93e7b016
Child:
22:4632f58461e0
Optimasi, Kontrol semi-otomatis fixed

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 21:da2f3d04468f 67 encoderKRAI encoderDepan( PB_13, PB_14, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
MarchioKevin 21:da2f3d04468f 68 encoderKRAI encoderBelakang( PC_11, PC_10, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
MarchioKevin 21:da2f3d04468f 69 encoderKRAI encoderKanan( PC_12, PD_2, 720, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
MarchioKevin 21:da2f3d04468f 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
MarchioKevin 21:da2f3d04468f 341 if (case_ger != caseSebelum)
MarchioKevin 21:da2f3d04468f 342 XT = XT + PERPINDAHAN;
rahmadirizki18 3:1287fccc11be 343 break;
rahmadirizki18 17:e4229d77a5ab 344 }
rahmadirizki18 17:e4229d77a5ab 345 case (10) :{
rahmadirizki18 17:e4229d77a5ab 346 // Kiri
MarchioKevin 21:da2f3d04468f 347 if (case_ger != caseSebelum)
MarchioKevin 21:da2f3d04468f 348 XT = XT - PERPINDAHAN;
rahmadirizki18 17:e4229d77a5ab 349 break;
rahmadirizki18 17:e4229d77a5ab 350 }
rahmadirizki18 17:e4229d77a5ab 351 default :{}
MarchioKevin 20:54dc93e7b016 352 } // End of Switch
rahmadirizki18 18:1da121ddb7c1 353 caseSebelum = case_ger;
rahmadirizki18 17:e4229d77a5ab 354 }
rahmadirizki18 5:3aa203218306 355 }
rahmadirizki18 5:3aa203218306 356
calmantara186 16:90119f03c5d1 357 void setCenter(){
calmantara186 16:90119f03c5d1 358 /* Fungsi untuk menentukan center dari robot */
rahmadirizki18 5:3aa203218306 359 encoderDepan.reset();
rahmadirizki18 5:3aa203218306 360 encoderBelakang.reset();
rahmadirizki18 5:3aa203218306 361 encoderKanan.reset();
rahmadirizki18 5:3aa203218306 362 encoderKiri.reset();
rahmadirizki18 5:3aa203218306 363 }
rahmadirizki18 5:3aa203218306 364
calmantara186 16:90119f03c5d1 365 float getX(){
calmantara186 16:90119f03c5d1 366 /* Fungsi untuk mendapatkan jarak X */
rahmadirizki18 5:3aa203218306 367 float jarakEncDpn, jarakEncBlk;
calmantara186 16:90119f03c5d1 368 jarakEncDpn = (encoderDepan.getPulses())/(float)(2000.0)*k_enc;
calmantara186 16:90119f03c5d1 369 jarakEncBlk = (encoderBelakang.getPulses())/(float)(2000.0)*k_enc;
rahmadirizki18 5:3aa203218306 370 return (jarakEncDpn-jarakEncBlk)/2;
rahmadirizki18 5:3aa203218306 371 }
rahmadirizki18 5:3aa203218306 372
calmantara186 16:90119f03c5d1 373 float getY(){
calmantara186 16:90119f03c5d1 374 /* Fungsi untuk mendapatkan jarak Y */
rahmadirizki18 5:3aa203218306 375 float jarakEncKir, jarakEncKan;
calmantara186 16:90119f03c5d1 376 jarakEncKir = (encoderKiri.getPulses())/(float)(2000.0)*k_enc;
calmantara186 16:90119f03c5d1 377 jarakEncKan = (encoderKanan.getPulses())/(float)(720.0)*k_enc;
rahmadirizki18 5:3aa203218306 378 return (jarakEncKir-jarakEncKan)/2;
rahmadirizki18 5:3aa203218306 379 }
rahmadirizki18 5:3aa203218306 380
calmantara186 16:90119f03c5d1 381 float getTetha(){
calmantara186 16:90119f03c5d1 382 /* Fungsi untuk mendapatkan nilai tetha */
rahmadirizki18 5:3aa203218306 383 float busurDpn, busurBlk, busurKir, busurKan;
calmantara186 16:90119f03c5d1 384 busurDpn = ((encoderDepan.getPulses())/(float)(2000.0)*k_enc)/k_robot*360.0;
calmantara186 16:90119f03c5d1 385 busurBlk = ((encoderBelakang.getPulses())/(float)(2000.0)*k_enc)/k_robot*360.0;
calmantara186 16:90119f03c5d1 386 busurKir = ((encoderKiri.getPulses())/(float)(2000.0)*k_enc)/k_robot*360.0;
calmantara186 16:90119f03c5d1 387 busurKan = ((encoderKanan.getPulses())/(float)(720.0)*k_enc)/k_robot*360.0;
rahmadirizki18 5:3aa203218306 388
rahmadirizki18 5:3aa203218306 389 return -(busurDpn+busurBlk+busurKir+busurKan)/4;
fanny868 0:9072e932503c 390 }
fanny868 0:9072e932503c 391
calmantara186 16:90119f03c5d1 392 void gotoXYT(float xa, float ya, float Ta){
calmantara186 16:90119f03c5d1 393 /* Fungsi untuk bergerat ke target */
calmantara186 16:90119f03c5d1 394 errX = xa-getX();
calmantara186 16:90119f03c5d1 395 Vx = kpX*errX;
calmantara186 16:90119f03c5d1 396
calmantara186 16:90119f03c5d1 397 errY = ya-getY();
calmantara186 16:90119f03c5d1 398 Vy = kpY*errY;
calmantara186 16:90119f03c5d1 399
calmantara186 16:90119f03c5d1 400 errT = Ta-getTetha();
calmantara186 16:90119f03c5d1 401 Vt = kp_tetha*errT;
calmantara186 16:90119f03c5d1 402
calmantara186 16:90119f03c5d1 403 v1 = Vx+Vy-Vt;
calmantara186 16:90119f03c5d1 404 v2 = Vx-Vy-Vt;
calmantara186 16:90119f03c5d1 405 v3 = -Vx-Vy-Vt;
calmantara186 16:90119f03c5d1 406 v4 = -Vx+Vy-Vt;
calmantara186 16:90119f03c5d1 407
calmantara186 16:90119f03c5d1 408 if (v1>speed1)
calmantara186 16:90119f03c5d1 409 { v1 = speed1; }
calmantara186 16:90119f03c5d1 410 else if (v1<-speed1)
calmantara186 16:90119f03c5d1 411 { v1 = -speed1; }
calmantara186 16:90119f03c5d1 412
calmantara186 16:90119f03c5d1 413 if (v2>speed2)
calmantara186 16:90119f03c5d1 414 { v2 = speed2; }
calmantara186 16:90119f03c5d1 415 else if (v2<-speed2)
calmantara186 16:90119f03c5d1 416 { v2 = -speed2; }
calmantara186 16:90119f03c5d1 417
calmantara186 16:90119f03c5d1 418 if (v3>speed3)
calmantara186 16:90119f03c5d1 419 { v3 = speed3; }
calmantara186 16:90119f03c5d1 420 else if (v3<-speed3)
calmantara186 16:90119f03c5d1 421 { v3 = -speed3; }
calmantara186 16:90119f03c5d1 422
calmantara186 16:90119f03c5d1 423 if (v4>speed4)
calmantara186 16:90119f03c5d1 424 { v4 = speed4; }
calmantara186 16:90119f03c5d1 425 else if (v4<-speed4)
calmantara186 16:90119f03c5d1 426 { v4 = -speed4; }
calmantara186 16:90119f03c5d1 427
calmantara186 16:90119f03c5d1 428 if (((errX > 0.05) || (errX<-0.05)) || ((errY > 0.05) || (errY<-0.05)) || ((errT > 0.05) || (errT<-0.05))){
calmantara186 16:90119f03c5d1 429 motor1.speed(v1);
calmantara186 16:90119f03c5d1 430 motor2.speed(v2);
calmantara186 16:90119f03c5d1 431 motor3.speed(v3);
calmantara186 16:90119f03c5d1 432 motor4.speed(v4);
calmantara186 16:90119f03c5d1 433 }
calmantara186 16:90119f03c5d1 434 else{
calmantara186 16:90119f03c5d1 435 motor1.brake(1);
calmantara186 16:90119f03c5d1 436 motor2.brake(1);
calmantara186 16:90119f03c5d1 437 motor3.brake(1);
calmantara186 16:90119f03c5d1 438 motor4.brake(1);
calmantara186 16:90119f03c5d1 439 }
calmantara186 16:90119f03c5d1 440 }
rahmadirizki18 5:3aa203218306 441
calmantara186 16:90119f03c5d1 442 void speedLauncher(){
calmantara186 16:90119f03c5d1 443 /* Fungsi untuk speed launcher */
franshendri 12:e07c59c28c29 444 if (joystick.R3_click and speedL < 0.8){
calmantara186 16:90119f03c5d1 445 speedL = speedL + 0.01;
calmantara186 16:90119f03c5d1 446 }
rahmadirizki18 7:d138c56dab20 447 if (joystick.L3_click and speedL > 0.1){
calmantara186 16:90119f03c5d1 448 speedL = speedL - 0.01;
calmantara186 16:90119f03c5d1 449 }
rahmadirizki18 13:8ab42383a2ca 450 if (joystick.R2_click and speedB < 0.8 ){
calmantara186 16:90119f03c5d1 451 speedB = speedB + 0.01;
calmantara186 16:90119f03c5d1 452 }
rahmadirizki18 13:8ab42383a2ca 453 if (joystick.L2_click and speedB > 0.1 ){
calmantara186 16:90119f03c5d1 454 speedB = speedB - 0.01;
calmantara186 16:90119f03c5d1 455 }
rahmadirizki18 7:d138c56dab20 456 }
franshendri 12:e07c59c28c29 457
calmantara186 16:90119f03c5d1 458 /***********************************************/
calmantara186 16:90119f03c5d1 459 /* Main Function */
calmantara186 16:90119f03c5d1 460 /***********************************************/
calmantara186 16:90119f03c5d1 461
calmantara186 16:90119f03c5d1 462 int main (void){
calmantara186 16:90119f03c5d1 463 /* Set baud rate - 115200 */
fanny868 0:9072e932503c 464 joystick.setup();
rahmadirizki18 6:68293bed71ea 465 wait_ms(1000);
rahmadirizki18 5:3aa203218306 466 setCenter();
rahmadirizki18 5:3aa203218306 467 wait_ms(500);
rahmadirizki18 5:3aa203218306 468
calmantara186 16:90119f03c5d1 469 /* Posisi Awal */
rahmadirizki18 5:3aa203218306 470 XT = 0;
rahmadirizki18 5:3aa203218306 471 YT = 0;
rahmadirizki18 5:3aa203218306 472 Tetha = 0;
calmantara186 16:90119f03c5d1 473
calmantara186 16:90119f03c5d1 474 /* Untuk mendapatkan serial dari Arduino */
fanny868 0:9072e932503c 475 while(1)
fanny868 0:9072e932503c 476 {
fanny868 0:9072e932503c 477 // Interrupt Serial
calmantara186 16:90119f03c5d1 478 joystick.idle();
calmantara186 16:90119f03c5d1 479
calmantara186 16:90119f03c5d1 480 if(joystick.readable() ) {
fanny868 0:9072e932503c 481 // Panggil fungsi pembacaan joystik
fanny868 0:9072e932503c 482 joystick.baca_data();
calmantara186 16:90119f03c5d1 483
fanny868 0:9072e932503c 484 // Panggil fungsi pengolahan data joystik
fanny868 0:9072e932503c 485 joystick.olah_data();
calmantara186 16:90119f03c5d1 486
calmantara186 16:90119f03c5d1 487 // Masuk ke case gerak
fanny868 0:9072e932503c 488 case_ger = case_gerak();
rahmadirizki18 3:1287fccc11be 489 aktuator();
calmantara186 16:90119f03c5d1 490
rahmadirizki18 17:e4229d77a5ab 491 if (joystick.segitiga_click) launcher = !launcher;
rahmadirizki18 17:e4229d77a5ab 492 if (joystick.lingkaran_click) servoGo = true;
rahmadirizki18 17:e4229d77a5ab 493 if (joystick.SELECT_click) manual = !manual;
rahmadirizki18 5:3aa203218306 494 if (joystick.silang) {
rahmadirizki18 5:3aa203218306 495 XT = 0;
rahmadirizki18 5:3aa203218306 496 YT = 0;
rahmadirizki18 5:3aa203218306 497 Tetha = 0;
calmantara186 16:90119f03c5d1 498 }
rahmadirizki18 7:d138c56dab20 499 speedLauncher();
calmantara186 16:90119f03c5d1 500 }
calmantara186 16:90119f03c5d1 501 else {
calmantara186 16:90119f03c5d1 502 joystick.idle();
MarchioKevin 21:da2f3d04468f 503 }
MarchioKevin 21:da2f3d04468f 504 if (!manual)
rahmadirizki18 5:3aa203218306 505 gotoXYT(XT,YT,Tetha);
fanny868 0:9072e932503c 506 }
fanny868 0:9072e932503c 507 }