KRAI 2017 / Mbed 2 deprecated Joystick_ManualV2_hapusSeluruhEncoder

aman semoga

Dependencies:   Motor PID Joystick_OrdoV5 mbed

Fork of Joystick_ManualV2 by KRAI 2017

main.cpp@19:bb304933a9f8, 2017-01-18 (annotated)

Committer:
rahmadirizki18
Date:
Wed Jan 18 14:08:37 2017 +0000
Revision:
19:bb304933a9f8
Parent:
18:23412e950394 
aman semoga

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rahmadirizki18 5:3aa203218306 1 /****************************************************************************/
rahmadirizki18 6:68293bed71ea 2 /* */
rahmadirizki18 6:68293bed71ea 3 /* Joystick */
rahmadirizki18 6:68293bed71ea 4 /* kanan => posisi target x ditambah 0.01 */
rahmadirizki18 6:68293bed71ea 5 /* kiri => posisi target x dikurang 0.01 */
rahmadirizki18 6:68293bed71ea 6 /* atas => posisi target y ditambah 0.01 */
rahmadirizki18 6:68293bed71ea 7 /* bawah => posisi target y dikurang 0.01 */
rahmadirizki18 6:68293bed71ea 8 /* */
rahmadirizki18 6:68293bed71ea 9 /* Tombol silang => Kembali keposisi Awal */
rahmadirizki18 6:68293bed71ea 10 /* Tombol segitiga => Aktif motor Launcher */
rahmadirizki18 13:8ab42383a2ca 11 /* Tombol lingkaran=> Aktif servo Launcher */
rahmadirizki18 13:8ab42383a2ca 12 /* Tombol L3 => PWM Launcher dikurangin */
rahmadirizki18 13:8ab42383a2ca 13 /* Tombol R3 => PWM Launcher ditambahin */
rahmadirizki18 13:8ab42383a2ca 14 /* */
rahmadirizki18 13:8ab42383a2ca 15 /* */
rahmadirizki18 6:68293bed71ea 16 /* */
rahmadirizki18 6:68293bed71ea 17 /****************************************************************************/
rahmadirizki18 6:68293bed71ea 18
fanny868 0:9072e932503c 19
fanny868 0:9072e932503c 20 #include "mbed.h"
fanny868 0:9072e932503c 21 #include "JoystickPS3.h"
fanny868 0:9072e932503c 22 #include "Motor.h"
rahmadirizki18 6:68293bed71ea 23 #include "Servo.h"
fanny868 0:9072e932503c 24
fanny868 0:9072e932503c 25 //#define koefperlambatan 0.8
rahmadirizki18 16:89093194ccc2 26 //#include "encoderKRAI.h"
rahmadirizki18 16:89093194ccc2 27 /*
rahmadirizki18 5:3aa203218306 28 #define pi 22/7
rahmadirizki18 5:3aa203218306 29 #define diaEncoder 0.058
rahmadirizki18 5:3aa203218306 30 #define PPR 1000
rahmadirizki18 5:3aa203218306 31 #define diaRobot 0.64
rahmadirizki18 16:89093194ccc2 32 */
rahmadirizki18 17:703072f5dce1 33 #define vmax 0.3 //0.4
rahmadirizki18 17:703072f5dce1 34 #define vmaxserong 0.3 //0.3
rahmadirizki18 17:703072f5dce1 35 #define vmaxpivot 0.3 //0.3
rahmadirizki18 17:703072f5dce1 36 #define vmaxanalog 0.3 //0.3
rahmadirizki18 16:89093194ccc2 37 #define ax 0.005
Joshua23 9:5a50782510fb 38 #define pinservo1 PC_8
Joshua23 9:5a50782510fb 39 #define pinservo2 PC_9
rahmadirizki18 5:3aa203218306 40
rahmadirizki18 5:3aa203218306 41 float speed1=0.6;
rahmadirizki18 5:3aa203218306 42 float speed2=0.6;
rahmadirizki18 5:3aa203218306 43 float speed3=0.6;
rahmadirizki18 5:3aa203218306 44 float speed4=0.6;
rahmadirizki18 19:bb304933a9f8 45 float speedB=0.23 ;
franshendri 12:e07c59c28c29 46 float speedL=0.4;
rahmadirizki18 5:3aa203218306 47
rahmadirizki18 6:68293bed71ea 48
fanny868 0:9072e932503c 49
fanny868 0:9072e932503c 50 // Deklarasi variabel motor
rahmadirizki18 17:703072f5dce1 51 Motor motor1(PB_7, PA_15 , PA_14); // pwm, fwd, rev
rahmadirizki18 17:703072f5dce1 52 Motor motor2(PB_8, PB_0 ,PA_13); // pwm, fwd, rev
rahmadirizki18 17:703072f5dce1 53 Motor motor3(PB_9, PC_5 ,PA_12); // pwm, fwd, rev
rahmadirizki18 17:703072f5dce1 54 Motor motor4(PB_6, PB_12 ,PB_1); // pwm, fwd, rev
rahmadirizki18 5:3aa203218306 55
rahmadirizki18 6:68293bed71ea 56 //Motor Atas
rahmadirizki18 14:6d389e99981c 57 Motor motorld(PA_8, PC_1 , PC_2); // pwm, fwd, rev
rahmadirizki18 14:6d389e99981c 58 Motor motorlb(PA_9, PA_4, PC_15 ); // pwm, fwd, rev
rahmadirizki18 6:68293bed71ea 59
rahmadirizki18 6:68293bed71ea 60 //Servo Atas
rahmadirizki18 14:6d389e99981c 61 Servo servoS(PB_2);
rahmadirizki18 14:6d389e99981c 62 Servo servoB(PA_5);
rahmadirizki18 6:68293bed71ea 63
rahmadirizki18 5:3aa203218306 64 float jarak, posX, posY;
rahmadirizki18 5:3aa203218306 65
fanny868 0:9072e932503c 66 // Inisialisasi Pin TX-RX Joystik dan PC
rahmadirizki18 3:1287fccc11be 67 joysticknucleo joystick(PA_0,PA_1);
rahmadirizki18 19:bb304933a9f8 68 Serial pc(PA_2,PA_3);
fanny868 0:9072e932503c 69
rahmadirizki18 5:3aa203218306 70 //encoder variable
rahmadirizki18 5:3aa203218306 71 float errX, errY, errT, Vt, Vx, Vy;
rahmadirizki18 5:3aa203218306 72 float V1, V2, V3, V4;
rahmadirizki18 5:3aa203218306 73
fanny868 0:9072e932503c 74 //bool perlambatan=0;
fanny868 0:9072e932503c 75 char case_ger;
rahmadirizki18 3:1287fccc11be 76 bool maju=false,mundur=false,pivka=false,pivki=false,kiri=false,kanan=false,saka=false,saki=false,sbka=false,sbki=false,cw1=false,ccw1=false,cw2=false,ccw2=false,cw3=false,ccw3=false,analog=false;
fanny868 0:9072e932503c 77 bool stop = true;
rahmadirizki18 6:68293bed71ea 78 bool Launcher = false,ServoGo = false;
rahmadirizki18 4:483c07cc22e1 79 float jLX,jLY;
rahmadirizki18 5:3aa203218306 80 double vcurr;
rahmadirizki18 4:483c07cc22e1 81 float x,y; // untuk analoghat kiri
rahmadirizki18 3:1287fccc11be 82 float errorx=0,errory=0;
rahmadirizki18 3:1287fccc11be 83
rahmadirizki18 4:483c07cc22e1 84 // Fungsi mapping 0-255 ke -128 sampai 127
rahmadirizki18 4:483c07cc22e1 85 float mapping(float a,float error)
rahmadirizki18 3:1287fccc11be 86 {
rahmadirizki18 4:483c07cc22e1 87 float hasil,b;
rahmadirizki18 4:483c07cc22e1 88 b = (float)((a-128)/128);
rahmadirizki18 4:483c07cc22e1 89 if (b>(error - 0.2) && b<(error + 0.2))
rahmadirizki18 4:483c07cc22e1 90 {
rahmadirizki18 4:483c07cc22e1 91 hasil = 0;
rahmadirizki18 4:483c07cc22e1 92 } else {
rahmadirizki18 4:483c07cc22e1 93 hasil = b;
rahmadirizki18 4:483c07cc22e1 94 }
rahmadirizki18 3:1287fccc11be 95 return (hasil);
rahmadirizki18 3:1287fccc11be 96 }
fanny868 0:9072e932503c 97
rahmadirizki18 4:483c07cc22e1 98 // Kalibrasi tombol analog kiri
rahmadirizki18 4:483c07cc22e1 99 void kalibrasi()
rahmadirizki18 4:483c07cc22e1 100 {
rahmadirizki18 4:483c07cc22e1 101 errorx = (jLX - 128)/128;
rahmadirizki18 4:483c07cc22e1 102 errory = (jLY - 128)/128;
rahmadirizki18 4:483c07cc22e1 103
rahmadirizki18 4:483c07cc22e1 104 }
rahmadirizki18 4:483c07cc22e1 105
rahmadirizki18 3:1287fccc11be 106 // simpan data analog
rahmadirizki18 3:1287fccc11be 107 void baca_analog()
rahmadirizki18 3:1287fccc11be 108 {
rahmadirizki18 3:1287fccc11be 109 jLX = joystick.LX;
rahmadirizki18 3:1287fccc11be 110 jLY = joystick.LY;
rahmadirizki18 3:1287fccc11be 111
rahmadirizki18 3:1287fccc11be 112 // Pembacaan nilai Y terbalik
rahmadirizki18 4:483c07cc22e1 113 x = mapping(jLX,errorx);
rahmadirizki18 4:483c07cc22e1 114 y = -mapping(jLY,errory);
rahmadirizki18 3:1287fccc11be 115 }
fanny868 0:9072e932503c 116
rahmadirizki18 3:1287fccc11be 117 // Gerak dari Motor base
fanny868 0:9072e932503c 118 int case_gerak()
fanny868 0:9072e932503c 119 {
fanny868 0:9072e932503c 120 int casegerak;
rahmadirizki18 3:1287fccc11be 121 baca_analog();
rahmadirizki18 3:1287fccc11be 122 if (!joystick.L1 && joystick.R1) {
fanny868 0:9072e932503c 123 // Pivot Kanan
fanny868 0:9072e932503c 124 casegerak = 1;
fanny868 0:9072e932503c 125 } else if (!joystick.R1 && joystick.L1) {
fanny868 0:9072e932503c 126 // Pivot Kiri
fanny868 0:9072e932503c 127 casegerak = 2;
rahmadirizki18 3:1287fccc11be 128 } else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
fanny868 0:9072e932503c 129 // Maju
rahmadirizki18 3:1287fccc11be 130 casegerak = 3;
rahmadirizki18 3:1287fccc11be 131 } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
fanny868 0:9072e932503c 132 // Mundur
fanny868 0:9072e932503c 133 casegerak = 4;
rahmadirizki18 3:1287fccc11be 134 } else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan)) {
fanny868 0:9072e932503c 135 // Serong Atas Kanan
fanny868 0:9072e932503c 136 casegerak = 5;
rahmadirizki18 3:1287fccc11be 137 } else if((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan)) {
fanny868 0:9072e932503c 138 // Serong Bawah Kanan
fanny868 0:9072e932503c 139 casegerak = 6;
rahmadirizki18 3:1287fccc11be 140 } else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)) {
fanny868 0:9072e932503c 141 // Serong Atas Kiri
fanny868 0:9072e932503c 142 casegerak = 7;
rahmadirizki18 3:1287fccc11be 143 } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)) {
fanny868 0:9072e932503c 144 // Serong Bawah Kiri
fanny868 0:9072e932503c 145 casegerak = 8;
rahmadirizki18 3:1287fccc11be 146 } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
fanny868 0:9072e932503c 147 // Kanan
fanny868 0:9072e932503c 148 casegerak = 9;
rahmadirizki18 3:1287fccc11be 149 } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
fanny868 0:9072e932503c 150 // Kiri
rahmadirizki18 3:1287fccc11be 151 casegerak = 10;
rahmadirizki18 3:1287fccc11be 152 } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
rahmadirizki18 3:1287fccc11be 153 // case gerak analog on off
rahmadirizki18 3:1287fccc11be 154 if (analog){
rahmadirizki18 3:1287fccc11be 155 casegerak = 11;
rahmadirizki18 3:1287fccc11be 156 } else {
rahmadirizki18 3:1287fccc11be 157 casegerak = 12;
rahmadirizki18 3:1287fccc11be 158 }
rahmadirizki18 3:1287fccc11be 159 }
fanny868 0:9072e932503c 160 return(casegerak);
fanny868 0:9072e932503c 161 }
fanny868 0:9072e932503c 162
fanny868 0:9072e932503c 163
fanny868 0:9072e932503c 164
fanny868 0:9072e932503c 165 /**
fanny868 0:9072e932503c 166
rahmadirizki18 3:1287fccc11be 167 ** Case 1 : Pivot kanan
fanny868 0:9072e932503c 168 ** Case 2 : Pivot Kiri
fanny868 0:9072e932503c 169 ** Case 3 : Maju
fanny868 0:9072e932503c 170 ** Case 4 : Mundur
fanny868 0:9072e932503c 171 ** Case 5 : Serong Atas Kanan
fanny868 0:9072e932503c 172 ** Case 6 : Serong Bawah Kanan
fanny868 0:9072e932503c 173 ** Case 7 : Serong Atas Kiri
fanny868 0:9072e932503c 174 ** Case 8 : Serong Bawah Kiri
fanny868 0:9072e932503c 175 ** Case 9 : Kanan
fanny868 0:9072e932503c 176 ** Case 10 : Kiri
rahmadirizki18 3:1287fccc11be 177 ** Case 11 : Analog
rahmadirizki18 3:1287fccc11be 178 ** Case 11 : break
fanny868 0:9072e932503c 179
fanny868 0:9072e932503c 180 **/
rahmadirizki18 3:1287fccc11be 181
rahmadirizki18 5:3aa203218306 182
rahmadirizki18 3:1287fccc11be 183 // Pergerakan dari base
fanny868 0:9072e932503c 184 void aktuator()
fanny868 0:9072e932503c 185 {
rahmadirizki18 6:68293bed71ea 186 //Servo
rahmadirizki18 7:d138c56dab20 187 if (ServoGo){
franshendri 12:e07c59c28c29 188 servoS.position(20);
franshendri 10:f0f0dc3904e0 189 wait_ms(500);
franshendri 12:e07c59c28c29 190 servoS.position(-28);
Joshua23 8:0711dea61312 191 wait_ms(500);
franshendri 12:e07c59c28c29 192 servoS.position(20);
Joshua23 8:0711dea61312 193 wait_ms(500);
franshendri 12:e07c59c28c29 194 for (int i = -0; i<=70; i++){
Joshua23 8:0711dea61312 195 servoB.position(i);
Joshua23 8:0711dea61312 196 wait_ms(10);
Joshua23 8:0711dea61312 197 }
Joshua23 8:0711dea61312 198 wait_ms(500);
Joshua23 8:0711dea61312 199 servoB.position(0);
rahmadirizki18 7:d138c56dab20 200 ServoGo = false;
rahmadirizki18 6:68293bed71ea 201
rahmadirizki18 6:68293bed71ea 202 }else{
franshendri 12:e07c59c28c29 203 servoS.position(20);
rahmadirizki18 6:68293bed71ea 204 servoB.position(0);
fanny868 0:9072e932503c 205
rahmadirizki18 7:d138c56dab20 206 }
rahmadirizki18 6:68293bed71ea 207
rahmadirizki18 6:68293bed71ea 208 // Motor Atas
rahmadirizki18 6:68293bed71ea 209 if (Launcher) {
rahmadirizki18 7:d138c56dab20 210 motorld.speed(speedL);
franshendri 12:e07c59c28c29 211 motorlb.speed(speedB);
rahmadirizki18 6:68293bed71ea 212 }else{
rahmadirizki18 6:68293bed71ea 213 motorld.speed(0);
rahmadirizki18 6:68293bed71ea 214 motorlb.speed(0);
rahmadirizki18 6:68293bed71ea 215 }
rahmadirizki18 6:68293bed71ea 216
rahmadirizki18 16:89093194ccc2 217 double s1=0,s2=0,s3=0,s4=0,s1t=0,s2t=0,s3t=0,s4t=0;
rahmadirizki18 16:89093194ccc2 218
rahmadirizki18 16:89093194ccc2 219 // MOTOR BAWAH
fanny868 0:9072e932503c 220 switch (case_ger)
fanny868 0:9072e932503c 221 {
rahmadirizki18 3:1287fccc11be 222 case (1):
fanny868 0:9072e932503c 223 {
rahmadirizki18 16:89093194ccc2 224 if (pivka) {
rahmadirizki18 16:89093194ccc2 225 if(vcurr<0.1) {
rahmadirizki18 16:89093194ccc2 226 vcurr=0.1;
rahmadirizki18 16:89093194ccc2 227 } else {
rahmadirizki18 16:89093194ccc2 228 vcurr+=ax;
rahmadirizki18 16:89093194ccc2 229 }
rahmadirizki18 16:89093194ccc2 230 //perlambatan=0;
rahmadirizki18 16:89093194ccc2 231 } else {
rahmadirizki18 16:89093194ccc2 232 //perlambatan=1;
rahmadirizki18 16:89093194ccc2 233 }
rahmadirizki18 16:89093194ccc2 234
rahmadirizki18 16:89093194ccc2 235 if (vcurr>=vmaxpivot) {
rahmadirizki18 16:89093194ccc2 236 vcurr=vmaxpivot;
rahmadirizki18 16:89093194ccc2 237 }
rahmadirizki18 16:89093194ccc2 238
rahmadirizki18 16:89093194ccc2 239
rahmadirizki18 18:23412e950394 240 s1 = (float)(-0.5* vcurr);
rahmadirizki18 18:23412e950394 241 s2 = (float)(-0.5* vcurr);
rahmadirizki18 18:23412e950394 242 s3 = (float)(-0.5* vcurr);
rahmadirizki18 18:23412e950394 243 s4 = (float)(-0.5* vcurr);
rahmadirizki18 16:89093194ccc2 244
fanny868 0:9072e932503c 245 pivka=true;
rahmadirizki18 3:1287fccc11be 246 maju=mundur=analog=kiri=kanan=saka=saki=sbka=sbki=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 247
rahmadirizki18 17:703072f5dce1 248 //pc.printf("pivKa\n");
fanny868 0:9072e932503c 249
rahmadirizki18 16:89093194ccc2 250 motor1.speed(s1);
rahmadirizki18 16:89093194ccc2 251 motor2.speed(s2);
rahmadirizki18 16:89093194ccc2 252 motor3.speed(s3);
rahmadirizki18 16:89093194ccc2 253 motor4.speed(s4);
rahmadirizki18 16:89093194ccc2 254
fanny868 0:9072e932503c 255 break;
fanny868 0:9072e932503c 256 }
fanny868 0:9072e932503c 257 case (2):
fanny868 0:9072e932503c 258 {
rahmadirizki18 16:89093194ccc2 259 if (pivki){
rahmadirizki18 16:89093194ccc2 260 if(vcurr<0.1) {
rahmadirizki18 16:89093194ccc2 261 vcurr=0.1;
rahmadirizki18 16:89093194ccc2 262 } else {
rahmadirizki18 16:89093194ccc2 263 vcurr+=ax;
rahmadirizki18 16:89093194ccc2 264 }
rahmadirizki18 16:89093194ccc2 265 //perlambatan=0;
rahmadirizki18 16:89093194ccc2 266 } else {
rahmadirizki18 16:89093194ccc2 267 //perlambatan=1;
rahmadirizki18 16:89093194ccc2 268 }
rahmadirizki18 16:89093194ccc2 269
rahmadirizki18 16:89093194ccc2 270 if (vcurr>=vmaxpivot) {
rahmadirizki18 16:89093194ccc2 271 vcurr=vmaxpivot;
rahmadirizki18 16:89093194ccc2 272 }
rahmadirizki18 16:89093194ccc2 273
rahmadirizki18 18:23412e950394 274
rahmadirizki18 18:23412e950394 275 s1 = (float)(0.5* vcurr);
rahmadirizki18 18:23412e950394 276 s2 = (float)(0.5* vcurr);
rahmadirizki18 18:23412e950394 277 s3 = (float)(0.5* vcurr);
rahmadirizki18 18:23412e950394 278 s4 = (float)(0.5* vcurr);
fanny868 0:9072e932503c 279
fanny868 0:9072e932503c 280 pivki=true;
rahmadirizki18 3:1287fccc11be 281 maju=mundur=kiri=analog=kanan=saka=saki=sbka=sbki=pivka=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 282
rahmadirizki18 17:703072f5dce1 283 //pc.printf("pivKi\n");
fanny868 0:9072e932503c 284
rahmadirizki18 16:89093194ccc2 285 motor1.speed(s1);
rahmadirizki18 16:89093194ccc2 286 motor2.speed(s2);
rahmadirizki18 16:89093194ccc2 287 motor3.speed(s3);
rahmadirizki18 16:89093194ccc2 288 motor4.speed(s4);
rahmadirizki18 16:89093194ccc2 289
fanny868 0:9072e932503c 290 break;
fanny868 0:9072e932503c 291 }
fanny868 0:9072e932503c 292 case (3):
fanny868 0:9072e932503c 293 {
rahmadirizki18 16:89093194ccc2 294 if (maju) {
rahmadirizki18 16:89093194ccc2 295 if(vcurr<0.1) {
rahmadirizki18 16:89093194ccc2 296 vcurr=0.1;
rahmadirizki18 16:89093194ccc2 297 } else {
rahmadirizki18 16:89093194ccc2 298 vcurr+=ax;
rahmadirizki18 16:89093194ccc2 299 }
rahmadirizki18 16:89093194ccc2 300 //perlambatan=0;
rahmadirizki18 16:89093194ccc2 301 } else {
rahmadirizki18 16:89093194ccc2 302 //perlambatan=1;
rahmadirizki18 16:89093194ccc2 303 }
rahmadirizki18 16:89093194ccc2 304
rahmadirizki18 16:89093194ccc2 305 if (vcurr>=vmax) {
rahmadirizki18 16:89093194ccc2 306 vcurr=vmax;
rahmadirizki18 16:89093194ccc2 307 }
rahmadirizki18 16:89093194ccc2 308
rahmadirizki18 16:89093194ccc2 309
rahmadirizki18 16:89093194ccc2 310 //Case s1 untuk mode L2 lebih lambat
rahmadirizki18 18:23412e950394 311 s1 = (float)(-1* (vcurr));
rahmadirizki18 18:23412e950394 312 s2 = (float)(1.0* vcurr);
rahmadirizki18 18:23412e950394 313 s3 = (float)(1.0* vcurr);
rahmadirizki18 18:23412e950394 314 s4 = (float)(-1* vcurr);
rahmadirizki18 16:89093194ccc2 315
rahmadirizki18 18:23412e950394 316 //s1 =-0.8* vcurr;
rahmadirizki18 18:23412e950394 317 //s2 = vcurr;
rahmadirizki18 18:23412e950394 318 //s3 =- vcurr;
rahmadirizki18 18:23412e950394 319 //s4 = vcurr;
fanny868 0:9072e932503c 320
fanny868 0:9072e932503c 321 maju=true;
rahmadirizki18 3:1287fccc11be 322 mundur=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 323
rahmadirizki18 17:703072f5dce1 324 //pc.printf("maju\n");
fanny868 0:9072e932503c 325
rahmadirizki18 16:89093194ccc2 326 motor1.speed(s1);
rahmadirizki18 16:89093194ccc2 327 motor2.speed(s2);
rahmadirizki18 16:89093194ccc2 328 motor3.speed(s3);
rahmadirizki18 16:89093194ccc2 329 motor4.speed(s4);
rahmadirizki18 16:89093194ccc2 330
fanny868 0:9072e932503c 331 break;
fanny868 0:9072e932503c 332 }
fanny868 0:9072e932503c 333 case (4):
fanny868 0:9072e932503c 334 {
rahmadirizki18 16:89093194ccc2 335 if (mundur) {
rahmadirizki18 16:89093194ccc2 336 if(vcurr<0.1) {
rahmadirizki18 16:89093194ccc2 337 vcurr=0.1;
rahmadirizki18 16:89093194ccc2 338 } else {
rahmadirizki18 16:89093194ccc2 339 vcurr+=ax;
rahmadirizki18 16:89093194ccc2 340 }
rahmadirizki18 16:89093194ccc2 341 //perlambatan=0;
rahmadirizki18 16:89093194ccc2 342 } else {
rahmadirizki18 16:89093194ccc2 343 //perlambatan=1;
rahmadirizki18 16:89093194ccc2 344 }
rahmadirizki18 16:89093194ccc2 345
rahmadirizki18 16:89093194ccc2 346 if (vcurr>=vmax) {
rahmadirizki18 16:89093194ccc2 347 vcurr=vmax;
rahmadirizki18 16:89093194ccc2 348 }
rahmadirizki18 16:89093194ccc2 349
rahmadirizki18 17:703072f5dce1 350
rahmadirizki18 18:23412e950394 351 s1 = (float)(1* (vcurr));
rahmadirizki18 18:23412e950394 352 s2 = (float)(-1* (vcurr));
rahmadirizki18 18:23412e950394 353 s3 = (float)(-1* (vcurr));
rahmadirizki18 18:23412e950394 354 s4 = (float)(1* (vcurr));
fanny868 0:9072e932503c 355
fanny868 0:9072e932503c 356 mundur=true;
rahmadirizki18 3:1287fccc11be 357 maju=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 358
rahmadirizki18 17:703072f5dce1 359 //pc.printf("mundur\n");
fanny868 0:9072e932503c 360
rahmadirizki18 16:89093194ccc2 361 motor1.speed(s1);
rahmadirizki18 16:89093194ccc2 362 motor2.speed(s2);
rahmadirizki18 16:89093194ccc2 363 motor3.speed(s3);
rahmadirizki18 16:89093194ccc2 364 motor4.speed(s4);
fanny868 0:9072e932503c 365
fanny868 0:9072e932503c 366 break;
fanny868 0:9072e932503c 367 }
fanny868 0:9072e932503c 368 case (5) :
fanny868 0:9072e932503c 369 {
rahmadirizki18 16:89093194ccc2 370 if (saka) {
rahmadirizki18 16:89093194ccc2 371 if(vcurr<0.1) {
rahmadirizki18 16:89093194ccc2 372 vcurr=0.1;
rahmadirizki18 16:89093194ccc2 373 } else {
rahmadirizki18 16:89093194ccc2 374 vcurr+=ax;
rahmadirizki18 16:89093194ccc2 375 }
rahmadirizki18 16:89093194ccc2 376 //perlambatan=0;
rahmadirizki18 16:89093194ccc2 377 } else {
rahmadirizki18 16:89093194ccc2 378 //perlambatan=1;
rahmadirizki18 16:89093194ccc2 379 }
rahmadirizki18 16:89093194ccc2 380
rahmadirizki18 16:89093194ccc2 381 if (vcurr>=vmax) {
rahmadirizki18 16:89093194ccc2 382 vcurr=vmax;
rahmadirizki18 18:23412e950394 383 }
rahmadirizki18 18:23412e950394 384
rahmadirizki18 16:89093194ccc2 385
rahmadirizki18 18:23412e950394 386 s1 = (float)(- vcurr);
rahmadirizki18 18:23412e950394 387 s2 = (float)(0); // 0.1*vcurr;
rahmadirizki18 18:23412e950394 388 s3 = (float)( vcurr);
rahmadirizki18 18:23412e950394 389 s4 = (float)(0); //- 0.1*vcurr;
fanny868 0:9072e932503c 390
fanny868 0:9072e932503c 391 saka=true;
rahmadirizki18 3:1287fccc11be 392 maju=mundur=kiri=kanan=sbka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 393
rahmadirizki18 17:703072f5dce1 394 //pc.printf("saka\n");
fanny868 0:9072e932503c 395
rahmadirizki18 16:89093194ccc2 396 motor1.speed(s1);
rahmadirizki18 16:89093194ccc2 397 motor2.brake(1);
rahmadirizki18 16:89093194ccc2 398 //motor2.speed(s2);
rahmadirizki18 16:89093194ccc2 399 motor3.speed(s3);
rahmadirizki18 16:89093194ccc2 400 motor4.brake(1);
rahmadirizki18 16:89093194ccc2 401 //motor4.speed(s4);
fanny868 0:9072e932503c 402
fanny868 0:9072e932503c 403 break;
fanny868 0:9072e932503c 404 }
fanny868 0:9072e932503c 405 case (6) :
fanny868 0:9072e932503c 406 {
rahmadirizki18 16:89093194ccc2 407 if (sbka){
rahmadirizki18 16:89093194ccc2 408 if(vcurr<0.1) {
rahmadirizki18 16:89093194ccc2 409 vcurr=0.1;
rahmadirizki18 16:89093194ccc2 410 } else {
rahmadirizki18 16:89093194ccc2 411 vcurr+=ax;
rahmadirizki18 16:89093194ccc2 412 }
rahmadirizki18 16:89093194ccc2 413 //perlambatan=0;
rahmadirizki18 16:89093194ccc2 414 } else {
rahmadirizki18 16:89093194ccc2 415 //perlambatan=1;
rahmadirizki18 16:89093194ccc2 416 }
rahmadirizki18 16:89093194ccc2 417
rahmadirizki18 16:89093194ccc2 418 if (vcurr>=vmaxserong) {
rahmadirizki18 16:89093194ccc2 419 vcurr=vmaxserong;
rahmadirizki18 16:89093194ccc2 420 }
rahmadirizki18 16:89093194ccc2 421
rahmadirizki18 16:89093194ccc2 422
rahmadirizki18 18:23412e950394 423 s1 = (float)(0); // 0.1*vcurr;
rahmadirizki18 18:23412e950394 424 s2 = (float)(- vcurr);
rahmadirizki18 18:23412e950394 425 s3 = (float)(0); //- 0.1*vcurr;
rahmadirizki18 18:23412e950394 426 s4 = (float)( vcurr);
fanny868 0:9072e932503c 427
fanny868 0:9072e932503c 428 sbka=true;
rahmadirizki18 3:1287fccc11be 429 maju=mundur=kiri=kanan=saka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 430
rahmadirizki18 17:703072f5dce1 431 //pc.printf("sbka\n");
fanny868 0:9072e932503c 432
rahmadirizki18 16:89093194ccc2 433 //motor1.speed(s1);
rahmadirizki18 16:89093194ccc2 434 motor1.brake(1);
rahmadirizki18 16:89093194ccc2 435 motor2.speed(s2);
rahmadirizki18 16:89093194ccc2 436 //motor3.speed(s3);
rahmadirizki18 16:89093194ccc2 437 motor3.brake(1);
rahmadirizki18 16:89093194ccc2 438 motor4.speed(s4);
fanny868 0:9072e932503c 439
fanny868 0:9072e932503c 440 break;
fanny868 0:9072e932503c 441 }
fanny868 0:9072e932503c 442 case (7) :
fanny868 0:9072e932503c 443 {
rahmadirizki18 16:89093194ccc2 444 if (saki) {
rahmadirizki18 16:89093194ccc2 445 if(vcurr<0.1) {
rahmadirizki18 16:89093194ccc2 446 vcurr=0.1;
rahmadirizki18 16:89093194ccc2 447 } else {
rahmadirizki18 16:89093194ccc2 448 vcurr+=ax;
rahmadirizki18 16:89093194ccc2 449 }
rahmadirizki18 16:89093194ccc2 450 //perlambatan=0;
rahmadirizki18 16:89093194ccc2 451 } else {
rahmadirizki18 16:89093194ccc2 452 //perlambatan=1;
rahmadirizki18 16:89093194ccc2 453 }
rahmadirizki18 16:89093194ccc2 454
rahmadirizki18 16:89093194ccc2 455 if (vcurr>=vmaxserong) {
rahmadirizki18 16:89093194ccc2 456 vcurr=vmaxserong;
rahmadirizki18 16:89093194ccc2 457 }
rahmadirizki18 16:89093194ccc2 458
rahmadirizki18 16:89093194ccc2 459
rahmadirizki18 18:23412e950394 460 s1 = (float)(0); //- 0.1*vcurr;
rahmadirizki18 18:23412e950394 461 s2 = (float)( vcurr);
rahmadirizki18 18:23412e950394 462 s3 = (float)(0); // 0.1*vcurr;
rahmadirizki18 18:23412e950394 463 s4 = (float)(- vcurr);
fanny868 0:9072e932503c 464
fanny868 0:9072e932503c 465 saki=true;
rahmadirizki18 3:1287fccc11be 466 maju=kiri=kanan=saka=mundur=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 467
rahmadirizki18 17:703072f5dce1 468 //pc.printf("saki\n");
fanny868 0:9072e932503c 469
rahmadirizki18 16:89093194ccc2 470 //motor1.speed(s1);
rahmadirizki18 16:89093194ccc2 471 motor1.brake(1);
rahmadirizki18 16:89093194ccc2 472 motor2.speed(s2);
rahmadirizki18 16:89093194ccc2 473 //motor3.speed(s3);
rahmadirizki18 16:89093194ccc2 474 motor3.brake(1);
rahmadirizki18 16:89093194ccc2 475 motor4.speed(s4);
fanny868 0:9072e932503c 476
fanny868 0:9072e932503c 477 break;
fanny868 0:9072e932503c 478 }
fanny868 0:9072e932503c 479 case (8) :
fanny868 0:9072e932503c 480 {
rahmadirizki18 16:89093194ccc2 481 if (sbki) {
rahmadirizki18 16:89093194ccc2 482 if(vcurr<0.1) {
rahmadirizki18 16:89093194ccc2 483 vcurr=0.1;
rahmadirizki18 16:89093194ccc2 484 } else {
rahmadirizki18 16:89093194ccc2 485 vcurr+=ax;
rahmadirizki18 16:89093194ccc2 486 }
rahmadirizki18 16:89093194ccc2 487 //perlambatan=0;
rahmadirizki18 16:89093194ccc2 488 } else {
rahmadirizki18 16:89093194ccc2 489 //perlambatan=1;
rahmadirizki18 16:89093194ccc2 490 }
rahmadirizki18 16:89093194ccc2 491
rahmadirizki18 16:89093194ccc2 492 if (vcurr>=vmaxserong) {
rahmadirizki18 16:89093194ccc2 493 vcurr=vmaxserong;
rahmadirizki18 16:89093194ccc2 494 }
fanny868 0:9072e932503c 495
rahmadirizki18 16:89093194ccc2 496
rahmadirizki18 18:23412e950394 497 s1 = (float)( vcurr);
rahmadirizki18 18:23412e950394 498 s2 = (float)(0); //- 0.1*vcurr;
rahmadirizki18 18:23412e950394 499 s3 = (float)(- vcurr);
rahmadirizki18 18:23412e950394 500 s4 = (float)(0); // 0.1*vcurr;
rahmadirizki18 16:89093194ccc2 501
rahmadirizki18 16:89093194ccc2 502 sbki=true;
rahmadirizki18 16:89093194ccc2 503 maju=kiri=kanan=saka=saki=sbka=mundur=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
rahmadirizki18 16:89093194ccc2 504
rahmadirizki18 17:703072f5dce1 505 //pc.printf("sbki\n");
rahmadirizki18 16:89093194ccc2 506
rahmadirizki18 16:89093194ccc2 507 motor1.speed(s1);
rahmadirizki18 16:89093194ccc2 508 //motor2.speed(s2);
rahmadirizki18 16:89093194ccc2 509 motor2.brake(1);
rahmadirizki18 16:89093194ccc2 510 motor3.speed(s3);
rahmadirizki18 16:89093194ccc2 511 //motor4.speed(s4);
rahmadirizki18 16:89093194ccc2 512 motor4.brake(1);
fanny868 0:9072e932503c 513
fanny868 0:9072e932503c 514 break;
fanny868 0:9072e932503c 515 }
fanny868 0:9072e932503c 516 case (9) :
fanny868 0:9072e932503c 517 {
rahmadirizki18 16:89093194ccc2 518 if (kanan) {
rahmadirizki18 16:89093194ccc2 519 if(vcurr<0.1) {
rahmadirizki18 16:89093194ccc2 520 vcurr=0.1;
rahmadirizki18 16:89093194ccc2 521 } else {
rahmadirizki18 16:89093194ccc2 522 vcurr+=ax;
rahmadirizki18 16:89093194ccc2 523 }
rahmadirizki18 16:89093194ccc2 524 //perlambatan=0;
rahmadirizki18 16:89093194ccc2 525 } else {
rahmadirizki18 16:89093194ccc2 526 //perlambatan=1;
rahmadirizki18 16:89093194ccc2 527 }
rahmadirizki18 16:89093194ccc2 528
rahmadirizki18 16:89093194ccc2 529 if (vcurr>=vmax) {
rahmadirizki18 16:89093194ccc2 530 vcurr=vmax;
rahmadirizki18 16:89093194ccc2 531 }
rahmadirizki18 16:89093194ccc2 532
rahmadirizki18 16:89093194ccc2 533
rahmadirizki18 18:23412e950394 534 s1 =(float)(-1* vcurr);
rahmadirizki18 18:23412e950394 535 s2 =(float)(-1.0* vcurr);
rahmadirizki18 18:23412e950394 536 s3 =(float)(1* vcurr);
rahmadirizki18 18:23412e950394 537 s4 =(float)(1.0* (vcurr+0.005));
rahmadirizki18 16:89093194ccc2 538
fanny868 0:9072e932503c 539 kanan=true;
rahmadirizki18 3:1287fccc11be 540 maju=kiri=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 541
rahmadirizki18 17:703072f5dce1 542 //pc.printf("Kanan\n");
fanny868 0:9072e932503c 543
rahmadirizki18 16:89093194ccc2 544 motor1.speed(s1);
rahmadirizki18 16:89093194ccc2 545 motor2.speed(s2);
rahmadirizki18 16:89093194ccc2 546 motor3.speed(s3);
rahmadirizki18 16:89093194ccc2 547 motor4.speed(s4);
fanny868 0:9072e932503c 548 break;
fanny868 0:9072e932503c 549 }
fanny868 0:9072e932503c 550 case (10) :
fanny868 0:9072e932503c 551 {
rahmadirizki18 16:89093194ccc2 552 if (kiri) {
rahmadirizki18 16:89093194ccc2 553 if(vcurr<0.1) {
rahmadirizki18 16:89093194ccc2 554 vcurr=0.1;
rahmadirizki18 16:89093194ccc2 555 } else {
rahmadirizki18 16:89093194ccc2 556 vcurr+=ax;
rahmadirizki18 16:89093194ccc2 557 }
rahmadirizki18 16:89093194ccc2 558 //perlambatan=1;
rahmadirizki18 16:89093194ccc2 559 } else {
rahmadirizki18 16:89093194ccc2 560 //perlambatan=1;
rahmadirizki18 16:89093194ccc2 561 }
rahmadirizki18 16:89093194ccc2 562
rahmadirizki18 16:89093194ccc2 563 if (vcurr>=vmax) {
rahmadirizki18 16:89093194ccc2 564 vcurr=vmax;
rahmadirizki18 16:89093194ccc2 565 }
rahmadirizki18 16:89093194ccc2 566
rahmadirizki18 16:89093194ccc2 567
rahmadirizki18 18:23412e950394 568 s1 =(float)(1* vcurr);
rahmadirizki18 18:23412e950394 569 s2 =(float)(1* (vcurr+0.003));
rahmadirizki18 18:23412e950394 570 s3 =(float)(-1* vcurr);
rahmadirizki18 18:23412e950394 571 s4 =(float)(-1.0* vcurr);
fanny868 0:9072e932503c 572
fanny868 0:9072e932503c 573 kiri=true;
rahmadirizki18 3:1287fccc11be 574 maju=kanan=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 575
rahmadirizki18 17:703072f5dce1 576 //pc.printf("Kiri\n");
fanny868 0:9072e932503c 577
rahmadirizki18 16:89093194ccc2 578 motor1.speed(s1);
rahmadirizki18 16:89093194ccc2 579 motor2.speed(s2);
rahmadirizki18 16:89093194ccc2 580 motor3.speed(s3);
rahmadirizki18 16:89093194ccc2 581 motor4.speed(s4);
fanny868 0:9072e932503c 582
fanny868 0:9072e932503c 583 break;
fanny868 0:9072e932503c 584 }
rahmadirizki18 3:1287fccc11be 585 case (11):
rahmadirizki18 3:1287fccc11be 586 {
rahmadirizki18 4:483c07cc22e1 587
rahmadirizki18 16:89093194ccc2 588
rahmadirizki18 16:89093194ccc2 589 s1t = (vmaxanalog*(-x+y));
rahmadirizki18 16:89093194ccc2 590 s2t = (vmaxanalog*(-x-y));
rahmadirizki18 16:89093194ccc2 591 s3t = (vmaxanalog*(x-y));
rahmadirizki18 16:89093194ccc2 592 s4t = (vmaxanalog*(x+y));
rahmadirizki18 16:89093194ccc2 593
rahmadirizki18 18:23412e950394 594 s1 = (float)(0.5* s1t);
rahmadirizki18 18:23412e950394 595 s2 = (float)(0.5* s2t);
rahmadirizki18 18:23412e950394 596 s3 = (float)(0.5* s3t);
rahmadirizki18 18:23412e950394 597 s4 = (float)(0.5* s4t);
rahmadirizki18 16:89093194ccc2 598
rahmadirizki18 16:89093194ccc2 599
rahmadirizki18 3:1287fccc11be 600
rahmadirizki18 3:1287fccc11be 601 analog=true;
rahmadirizki18 3:1287fccc11be 602 maju=mundur=kiri=kanan=saka=saki=sbka=sbki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
rahmadirizki18 3:1287fccc11be 603
rahmadirizki18 17:703072f5dce1 604 //pc.printf("analog X =%.2f Y =%.2f \n ",x,y);
rahmadirizki18 3:1287fccc11be 605
rahmadirizki18 16:89093194ccc2 606
rahmadirizki18 16:89093194ccc2 607 motor1.speed(s1);
rahmadirizki18 16:89093194ccc2 608 motor2.speed(s2);
rahmadirizki18 16:89093194ccc2 609 motor3.speed(s3);
rahmadirizki18 16:89093194ccc2 610 motor4.speed(s4);
rahmadirizki18 3:1287fccc11be 611 break;
rahmadirizki18 3:1287fccc11be 612 }
rahmadirizki18 3:1287fccc11be 613 default :
fanny868 0:9072e932503c 614 {
rahmadirizki18 16:89093194ccc2 615 motor1.brake(1);
rahmadirizki18 16:89093194ccc2 616 motor2.brake(1);
rahmadirizki18 16:89093194ccc2 617 motor3.brake(1);
rahmadirizki18 16:89093194ccc2 618 motor4.brake(1);
rahmadirizki18 16:89093194ccc2 619
rahmadirizki18 3:1287fccc11be 620 maju=mundur=kiri=kanan=saka=saki=sbka=sbki=analog=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 621 stop = true;
fanny868 0:9072e932503c 622
fanny868 0:9072e932503c 623
rahmadirizki18 17:703072f5dce1 624 //pc.printf("Stop\n");
rahmadirizki18 16:89093194ccc2 625 }
rahmadirizki18 5:3aa203218306 626 }
rahmadirizki18 5:3aa203218306 627 }
rahmadirizki18 5:3aa203218306 628
rahmadirizki18 5:3aa203218306 629
rahmadirizki18 5:3aa203218306 630
rahmadirizki18 7:d138c56dab20 631 void speedLauncher()
rahmadirizki18 7:d138c56dab20 632 {
franshendri 12:e07c59c28c29 633 if (joystick.R3_click and speedL < 0.8){
franshendri 12:e07c59c28c29 634 speedL = speedL + 0.01;}
rahmadirizki18 7:d138c56dab20 635 if (joystick.L3_click and speedL > 0.1){
franshendri 12:e07c59c28c29 636 speedL = speedL - 0.01;}
rahmadirizki18 13:8ab42383a2ca 637 if (joystick.R2_click and speedB < 0.8 ){
rahmadirizki18 13:8ab42383a2ca 638 speedB = speedB + 0.01;}
rahmadirizki18 13:8ab42383a2ca 639 if (joystick.L2_click and speedB > 0.1 ){
rahmadirizki18 13:8ab42383a2ca 640 speedB = speedB - 0.01;}
rahmadirizki18 15:98f0d56b14f0 641 //pc.printf("Pwm depan = %.3f\t Pwm belakang = %.3f\n", speedL, speedB);
rahmadirizki18 7:d138c56dab20 642 }
franshendri 12:e07c59c28c29 643
franshendri 12:e07c59c28c29 644
franshendri 12:e07c59c28c29 645
fanny868 0:9072e932503c 646 int main (void)
fanny868 0:9072e932503c 647 {
fanny868 0:9072e932503c 648 // Set baud rate - 115200
fanny868 0:9072e932503c 649 joystick.setup();
rahmadirizki18 19:bb304933a9f8 650 pc.baud(57600);
rahmadirizki18 5:3aa203218306 651 wait_ms(500);
rahmadirizki18 5:3aa203218306 652
rahmadirizki18 3:1287fccc11be 653 pc.printf("Ready...\n");
rahmadirizki18 4:483c07cc22e1 654 kalibrasi();
fanny868 0:9072e932503c 655 while(1)
fanny868 0:9072e932503c 656 {
fanny868 0:9072e932503c 657 // Interrupt Serial
fanny868 0:9072e932503c 658 joystick.idle();
fanny868 0:9072e932503c 659 if(joystick.readable() ) {
fanny868 0:9072e932503c 660 // Panggil fungsi pembacaan joystik
fanny868 0:9072e932503c 661 joystick.baca_data();
fanny868 0:9072e932503c 662 // Panggil fungsi pengolahan data joystik
fanny868 0:9072e932503c 663 joystick.olah_data();
franshendri 12:e07c59c28c29 664 //pc.printf("%3d %3d\n\r",joystick.R2, joystick.L2);
fanny868 0:9072e932503c 665 //pc.printf("%2x %2x %2x %2x %3d %3d %3d %3d %3d %3d\n\r",joystick.button, joystick.RL, joystick.button_click, joystick.RL_click, joystick.R2, joystick.L2, joystick.RX, joystick.RY, joystick.LX, joystick.LY);
fanny868 0:9072e932503c 666 case_ger = case_gerak();
rahmadirizki18 3:1287fccc11be 667 aktuator();
rahmadirizki18 6:68293bed71ea 668
franshendri 12:e07c59c28c29 669 //pc.printf("bacaS = %.2f\tbacaB = %.2f\n",servoS.read(), servoB.read());
rahmadirizki18 6:68293bed71ea 670
rahmadirizki18 3:1287fccc11be 671 //kalibrasi
rahmadirizki18 4:483c07cc22e1 672 if (joystick.START){
rahmadirizki18 4:483c07cc22e1 673 kalibrasi();}
rahmadirizki18 4:483c07cc22e1 674 // analog switch mode
rahmadirizki18 6:68293bed71ea 675 if (joystick.SELECT_click) {analog = !analog;}
rahmadirizki18 6:68293bed71ea 676 if (joystick.segitiga_click) {Launcher = !Launcher;}
rahmadirizki18 6:68293bed71ea 677 if (joystick.lingkaran_click){
rahmadirizki18 6:68293bed71ea 678 ServoGo = true;
rahmadirizki18 6:68293bed71ea 679 }
rahmadirizki18 19:bb304933a9f8 680 if (joystick.silang) {
rahmadirizki18 19:bb304933a9f8 681 pc.printf("Depan = %.3f \t Belakang = %.3f \n",speedB,speedL);
rahmadirizki18 19:bb304933a9f8 682 }
rahmadirizki18 7:d138c56dab20 683 speedLauncher();
franshendri 12:e07c59c28c29 684
fanny868 0:9072e932503c 685 } else {
fanny868 0:9072e932503c 686 joystick.idle();
fanny868 0:9072e932503c 687
fanny868 0:9072e932503c 688 }
rahmadirizki18 16:89093194ccc2 689 // gotoXYT(XT,YT,Tetha);
rahmadirizki18 5:3aa203218306 690
fanny868 0:9072e932503c 691 }
fanny868 0:9072e932503c 692 }