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@16:90119f03c5d1, 2017-01-24 (annotated)

Committer:
calmantara186
Date:
Tue Jan 24 12:34:29 2017 +0000
Revision:
16:90119f03c5d1
Parent:
15:98f0d56b14f0
Child:
17:e4229d77a5ab
insyaAllah Bersih (ok)

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 5:3aa203218306 50
calmantara186 16:90119f03c5d1 51 float k_enc = PI*D_ENCODER;
calmantara186 16:90119f03c5d1 52 float k_robot = PI*D_ROBOT;
rahmadirizki18 5:3aa203218306 53
calmantara186 16:90119f03c5d1 54 float speed1 =0.6;
calmantara186 16:90119f03c5d1 55 float speed2 =0.6;
calmantara186 16:90119f03c5d1 56 float speed3 =0.6;
calmantara186 16:90119f03c5d1 57 float speed4 =0.6;
calmantara186 16:90119f03c5d1 58 float speedB =0.43;
calmantara186 16:90119f03c5d1 59 float speedL =0.4;
rahmadirizki18 5:3aa203218306 60
calmantara186 16:90119f03c5d1 61 float kpX=0.5, kpY=0.5, kp_tetha=0.03;
calmantara186 16:90119f03c5d1 62
calmantara186 16:90119f03c5d1 63 /* Deklarasi encoder */
calmantara186 16:90119f03c5d1 64 encoderKRAI encoderDepan( PB_13, PB_14, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
calmantara186 16:90119f03c5d1 65 encoderKRAI encoderBelakang( PC_11, PC_10, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
calmantara186 16:90119f03c5d1 66 encoderKRAI encoderKanan( PC_12, PD_2, 720, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
calmantara186 16:90119f03c5d1 67 encoderKRAI encoderKiri( PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
rahmadirizki18 15:98f0d56b14f0 68
calmantara186 16:90119f03c5d1 69 /* Deklarasi Motor Base */
calmantara186 16:90119f03c5d1 70 Motor motor1(PB_7, PA_14 , PA_15); // pwm, fwd, rev
calmantara186 16:90119f03c5d1 71 Motor motor2(PB_8, PA_13 ,PB_0); // pwm, fwd, rev
calmantara186 16:90119f03c5d1 72 Motor motor3(PB_9, PA_12 ,PC_5); // pwm, fwd, rev
calmantara186 16:90119f03c5d1 73 Motor motor4(PB_6, PB_1 ,PB_12); // pwm, fwd, rev
fanny868 0:9072e932503c 74
calmantara186 16:90119f03c5d1 75 /* Deklarasi Motor Launcher */
calmantara186 16:90119f03c5d1 76 Motor motorld(PA_8, PC_1 , PC_2); // pwm, fwd, rev
calmantara186 16:90119f03c5d1 77 Motor motorlb(PA_9, PA_4, PC_15 ); // pwm, fwd, rev
rahmadirizki18 5:3aa203218306 78
calmantara186 16:90119f03c5d1 79 /* Deklarasi Servo Launcher */
rahmadirizki18 14:6d389e99981c 80 Servo servoS(PB_2);
rahmadirizki18 14:6d389e99981c 81 Servo servoB(PA_5);
rahmadirizki18 6:68293bed71ea 82
calmantara186 16:90119f03c5d1 83 /**
calmantara186 16:90119f03c5d1 84 * posX dan posY berdasarkan arah robot
calmantara186 16:90119f03c5d1 85 * encoder Depan & Belakang sejajar sumbu Y
calmantara186 16:90119f03c5d1 86 * encoder Kanan & Kiri sejajar sumbu X
calmantara186 16:90119f03c5d1 87 **/
rahmadirizki18 5:3aa203218306 88
calmantara186 16:90119f03c5d1 89 /* Variabel Encoder */
calmantara186 16:90119f03c5d1 90 float jarak, posX, posY; //
calmantara186 16:90119f03c5d1 91 float XT, YT, Tetha; // Jarak Target Robot
calmantara186 16:90119f03c5d1 92 float errX, errY, errT, Vt, Vx, Vy; // Variabel yang didapatkan encoder
calmantara186 16:90119f03c5d1 93 float v1, v2, v3, v4; // Variabel kecepatan motor dari encoder
rahmadirizki18 5:3aa203218306 94
calmantara186 16:90119f03c5d1 95 /* Fungsi dan Procedur Encoder */
calmantara186 16:90119f03c5d1 96 void setCenter(); // Fungsi reset agar robot di tengah
calmantara186 16:90119f03c5d1 97 float getY(); // FUngsi mendapatkan jarak Y
calmantara186 16:90119f03c5d1 98 float getX(); // FUngsi mendapatkan jarak X
calmantara186 16:90119f03c5d1 99 float getTetha(); // FUngsi mendapatkan jarak Tetha
calmantara186 16:90119f03c5d1 100
calmantara186 16:90119f03c5d1 101 /* Inisialisasi Pin TX-RX Joystik dan PC */
rahmadirizki18 3:1287fccc11be 102 joysticknucleo joystick(PA_0,PA_1);
fanny868 0:9072e932503c 103
calmantara186 16:90119f03c5d1 104 /* Variabel Stick */
fanny868 0:9072e932503c 105 char case_ger;
calmantara186 16:90119f03c5d1 106 bool launcher = false,servoGo = false;
fanny868 0:9072e932503c 107
calmantara186 16:90119f03c5d1 108 /***********************************************/
calmantara186 16:90119f03c5d1 109 /* Deklarasi Fungsi dan Procedure */
calmantara186 16:90119f03c5d1 110 /***********************************************/
calmantara186 16:90119f03c5d1 111 int case_gerak(){
calmantara186 16:90119f03c5d1 112 /*****************************************************
calmantara186 16:90119f03c5d1 113 ** Gerak Motor Base
calmantara186 16:90119f03c5d1 114 ** Case 1 : Pivot kanan
calmantara186 16:90119f03c5d1 115 ** Case 2 : Pivot Kiri
calmantara186 16:90119f03c5d1 116 ** Case 3 : Maju
calmantara186 16:90119f03c5d1 117 ** Case 4 : Mundur
calmantara186 16:90119f03c5d1 118 ** Case 5 : Serong Atas Kanan
calmantara186 16:90119f03c5d1 119 ** Case 6 : Serong Bawah Kanan
calmantara186 16:90119f03c5d1 120 ** Case 7 : Serong Atas Kiri
calmantara186 16:90119f03c5d1 121 ** Case 8 : Serong Bawah Kiri
calmantara186 16:90119f03c5d1 122 ** Case 9 : Kanan
calmantara186 16:90119f03c5d1 123 ** Case 10 : Kiri
calmantara186 16:90119f03c5d1 124 ** Case 12 : break
calmantara186 16:90119f03c5d1 125 ****************************************************/
fanny868 0:9072e932503c 126 int casegerak;
calmantara186 16:90119f03c5d1 127 if (!joystick.L1 && joystick.R1) {
fanny868 0:9072e932503c 128 // Pivot Kanan
fanny868 0:9072e932503c 129 casegerak = 1;
calmantara186 16:90119f03c5d1 130 }
calmantara186 16:90119f03c5d1 131 else if (!joystick.R1 && joystick.L1) {
fanny868 0:9072e932503c 132 // Pivot Kiri
fanny868 0:9072e932503c 133 casegerak = 2;
calmantara186 16:90119f03c5d1 134 }
calmantara186 16:90119f03c5d1 135 else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
fanny868 0:9072e932503c 136 // Maju
rahmadirizki18 3:1287fccc11be 137 casegerak = 3;
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 // Mundur
fanny868 0:9072e932503c 141 casegerak = 4;
calmantara186 16:90119f03c5d1 142 }
calmantara186 16:90119f03c5d1 143 else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan)) {
fanny868 0:9072e932503c 144 // Serong Atas Kanan
fanny868 0:9072e932503c 145 casegerak = 5;
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 Bawah Kanan
fanny868 0:9072e932503c 149 casegerak = 6;
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 Atas Kiri
fanny868 0:9072e932503c 153 casegerak = 7;
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 Bawah Kiri
fanny868 0:9072e932503c 157 casegerak = 8;
calmantara186 16:90119f03c5d1 158 }
calmantara186 16:90119f03c5d1 159 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
fanny868 0:9072e932503c 160 // Kanan
fanny868 0:9072e932503c 161 casegerak = 9;
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 // Kiri
rahmadirizki18 3:1287fccc11be 165 casegerak = 10;
calmantara186 16:90119f03c5d1 166 }
calmantara186 16:90119f03c5d1 167 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
calmantara186 16:90119f03c5d1 168 casegerak = 12;
calmantara186 16:90119f03c5d1 169 }
fanny868 0:9072e932503c 170 return(casegerak);
fanny868 0:9072e932503c 171 }
fanny868 0:9072e932503c 172
calmantara186 16:90119f03c5d1 173 void aktuator(){
calmantara186 16:90119f03c5d1 174 /* Fungsi untuk menggerakkan servo */
calmantara186 16:90119f03c5d1 175 // Servo
calmantara186 16:90119f03c5d1 176 if (servoGo){
franshendri 12:e07c59c28c29 177 servoS.position(20);
franshendri 10:f0f0dc3904e0 178 wait_ms(500);
franshendri 12:e07c59c28c29 179 servoS.position(-28);
Joshua23 8:0711dea61312 180 wait_ms(500);
franshendri 12:e07c59c28c29 181 servoS.position(20);
Joshua23 8:0711dea61312 182 wait_ms(500);
franshendri 12:e07c59c28c29 183 for (int i = -0; i<=70; i++){
Joshua23 8:0711dea61312 184 servoB.position(i);
Joshua23 8:0711dea61312 185 wait_ms(10);
Joshua23 8:0711dea61312 186 }
Joshua23 8:0711dea61312 187 wait_ms(500);
Joshua23 8:0711dea61312 188 servoB.position(0);
calmantara186 16:90119f03c5d1 189 servoGo = false;
calmantara186 16:90119f03c5d1 190 }
calmantara186 16:90119f03c5d1 191 else{
franshendri 12:e07c59c28c29 192 servoS.position(20);
rahmadirizki18 6:68293bed71ea 193 servoB.position(0);
rahmadirizki18 7:d138c56dab20 194 }
rahmadirizki18 6:68293bed71ea 195
rahmadirizki18 6:68293bed71ea 196 // Motor Atas
calmantara186 16:90119f03c5d1 197 if (launcher) {
rahmadirizki18 7:d138c56dab20 198 motorld.speed(speedL);
franshendri 12:e07c59c28c29 199 motorlb.speed(speedB);
rahmadirizki18 6:68293bed71ea 200 }else{
rahmadirizki18 6:68293bed71ea 201 motorld.speed(0);
rahmadirizki18 6:68293bed71ea 202 motorlb.speed(0);
rahmadirizki18 6:68293bed71ea 203 }
rahmadirizki18 6:68293bed71ea 204
rahmadirizki18 6:68293bed71ea 205 // MOTOR Bawah
calmantara186 16:90119f03c5d1 206 switch (case_ger) {
calmantara186 16:90119f03c5d1 207 case (1):{
calmantara186 16:90119f03c5d1 208 Tetha = Tetha - 0.05;
fanny868 0:9072e932503c 209 break;
fanny868 0:9072e932503c 210 }
calmantara186 16:90119f03c5d1 211 case (2):{
calmantara186 16:90119f03c5d1 212 Tetha = Tetha + 0.05;
fanny868 0:9072e932503c 213 break;
fanny868 0:9072e932503c 214 }
calmantara186 16:90119f03c5d1 215 case (3):{
calmantara186 16:90119f03c5d1 216 YT = YT + 0.01;
fanny868 0:9072e932503c 217 break;
fanny868 0:9072e932503c 218 }
calmantara186 16:90119f03c5d1 219 case (4):{
calmantara186 16:90119f03c5d1 220 YT = YT - 0.01;
fanny868 0:9072e932503c 221 break;
fanny868 0:9072e932503c 222 }
calmantara186 16:90119f03c5d1 223 case (5) :{
calmantara186 16:90119f03c5d1 224 XT = XT + 0.01;
calmantara186 16:90119f03c5d1 225 YT = YT + 0.01;
fanny868 0:9072e932503c 226 break;
fanny868 0:9072e932503c 227 }
calmantara186 16:90119f03c5d1 228 case (6) :{
calmantara186 16:90119f03c5d1 229 XT = XT + 0.01;
calmantara186 16:90119f03c5d1 230 YT = YT - 0.01;
fanny868 0:9072e932503c 231 break;
fanny868 0:9072e932503c 232 }
calmantara186 16:90119f03c5d1 233 case (7) :{
calmantara186 16:90119f03c5d1 234 XT = XT - 0.01;
calmantara186 16:90119f03c5d1 235 YT = YT + 0.01;
fanny868 0:9072e932503c 236 break;
fanny868 0:9072e932503c 237 }
calmantara186 16:90119f03c5d1 238 case (8) :{
calmantara186 16:90119f03c5d1 239 XT = XT - 0.01;
calmantara186 16:90119f03c5d1 240 YT = YT - 0.01;
fanny868 0:9072e932503c 241 break;
fanny868 0:9072e932503c 242 }
calmantara186 16:90119f03c5d1 243 case (9) :{
calmantara186 16:90119f03c5d1 244 XT = XT + 0.01;
fanny868 0:9072e932503c 245 break;
fanny868 0:9072e932503c 246 }
calmantara186 16:90119f03c5d1 247 case (10) :{
calmantara186 16:90119f03c5d1 248 XT = XT - 0.01;
rahmadirizki18 3:1287fccc11be 249 break;
calmantara186 16:90119f03c5d1 250 }
calmantara186 16:90119f03c5d1 251 default :{}
calmantara186 16:90119f03c5d1 252 } //end of switch
rahmadirizki18 5:3aa203218306 253 }
rahmadirizki18 5:3aa203218306 254
calmantara186 16:90119f03c5d1 255 void setCenter(){
calmantara186 16:90119f03c5d1 256 /* Fungsi untuk menentukan center dari robot */
rahmadirizki18 5:3aa203218306 257 encoderDepan.reset();
rahmadirizki18 5:3aa203218306 258 encoderBelakang.reset();
rahmadirizki18 5:3aa203218306 259 encoderKanan.reset();
rahmadirizki18 5:3aa203218306 260 encoderKiri.reset();
rahmadirizki18 5:3aa203218306 261 }
rahmadirizki18 5:3aa203218306 262
calmantara186 16:90119f03c5d1 263 float getX(){
calmantara186 16:90119f03c5d1 264 /* Fungsi untuk mendapatkan jarak X */
rahmadirizki18 5:3aa203218306 265 float jarakEncDpn, jarakEncBlk;
calmantara186 16:90119f03c5d1 266 jarakEncDpn = (encoderDepan.getPulses())/(float)(2000.0)*k_enc;
calmantara186 16:90119f03c5d1 267 jarakEncBlk = (encoderBelakang.getPulses())/(float)(2000.0)*k_enc;
rahmadirizki18 5:3aa203218306 268 return (jarakEncDpn-jarakEncBlk)/2;
rahmadirizki18 5:3aa203218306 269 }
rahmadirizki18 5:3aa203218306 270
calmantara186 16:90119f03c5d1 271 float getY(){
calmantara186 16:90119f03c5d1 272 /* Fungsi untuk mendapatkan jarak Y */
rahmadirizki18 5:3aa203218306 273 float jarakEncKir, jarakEncKan;
calmantara186 16:90119f03c5d1 274 jarakEncKir = (encoderKiri.getPulses())/(float)(2000.0)*k_enc;
calmantara186 16:90119f03c5d1 275 jarakEncKan = (encoderKanan.getPulses())/(float)(720.0)*k_enc;
rahmadirizki18 5:3aa203218306 276 return (jarakEncKir-jarakEncKan)/2;
rahmadirizki18 5:3aa203218306 277 }
rahmadirizki18 5:3aa203218306 278
calmantara186 16:90119f03c5d1 279 float getTetha(){
calmantara186 16:90119f03c5d1 280 /* Fungsi untuk mendapatkan nilai tetha */
rahmadirizki18 5:3aa203218306 281 float busurDpn, busurBlk, busurKir, busurKan;
calmantara186 16:90119f03c5d1 282 busurDpn = ((encoderDepan.getPulses())/(float)(2000.0)*k_enc)/k_robot*360.0;
calmantara186 16:90119f03c5d1 283 busurBlk = ((encoderBelakang.getPulses())/(float)(2000.0)*k_enc)/k_robot*360.0;
calmantara186 16:90119f03c5d1 284 busurKir = ((encoderKiri.getPulses())/(float)(2000.0)*k_enc)/k_robot*360.0;
calmantara186 16:90119f03c5d1 285 busurKan = ((encoderKanan.getPulses())/(float)(720.0)*k_enc)/k_robot*360.0;
rahmadirizki18 5:3aa203218306 286
rahmadirizki18 5:3aa203218306 287 return -(busurDpn+busurBlk+busurKir+busurKan)/4;
fanny868 0:9072e932503c 288 }
fanny868 0:9072e932503c 289
calmantara186 16:90119f03c5d1 290 void gotoXYT(float xa, float ya, float Ta){
calmantara186 16:90119f03c5d1 291 /* Fungsi untuk bergerat ke target */
calmantara186 16:90119f03c5d1 292 errX = xa-getX();
calmantara186 16:90119f03c5d1 293 Vx = kpX*errX;
calmantara186 16:90119f03c5d1 294
calmantara186 16:90119f03c5d1 295 errY = ya-getY();
calmantara186 16:90119f03c5d1 296 Vy = kpY*errY;
calmantara186 16:90119f03c5d1 297
calmantara186 16:90119f03c5d1 298 errT = Ta-getTetha();
calmantara186 16:90119f03c5d1 299 Vt = kp_tetha*errT;
calmantara186 16:90119f03c5d1 300
calmantara186 16:90119f03c5d1 301 v1 = Vx+Vy-Vt;
calmantara186 16:90119f03c5d1 302 v2 = Vx-Vy-Vt;
calmantara186 16:90119f03c5d1 303 v3 = -Vx-Vy-Vt;
calmantara186 16:90119f03c5d1 304 v4 = -Vx+Vy-Vt;
calmantara186 16:90119f03c5d1 305
calmantara186 16:90119f03c5d1 306 if (v1>speed1)
calmantara186 16:90119f03c5d1 307 { v1 = speed1; }
calmantara186 16:90119f03c5d1 308 else if (v1<-speed1)
calmantara186 16:90119f03c5d1 309 { v1 = -speed1; }
calmantara186 16:90119f03c5d1 310
calmantara186 16:90119f03c5d1 311 if (v2>speed2)
calmantara186 16:90119f03c5d1 312 { v2 = speed2; }
calmantara186 16:90119f03c5d1 313 else if (v2<-speed2)
calmantara186 16:90119f03c5d1 314 { v2 = -speed2; }
calmantara186 16:90119f03c5d1 315
calmantara186 16:90119f03c5d1 316 if (v3>speed3)
calmantara186 16:90119f03c5d1 317 { v3 = speed3; }
calmantara186 16:90119f03c5d1 318 else if (v3<-speed3)
calmantara186 16:90119f03c5d1 319 { v3 = -speed3; }
calmantara186 16:90119f03c5d1 320
calmantara186 16:90119f03c5d1 321 if (v4>speed4)
calmantara186 16:90119f03c5d1 322 { v4 = speed4; }
calmantara186 16:90119f03c5d1 323 else if (v4<-speed4)
calmantara186 16:90119f03c5d1 324 { v4 = -speed4; }
calmantara186 16:90119f03c5d1 325
calmantara186 16:90119f03c5d1 326 if (((errX > 0.05) || (errX<-0.05)) || ((errY > 0.05) || (errY<-0.05)) || ((errT > 0.05) || (errT<-0.05))){
calmantara186 16:90119f03c5d1 327 motor1.speed(v1);
calmantara186 16:90119f03c5d1 328 motor2.speed(v2);
calmantara186 16:90119f03c5d1 329 motor3.speed(v3);
calmantara186 16:90119f03c5d1 330 motor4.speed(v4);
calmantara186 16:90119f03c5d1 331 }
calmantara186 16:90119f03c5d1 332 else{
calmantara186 16:90119f03c5d1 333 motor1.brake(1);
calmantara186 16:90119f03c5d1 334 motor2.brake(1);
calmantara186 16:90119f03c5d1 335 motor3.brake(1);
calmantara186 16:90119f03c5d1 336 motor4.brake(1);
calmantara186 16:90119f03c5d1 337 }
calmantara186 16:90119f03c5d1 338 }
rahmadirizki18 5:3aa203218306 339
calmantara186 16:90119f03c5d1 340 void speedLauncher(){
calmantara186 16:90119f03c5d1 341 /* Fungsi untuk speed launcher */
franshendri 12:e07c59c28c29 342 if (joystick.R3_click and speedL < 0.8){
calmantara186 16:90119f03c5d1 343 speedL = speedL + 0.01;
calmantara186 16:90119f03c5d1 344 }
rahmadirizki18 7:d138c56dab20 345 if (joystick.L3_click and speedL > 0.1){
calmantara186 16:90119f03c5d1 346 speedL = speedL - 0.01;
calmantara186 16:90119f03c5d1 347 }
rahmadirizki18 13:8ab42383a2ca 348 if (joystick.R2_click and speedB < 0.8 ){
calmantara186 16:90119f03c5d1 349 speedB = speedB + 0.01;
calmantara186 16:90119f03c5d1 350 }
rahmadirizki18 13:8ab42383a2ca 351 if (joystick.L2_click and speedB > 0.1 ){
calmantara186 16:90119f03c5d1 352 speedB = speedB - 0.01;
calmantara186 16:90119f03c5d1 353 }
rahmadirizki18 7:d138c56dab20 354 }
franshendri 12:e07c59c28c29 355
calmantara186 16:90119f03c5d1 356 /***********************************************/
calmantara186 16:90119f03c5d1 357 /* Main Function */
calmantara186 16:90119f03c5d1 358 /***********************************************/
calmantara186 16:90119f03c5d1 359
calmantara186 16:90119f03c5d1 360 int main (void){
calmantara186 16:90119f03c5d1 361 /* Set baud rate - 115200 */
fanny868 0:9072e932503c 362 joystick.setup();
rahmadirizki18 6:68293bed71ea 363 wait_ms(1000);
rahmadirizki18 5:3aa203218306 364 setCenter();
rahmadirizki18 5:3aa203218306 365 wait_ms(500);
rahmadirizki18 5:3aa203218306 366
calmantara186 16:90119f03c5d1 367 /* Posisi Awal */
rahmadirizki18 5:3aa203218306 368 XT = 0;
rahmadirizki18 5:3aa203218306 369 YT = 0;
rahmadirizki18 5:3aa203218306 370 Tetha = 0;
calmantara186 16:90119f03c5d1 371
calmantara186 16:90119f03c5d1 372 /* Untuk mendapatkan serial dari Arduino */
fanny868 0:9072e932503c 373 while(1)
fanny868 0:9072e932503c 374 {
fanny868 0:9072e932503c 375 // Interrupt Serial
calmantara186 16:90119f03c5d1 376 joystick.idle();
calmantara186 16:90119f03c5d1 377
calmantara186 16:90119f03c5d1 378 if(joystick.readable() ) {
fanny868 0:9072e932503c 379 // Panggil fungsi pembacaan joystik
fanny868 0:9072e932503c 380 joystick.baca_data();
calmantara186 16:90119f03c5d1 381
fanny868 0:9072e932503c 382 // Panggil fungsi pengolahan data joystik
fanny868 0:9072e932503c 383 joystick.olah_data();
calmantara186 16:90119f03c5d1 384
calmantara186 16:90119f03c5d1 385 // Masuk ke case gerak
fanny868 0:9072e932503c 386 case_ger = case_gerak();
rahmadirizki18 3:1287fccc11be 387 aktuator();
calmantara186 16:90119f03c5d1 388
calmantara186 16:90119f03c5d1 389 if (joystick.segitiga_click) launcher = !launcher;
calmantara186 16:90119f03c5d1 390 if (joystick.lingkaran_click) servoGo = true;
rahmadirizki18 5:3aa203218306 391 if (joystick.silang) {
rahmadirizki18 5:3aa203218306 392 XT = 0;
rahmadirizki18 5:3aa203218306 393 YT = 0;
rahmadirizki18 5:3aa203218306 394 Tetha = 0;
calmantara186 16:90119f03c5d1 395 }
rahmadirizki18 7:d138c56dab20 396 speedLauncher();
franshendri 12:e07c59c28c29 397
calmantara186 16:90119f03c5d1 398 }
calmantara186 16:90119f03c5d1 399 else {
calmantara186 16:90119f03c5d1 400 joystick.idle();
fanny868 0:9072e932503c 401 }
rahmadirizki18 5:3aa203218306 402 gotoXYT(XT,YT,Tetha);
rahmadirizki18 5:3aa203218306 403
fanny868 0:9072e932503c 404 }
fanny868 0:9072e932503c 405 }