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@17:703072f5dce1, 2017-01-14 (annotated)

Committer:
rahmadirizki18
Date:
Sat Jan 14 08:01:09 2017 +0000
Revision:
17:703072f5dce1
Parent:
16:89093194ccc2
Child:
18:23412e950394
yoi;

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;
franshendri 12:e07c59c28c29 45 float speedB=0.43 ;
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 3:1287fccc11be 68 Serial pc(USBTX,USBRX);
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 koef;
rahmadirizki18 16:89093194ccc2 218 double s1=0,s2=0,s3=0,s4=0,s1t=0,s2t=0,s3t=0,s4t=0;
rahmadirizki18 16:89093194ccc2 219
rahmadirizki18 16:89093194ccc2 220 // MOTOR BAWAH
fanny868 0:9072e932503c 221 switch (case_ger)
fanny868 0:9072e932503c 222 {
rahmadirizki18 3:1287fccc11be 223 case (1):
fanny868 0:9072e932503c 224 {
rahmadirizki18 16:89093194ccc2 225 if (pivka) {
rahmadirizki18 16:89093194ccc2 226 if(vcurr<0.1) {
rahmadirizki18 16:89093194ccc2 227 vcurr=0.1;
rahmadirizki18 16:89093194ccc2 228 } else {
rahmadirizki18 16:89093194ccc2 229 vcurr+=ax;
rahmadirizki18 16:89093194ccc2 230 }
rahmadirizki18 16:89093194ccc2 231 //perlambatan=0;
rahmadirizki18 16:89093194ccc2 232 } else {
rahmadirizki18 16:89093194ccc2 233 //perlambatan=1;
rahmadirizki18 16:89093194ccc2 234 }
rahmadirizki18 16:89093194ccc2 235
rahmadirizki18 16:89093194ccc2 236 if (vcurr>=vmaxpivot) {
rahmadirizki18 16:89093194ccc2 237 vcurr=vmaxpivot;
rahmadirizki18 16:89093194ccc2 238 }
rahmadirizki18 16:89093194ccc2 239
rahmadirizki18 16:89093194ccc2 240 if(joystick.R2==255 && joystick.L2==0) {
rahmadirizki18 16:89093194ccc2 241 koef=2;
rahmadirizki18 16:89093194ccc2 242 } else if (joystick.L2==255 && joystick.R2==0) {
rahmadirizki18 16:89093194ccc2 243 koef=0.5;
rahmadirizki18 16:89093194ccc2 244 }
rahmadirizki18 16:89093194ccc2 245 else {
rahmadirizki18 16:89093194ccc2 246 koef=1;
rahmadirizki18 16:89093194ccc2 247 }
rahmadirizki18 16:89093194ccc2 248
rahmadirizki18 16:89093194ccc2 249 s1 = (float)(-0.5*koef*vcurr);
rahmadirizki18 16:89093194ccc2 250 s2 = (float)(-0.5*koef*vcurr);
rahmadirizki18 16:89093194ccc2 251 s3 = (float)(-0.5*koef*vcurr);
rahmadirizki18 16:89093194ccc2 252 s4 = (float)(-0.5*koef*vcurr);
rahmadirizki18 16:89093194ccc2 253
fanny868 0:9072e932503c 254 pivka=true;
rahmadirizki18 3:1287fccc11be 255 maju=mundur=analog=kiri=kanan=saka=saki=sbka=sbki=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 256
rahmadirizki18 17:703072f5dce1 257 //pc.printf("pivKa\n");
fanny868 0:9072e932503c 258
rahmadirizki18 16:89093194ccc2 259 motor1.speed(s1);
rahmadirizki18 16:89093194ccc2 260 motor2.speed(s2);
rahmadirizki18 16:89093194ccc2 261 motor3.speed(s3);
rahmadirizki18 16:89093194ccc2 262 motor4.speed(s4);
rahmadirizki18 16:89093194ccc2 263
fanny868 0:9072e932503c 264 break;
fanny868 0:9072e932503c 265 }
fanny868 0:9072e932503c 266 case (2):
fanny868 0:9072e932503c 267 {
rahmadirizki18 16:89093194ccc2 268 if (pivki){
rahmadirizki18 16:89093194ccc2 269 if(vcurr<0.1) {
rahmadirizki18 16:89093194ccc2 270 vcurr=0.1;
rahmadirizki18 16:89093194ccc2 271 } else {
rahmadirizki18 16:89093194ccc2 272 vcurr+=ax;
rahmadirizki18 16:89093194ccc2 273 }
rahmadirizki18 16:89093194ccc2 274 //perlambatan=0;
rahmadirizki18 16:89093194ccc2 275 } else {
rahmadirizki18 16:89093194ccc2 276 //perlambatan=1;
rahmadirizki18 16:89093194ccc2 277 }
rahmadirizki18 16:89093194ccc2 278
rahmadirizki18 16:89093194ccc2 279 if (vcurr>=vmaxpivot) {
rahmadirizki18 16:89093194ccc2 280 vcurr=vmaxpivot;
rahmadirizki18 16:89093194ccc2 281 }
rahmadirizki18 16:89093194ccc2 282
rahmadirizki18 16:89093194ccc2 283 if(joystick.R2==255 && joystick.L2==0) {
rahmadirizki18 16:89093194ccc2 284 koef=2;
rahmadirizki18 16:89093194ccc2 285 } else if (joystick.L2==255 && joystick.R2==0) {
rahmadirizki18 16:89093194ccc2 286 koef=0.5;
rahmadirizki18 16:89093194ccc2 287 } else {
rahmadirizki18 16:89093194ccc2 288 koef=1;
rahmadirizki18 16:89093194ccc2 289 }
rahmadirizki18 16:89093194ccc2 290
rahmadirizki18 16:89093194ccc2 291 s1 = (float)(0.5*koef*vcurr);
rahmadirizki18 16:89093194ccc2 292 s2 = (float)(0.5*koef*vcurr);
rahmadirizki18 16:89093194ccc2 293 s3 = (float)(0.5*koef*vcurr);
rahmadirizki18 16:89093194ccc2 294 s4 = (float)(0.5*koef*vcurr);
fanny868 0:9072e932503c 295
fanny868 0:9072e932503c 296 pivki=true;
rahmadirizki18 3:1287fccc11be 297 maju=mundur=kiri=analog=kanan=saka=saki=sbka=sbki=pivka=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 298
rahmadirizki18 17:703072f5dce1 299 //pc.printf("pivKi\n");
fanny868 0:9072e932503c 300
rahmadirizki18 16:89093194ccc2 301 motor1.speed(s1);
rahmadirizki18 16:89093194ccc2 302 motor2.speed(s2);
rahmadirizki18 16:89093194ccc2 303 motor3.speed(s3);
rahmadirizki18 16:89093194ccc2 304 motor4.speed(s4);
rahmadirizki18 16:89093194ccc2 305
fanny868 0:9072e932503c 306 break;
fanny868 0:9072e932503c 307 }
fanny868 0:9072e932503c 308 case (3):
fanny868 0:9072e932503c 309 {
rahmadirizki18 16:89093194ccc2 310 if (maju) {
rahmadirizki18 16:89093194ccc2 311 if(vcurr<0.1) {
rahmadirizki18 16:89093194ccc2 312 vcurr=0.1;
rahmadirizki18 16:89093194ccc2 313 } else {
rahmadirizki18 16:89093194ccc2 314 vcurr+=ax;
rahmadirizki18 16:89093194ccc2 315 }
rahmadirizki18 16:89093194ccc2 316 //perlambatan=0;
rahmadirizki18 16:89093194ccc2 317 } else {
rahmadirizki18 16:89093194ccc2 318 //perlambatan=1;
rahmadirizki18 16:89093194ccc2 319 }
rahmadirizki18 16:89093194ccc2 320
rahmadirizki18 16:89093194ccc2 321 if (vcurr>=vmax) {
rahmadirizki18 16:89093194ccc2 322 vcurr=vmax;
rahmadirizki18 16:89093194ccc2 323 }
rahmadirizki18 16:89093194ccc2 324
rahmadirizki18 16:89093194ccc2 325 if(joystick.R2==255 && joystick.L2==0) {
rahmadirizki18 16:89093194ccc2 326 koef=2;
rahmadirizki18 16:89093194ccc2 327 } else if (joystick.L2==255 && joystick.R2==0) { koef=0.5;}
rahmadirizki18 16:89093194ccc2 328 else {
rahmadirizki18 16:89093194ccc2 329 koef=1;
rahmadirizki18 16:89093194ccc2 330 }
rahmadirizki18 16:89093194ccc2 331
rahmadirizki18 16:89093194ccc2 332 //Case s1 untuk mode L2 lebih lambat
rahmadirizki18 17:703072f5dce1 333 s1 = (float)(-1*koef*(vcurr));
rahmadirizki18 16:89093194ccc2 334 s2 = (float)(1.0*koef*vcurr);
rahmadirizki18 16:89093194ccc2 335 s3 = (float)(1.0*koef*vcurr);
rahmadirizki18 16:89093194ccc2 336 s4 = (float)(-1*koef*vcurr);
rahmadirizki18 16:89093194ccc2 337
rahmadirizki18 16:89093194ccc2 338 //s1 =-0.8*koef*vcurr;
rahmadirizki18 16:89093194ccc2 339 //s2 =koef*vcurr;
rahmadirizki18 16:89093194ccc2 340 //s3 =-koef*vcurr;
rahmadirizki18 16:89093194ccc2 341 //s4 =koef*vcurr;
fanny868 0:9072e932503c 342
fanny868 0:9072e932503c 343 maju=true;
rahmadirizki18 3:1287fccc11be 344 mundur=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 345
rahmadirizki18 17:703072f5dce1 346 //pc.printf("maju\n");
fanny868 0:9072e932503c 347
rahmadirizki18 16:89093194ccc2 348 motor1.speed(s1);
rahmadirizki18 16:89093194ccc2 349 motor2.speed(s2);
rahmadirizki18 16:89093194ccc2 350 motor3.speed(s3);
rahmadirizki18 16:89093194ccc2 351 motor4.speed(s4);
rahmadirizki18 16:89093194ccc2 352
fanny868 0:9072e932503c 353 break;
fanny868 0:9072e932503c 354 }
fanny868 0:9072e932503c 355 case (4):
fanny868 0:9072e932503c 356 {
rahmadirizki18 16:89093194ccc2 357 if (mundur) {
rahmadirizki18 16:89093194ccc2 358 if(vcurr<0.1) {
rahmadirizki18 16:89093194ccc2 359 vcurr=0.1;
rahmadirizki18 16:89093194ccc2 360 } else {
rahmadirizki18 16:89093194ccc2 361 vcurr+=ax;
rahmadirizki18 16:89093194ccc2 362 }
rahmadirizki18 16:89093194ccc2 363 //perlambatan=0;
rahmadirizki18 16:89093194ccc2 364 } else {
rahmadirizki18 16:89093194ccc2 365 //perlambatan=1;
rahmadirizki18 16:89093194ccc2 366 }
rahmadirizki18 16:89093194ccc2 367
rahmadirizki18 16:89093194ccc2 368 if (vcurr>=vmax) {
rahmadirizki18 16:89093194ccc2 369 vcurr=vmax;
rahmadirizki18 16:89093194ccc2 370 }
rahmadirizki18 16:89093194ccc2 371
rahmadirizki18 16:89093194ccc2 372 if(joystick.R2==255 && joystick.L2==0) {
rahmadirizki18 16:89093194ccc2 373 koef=2;
rahmadirizki18 16:89093194ccc2 374 } else if (joystick.L2==255 && joystick.R2==0) {
rahmadirizki18 16:89093194ccc2 375 koef=0.5;
rahmadirizki18 16:89093194ccc2 376 } else {
rahmadirizki18 16:89093194ccc2 377 koef=1;
rahmadirizki18 16:89093194ccc2 378 }
rahmadirizki18 17:703072f5dce1 379
rahmadirizki18 17:703072f5dce1 380 s1 = (float)(1*koef*(vcurr));
rahmadirizki18 17:703072f5dce1 381 s2 = (float)(-1*koef*(vcurr));
rahmadirizki18 17:703072f5dce1 382 s3 = (float)(-1*koef*(vcurr));
rahmadirizki18 17:703072f5dce1 383 s4 = (float)(1*koef*(vcurr));
fanny868 0:9072e932503c 384
fanny868 0:9072e932503c 385 mundur=true;
rahmadirizki18 3:1287fccc11be 386 maju=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 387
rahmadirizki18 17:703072f5dce1 388 //pc.printf("mundur\n");
fanny868 0:9072e932503c 389
rahmadirizki18 16:89093194ccc2 390 motor1.speed(s1);
rahmadirizki18 16:89093194ccc2 391 motor2.speed(s2);
rahmadirizki18 16:89093194ccc2 392 motor3.speed(s3);
rahmadirizki18 16:89093194ccc2 393 motor4.speed(s4);
fanny868 0:9072e932503c 394
fanny868 0:9072e932503c 395 break;
fanny868 0:9072e932503c 396 }
fanny868 0:9072e932503c 397 case (5) :
fanny868 0:9072e932503c 398 {
rahmadirizki18 16:89093194ccc2 399 if (saka) {
rahmadirizki18 16:89093194ccc2 400 if(vcurr<0.1) {
rahmadirizki18 16:89093194ccc2 401 vcurr=0.1;
rahmadirizki18 16:89093194ccc2 402 } else {
rahmadirizki18 16:89093194ccc2 403 vcurr+=ax;
rahmadirizki18 16:89093194ccc2 404 }
rahmadirizki18 16:89093194ccc2 405 //perlambatan=0;
rahmadirizki18 16:89093194ccc2 406 } else {
rahmadirizki18 16:89093194ccc2 407 //perlambatan=1;
rahmadirizki18 16:89093194ccc2 408 }
rahmadirizki18 16:89093194ccc2 409
rahmadirizki18 16:89093194ccc2 410 if (vcurr>=vmax) {
rahmadirizki18 16:89093194ccc2 411 vcurr=vmax;
rahmadirizki18 16:89093194ccc2 412 } if(joystick.R2==255 && joystick.L2==0) {
rahmadirizki18 16:89093194ccc2 413 koef=2;
rahmadirizki18 16:89093194ccc2 414 } else if (joystick.L2==255 && joystick.R2==0) {
rahmadirizki18 16:89093194ccc2 415 koef=0.5;
rahmadirizki18 16:89093194ccc2 416 } else {
rahmadirizki18 16:89093194ccc2 417 koef=1;
rahmadirizki18 16:89093194ccc2 418 }
rahmadirizki18 16:89093194ccc2 419
rahmadirizki18 16:89093194ccc2 420 s1 = (float)(-koef*vcurr);
rahmadirizki18 16:89093194ccc2 421 s2 = (float)(0); //koef*0.1*vcurr;
rahmadirizki18 16:89093194ccc2 422 s3 = (float)(koef*vcurr);
rahmadirizki18 16:89093194ccc2 423 s4 = (float)(0); //-koef*0.1*vcurr;
fanny868 0:9072e932503c 424
fanny868 0:9072e932503c 425 saka=true;
rahmadirizki18 3:1287fccc11be 426 maju=mundur=kiri=kanan=sbka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 427
rahmadirizki18 17:703072f5dce1 428 //pc.printf("saka\n");
fanny868 0:9072e932503c 429
rahmadirizki18 16:89093194ccc2 430 motor1.speed(s1);
rahmadirizki18 16:89093194ccc2 431 motor2.brake(1);
rahmadirizki18 16:89093194ccc2 432 //motor2.speed(s2);
rahmadirizki18 16:89093194ccc2 433 motor3.speed(s3);
rahmadirizki18 16:89093194ccc2 434 motor4.brake(1);
rahmadirizki18 16:89093194ccc2 435 //motor4.speed(s4);
fanny868 0:9072e932503c 436
fanny868 0:9072e932503c 437 break;
fanny868 0:9072e932503c 438 }
fanny868 0:9072e932503c 439 case (6) :
fanny868 0:9072e932503c 440 {
rahmadirizki18 16:89093194ccc2 441 if (sbka){
rahmadirizki18 16:89093194ccc2 442 if(vcurr<0.1) {
rahmadirizki18 16:89093194ccc2 443 vcurr=0.1;
rahmadirizki18 16:89093194ccc2 444 } else {
rahmadirizki18 16:89093194ccc2 445 vcurr+=ax;
rahmadirizki18 16:89093194ccc2 446 }
rahmadirizki18 16:89093194ccc2 447 //perlambatan=0;
rahmadirizki18 16:89093194ccc2 448 } else {
rahmadirizki18 16:89093194ccc2 449 //perlambatan=1;
rahmadirizki18 16:89093194ccc2 450 }
rahmadirizki18 16:89093194ccc2 451
rahmadirizki18 16:89093194ccc2 452 if (vcurr>=vmaxserong) {
rahmadirizki18 16:89093194ccc2 453 vcurr=vmaxserong;
rahmadirizki18 16:89093194ccc2 454 }
rahmadirizki18 16:89093194ccc2 455
rahmadirizki18 16:89093194ccc2 456 if(joystick.R2==255 && joystick.L2==0) {
rahmadirizki18 16:89093194ccc2 457 koef=2;
rahmadirizki18 16:89093194ccc2 458 } else if (joystick.L2==255 && joystick.R2==0) {
rahmadirizki18 16:89093194ccc2 459 koef=0.5;
rahmadirizki18 16:89093194ccc2 460 } else {
rahmadirizki18 16:89093194ccc2 461 koef=1;
rahmadirizki18 16:89093194ccc2 462 }
rahmadirizki18 16:89093194ccc2 463
rahmadirizki18 16:89093194ccc2 464 s1 = (float)(0); //koef*0.1*vcurr;
rahmadirizki18 16:89093194ccc2 465 s2 = (float)(-koef*vcurr);
rahmadirizki18 16:89093194ccc2 466 s3 = (float)(0); //-koef*0.1*vcurr;
rahmadirizki18 16:89093194ccc2 467 s4 = (float)(koef*vcurr);
fanny868 0:9072e932503c 468
fanny868 0:9072e932503c 469 sbka=true;
rahmadirizki18 3:1287fccc11be 470 maju=mundur=kiri=kanan=saka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 471
rahmadirizki18 17:703072f5dce1 472 //pc.printf("sbka\n");
fanny868 0:9072e932503c 473
rahmadirizki18 16:89093194ccc2 474 //motor1.speed(s1);
rahmadirizki18 16:89093194ccc2 475 motor1.brake(1);
rahmadirizki18 16:89093194ccc2 476 motor2.speed(s2);
rahmadirizki18 16:89093194ccc2 477 //motor3.speed(s3);
rahmadirizki18 16:89093194ccc2 478 motor3.brake(1);
rahmadirizki18 16:89093194ccc2 479 motor4.speed(s4);
fanny868 0:9072e932503c 480
fanny868 0:9072e932503c 481 break;
fanny868 0:9072e932503c 482 }
fanny868 0:9072e932503c 483 case (7) :
fanny868 0:9072e932503c 484 {
rahmadirizki18 16:89093194ccc2 485 if (saki) {
rahmadirizki18 16:89093194ccc2 486 if(vcurr<0.1) {
rahmadirizki18 16:89093194ccc2 487 vcurr=0.1;
rahmadirizki18 16:89093194ccc2 488 } else {
rahmadirizki18 16:89093194ccc2 489 vcurr+=ax;
rahmadirizki18 16:89093194ccc2 490 }
rahmadirizki18 16:89093194ccc2 491 //perlambatan=0;
rahmadirizki18 16:89093194ccc2 492 } else {
rahmadirizki18 16:89093194ccc2 493 //perlambatan=1;
rahmadirizki18 16:89093194ccc2 494 }
rahmadirizki18 16:89093194ccc2 495
rahmadirizki18 16:89093194ccc2 496 if (vcurr>=vmaxserong) {
rahmadirizki18 16:89093194ccc2 497 vcurr=vmaxserong;
rahmadirizki18 16:89093194ccc2 498 }
rahmadirizki18 16:89093194ccc2 499
rahmadirizki18 16:89093194ccc2 500 if(joystick.R2==255 && joystick.L2==0) {
rahmadirizki18 16:89093194ccc2 501 koef=2;
rahmadirizki18 16:89093194ccc2 502 } else if (joystick.L2==255 && joystick.R2==0) {
rahmadirizki18 16:89093194ccc2 503 koef=0.5;
rahmadirizki18 16:89093194ccc2 504 } else {
rahmadirizki18 16:89093194ccc2 505 koef=1;
rahmadirizki18 16:89093194ccc2 506 }
rahmadirizki18 16:89093194ccc2 507
rahmadirizki18 16:89093194ccc2 508 s1 = (float)(0); //-koef*0.1*vcurr;
rahmadirizki18 16:89093194ccc2 509 s2 = (float)(koef*vcurr);
rahmadirizki18 16:89093194ccc2 510 s3 = (float)(0); //koef*0.1*vcurr;
rahmadirizki18 16:89093194ccc2 511 s4 = (float)(-koef*vcurr);
fanny868 0:9072e932503c 512
fanny868 0:9072e932503c 513 saki=true;
rahmadirizki18 3:1287fccc11be 514 maju=kiri=kanan=saka=mundur=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 515
rahmadirizki18 17:703072f5dce1 516 //pc.printf("saki\n");
fanny868 0:9072e932503c 517
rahmadirizki18 16:89093194ccc2 518 //motor1.speed(s1);
rahmadirizki18 16:89093194ccc2 519 motor1.brake(1);
rahmadirizki18 16:89093194ccc2 520 motor2.speed(s2);
rahmadirizki18 16:89093194ccc2 521 //motor3.speed(s3);
rahmadirizki18 16:89093194ccc2 522 motor3.brake(1);
rahmadirizki18 16:89093194ccc2 523 motor4.speed(s4);
fanny868 0:9072e932503c 524
fanny868 0:9072e932503c 525 break;
fanny868 0:9072e932503c 526 }
fanny868 0:9072e932503c 527 case (8) :
fanny868 0:9072e932503c 528 {
rahmadirizki18 16:89093194ccc2 529 if (sbki) {
rahmadirizki18 16:89093194ccc2 530 if(vcurr<0.1) {
rahmadirizki18 16:89093194ccc2 531 vcurr=0.1;
rahmadirizki18 16:89093194ccc2 532 } else {
rahmadirizki18 16:89093194ccc2 533 vcurr+=ax;
rahmadirizki18 16:89093194ccc2 534 }
rahmadirizki18 16:89093194ccc2 535 //perlambatan=0;
rahmadirizki18 16:89093194ccc2 536 } else {
rahmadirizki18 16:89093194ccc2 537 //perlambatan=1;
rahmadirizki18 16:89093194ccc2 538 }
rahmadirizki18 16:89093194ccc2 539
rahmadirizki18 16:89093194ccc2 540 if (vcurr>=vmaxserong) {
rahmadirizki18 16:89093194ccc2 541 vcurr=vmaxserong;
rahmadirizki18 16:89093194ccc2 542 }
fanny868 0:9072e932503c 543
rahmadirizki18 16:89093194ccc2 544 if(joystick.R2==255 && joystick.L2==0) {
rahmadirizki18 16:89093194ccc2 545 koef=2;
rahmadirizki18 16:89093194ccc2 546 } else if (joystick.L2==255 && joystick.R2==0) {
rahmadirizki18 16:89093194ccc2 547 koef=0.5;
rahmadirizki18 16:89093194ccc2 548 } else {
rahmadirizki18 16:89093194ccc2 549 koef=1;
rahmadirizki18 16:89093194ccc2 550 }
rahmadirizki18 16:89093194ccc2 551
rahmadirizki18 16:89093194ccc2 552 s1 = (float)(koef*vcurr);
rahmadirizki18 16:89093194ccc2 553 s2 = (float)(0); //-koef*0.1*vcurr;
rahmadirizki18 16:89093194ccc2 554 s3 = (float)(-koef*vcurr);
rahmadirizki18 16:89093194ccc2 555 s4 = (float)(0); //koef*0.1*vcurr;
rahmadirizki18 16:89093194ccc2 556
rahmadirizki18 16:89093194ccc2 557 sbki=true;
rahmadirizki18 16:89093194ccc2 558 maju=kiri=kanan=saka=saki=sbka=mundur=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
rahmadirizki18 16:89093194ccc2 559
rahmadirizki18 17:703072f5dce1 560 //pc.printf("sbki\n");
rahmadirizki18 16:89093194ccc2 561
rahmadirizki18 16:89093194ccc2 562 motor1.speed(s1);
rahmadirizki18 16:89093194ccc2 563 //motor2.speed(s2);
rahmadirizki18 16:89093194ccc2 564 motor2.brake(1);
rahmadirizki18 16:89093194ccc2 565 motor3.speed(s3);
rahmadirizki18 16:89093194ccc2 566 //motor4.speed(s4);
rahmadirizki18 16:89093194ccc2 567 motor4.brake(1);
fanny868 0:9072e932503c 568
fanny868 0:9072e932503c 569 break;
fanny868 0:9072e932503c 570 }
fanny868 0:9072e932503c 571 case (9) :
fanny868 0:9072e932503c 572 {
rahmadirizki18 16:89093194ccc2 573 if (kanan) {
rahmadirizki18 16:89093194ccc2 574 if(vcurr<0.1) {
rahmadirizki18 16:89093194ccc2 575 vcurr=0.1;
rahmadirizki18 16:89093194ccc2 576 } else {
rahmadirizki18 16:89093194ccc2 577 vcurr+=ax;
rahmadirizki18 16:89093194ccc2 578 }
rahmadirizki18 16:89093194ccc2 579 //perlambatan=0;
rahmadirizki18 16:89093194ccc2 580 } else {
rahmadirizki18 16:89093194ccc2 581 //perlambatan=1;
rahmadirizki18 16:89093194ccc2 582 }
rahmadirizki18 16:89093194ccc2 583
rahmadirizki18 16:89093194ccc2 584 if (vcurr>=vmax) {
rahmadirizki18 16:89093194ccc2 585 vcurr=vmax;
rahmadirizki18 16:89093194ccc2 586 }
rahmadirizki18 16:89093194ccc2 587
rahmadirizki18 16:89093194ccc2 588 if(joystick.R2==255 && joystick.L2==0) {
rahmadirizki18 16:89093194ccc2 589 koef=2;
rahmadirizki18 16:89093194ccc2 590 } else if (joystick.L2==255 && joystick.R2==0) {
rahmadirizki18 16:89093194ccc2 591 koef=0.5;
rahmadirizki18 16:89093194ccc2 592 } else {
rahmadirizki18 16:89093194ccc2 593 koef=1;
rahmadirizki18 16:89093194ccc2 594 }
rahmadirizki18 16:89093194ccc2 595
rahmadirizki18 16:89093194ccc2 596 s1 =(float)(-1*koef*vcurr);
rahmadirizki18 16:89093194ccc2 597 s2 =(float)(-1.0*koef*vcurr);
rahmadirizki18 16:89093194ccc2 598 s3 =(float)(1*koef*vcurr);
rahmadirizki18 17:703072f5dce1 599 s4 =(float)(1.0*koef*(vcurr+0.005));
rahmadirizki18 16:89093194ccc2 600
fanny868 0:9072e932503c 601 kanan=true;
rahmadirizki18 3:1287fccc11be 602 maju=kiri=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 603
rahmadirizki18 17:703072f5dce1 604 //pc.printf("Kanan\n");
fanny868 0:9072e932503c 605
rahmadirizki18 16:89093194ccc2 606 motor1.speed(s1);
rahmadirizki18 16:89093194ccc2 607 motor2.speed(s2);
rahmadirizki18 16:89093194ccc2 608 motor3.speed(s3);
rahmadirizki18 16:89093194ccc2 609 motor4.speed(s4);
fanny868 0:9072e932503c 610 break;
fanny868 0:9072e932503c 611 }
fanny868 0:9072e932503c 612 case (10) :
fanny868 0:9072e932503c 613 {
rahmadirizki18 16:89093194ccc2 614 if (kiri) {
rahmadirizki18 16:89093194ccc2 615 if(vcurr<0.1) {
rahmadirizki18 16:89093194ccc2 616 vcurr=0.1;
rahmadirizki18 16:89093194ccc2 617 } else {
rahmadirizki18 16:89093194ccc2 618 vcurr+=ax;
rahmadirizki18 16:89093194ccc2 619 }
rahmadirizki18 16:89093194ccc2 620 //perlambatan=1;
rahmadirizki18 16:89093194ccc2 621 } else {
rahmadirizki18 16:89093194ccc2 622 //perlambatan=1;
rahmadirizki18 16:89093194ccc2 623 }
rahmadirizki18 16:89093194ccc2 624
rahmadirizki18 16:89093194ccc2 625 if (vcurr>=vmax) {
rahmadirizki18 16:89093194ccc2 626 vcurr=vmax;
rahmadirizki18 16:89093194ccc2 627 }
rahmadirizki18 16:89093194ccc2 628
rahmadirizki18 16:89093194ccc2 629 if(joystick.R2==255 && joystick.L2==0) {
rahmadirizki18 16:89093194ccc2 630 koef=2;
rahmadirizki18 16:89093194ccc2 631 } else if (joystick.L2==255 && joystick.R2==0) {
rahmadirizki18 16:89093194ccc2 632 koef=0.5;
rahmadirizki18 16:89093194ccc2 633 } else {
rahmadirizki18 16:89093194ccc2 634 koef=1;
rahmadirizki18 16:89093194ccc2 635 }
rahmadirizki18 16:89093194ccc2 636
rahmadirizki18 16:89093194ccc2 637 s1 =(float)(1*koef*vcurr);
rahmadirizki18 17:703072f5dce1 638 s2 =(float)(1*koef*(vcurr+0.003));
rahmadirizki18 16:89093194ccc2 639 s3 =(float)(-1*koef*vcurr);
rahmadirizki18 16:89093194ccc2 640 s4 =(float)(-1.0*koef*vcurr);
fanny868 0:9072e932503c 641
fanny868 0:9072e932503c 642 kiri=true;
rahmadirizki18 3:1287fccc11be 643 maju=kanan=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 644
rahmadirizki18 17:703072f5dce1 645 //pc.printf("Kiri\n");
fanny868 0:9072e932503c 646
rahmadirizki18 16:89093194ccc2 647 motor1.speed(s1);
rahmadirizki18 16:89093194ccc2 648 motor2.speed(s2);
rahmadirizki18 16:89093194ccc2 649 motor3.speed(s3);
rahmadirizki18 16:89093194ccc2 650 motor4.speed(s4);
fanny868 0:9072e932503c 651
fanny868 0:9072e932503c 652 break;
fanny868 0:9072e932503c 653 }
rahmadirizki18 3:1287fccc11be 654 case (11):
rahmadirizki18 3:1287fccc11be 655 {
rahmadirizki18 4:483c07cc22e1 656
rahmadirizki18 16:89093194ccc2 657 if(joystick.R2==255 && joystick.L2==0) {
rahmadirizki18 16:89093194ccc2 658 koef=2;
rahmadirizki18 16:89093194ccc2 659 } else if (joystick.L2==255 && joystick.R2==0) {
rahmadirizki18 16:89093194ccc2 660 koef=0.5;
rahmadirizki18 16:89093194ccc2 661 }
rahmadirizki18 16:89093194ccc2 662 else {
rahmadirizki18 16:89093194ccc2 663 koef=1;
rahmadirizki18 16:89093194ccc2 664 }
rahmadirizki18 16:89093194ccc2 665
rahmadirizki18 16:89093194ccc2 666 s1t = (vmaxanalog*(-x+y));
rahmadirizki18 16:89093194ccc2 667 s2t = (vmaxanalog*(-x-y));
rahmadirizki18 16:89093194ccc2 668 s3t = (vmaxanalog*(x-y));
rahmadirizki18 16:89093194ccc2 669 s4t = (vmaxanalog*(x+y));
rahmadirizki18 16:89093194ccc2 670
rahmadirizki18 16:89093194ccc2 671 s1 = (float)(0.5*koef*s1t);
rahmadirizki18 16:89093194ccc2 672 s2 = (float)(0.5*koef*s2t);
rahmadirizki18 16:89093194ccc2 673 s3 = (float)(0.5*koef*s3t);
rahmadirizki18 16:89093194ccc2 674 s4 = (float)(0.5*koef*s4t);
rahmadirizki18 16:89093194ccc2 675
rahmadirizki18 16:89093194ccc2 676
rahmadirizki18 3:1287fccc11be 677
rahmadirizki18 3:1287fccc11be 678 analog=true;
rahmadirizki18 3:1287fccc11be 679 maju=mundur=kiri=kanan=saka=saki=sbka=sbki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
rahmadirizki18 3:1287fccc11be 680
rahmadirizki18 17:703072f5dce1 681 //pc.printf("analog X =%.2f Y =%.2f \n ",x,y);
rahmadirizki18 3:1287fccc11be 682
rahmadirizki18 16:89093194ccc2 683
rahmadirizki18 16:89093194ccc2 684 motor1.speed(s1);
rahmadirizki18 16:89093194ccc2 685 motor2.speed(s2);
rahmadirizki18 16:89093194ccc2 686 motor3.speed(s3);
rahmadirizki18 16:89093194ccc2 687 motor4.speed(s4);
rahmadirizki18 3:1287fccc11be 688 break;
rahmadirizki18 3:1287fccc11be 689 }
rahmadirizki18 3:1287fccc11be 690 default :
fanny868 0:9072e932503c 691 {
rahmadirizki18 16:89093194ccc2 692 motor1.brake(1);
rahmadirizki18 16:89093194ccc2 693 motor2.brake(1);
rahmadirizki18 16:89093194ccc2 694 motor3.brake(1);
rahmadirizki18 16:89093194ccc2 695 motor4.brake(1);
rahmadirizki18 16:89093194ccc2 696
rahmadirizki18 3:1287fccc11be 697 maju=mundur=kiri=kanan=saka=saki=sbka=sbki=analog=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 698 stop = true;
fanny868 0:9072e932503c 699
fanny868 0:9072e932503c 700
rahmadirizki18 17:703072f5dce1 701 //pc.printf("Stop\n");
rahmadirizki18 16:89093194ccc2 702 }
rahmadirizki18 5:3aa203218306 703 }
rahmadirizki18 5:3aa203218306 704 }
rahmadirizki18 5:3aa203218306 705
rahmadirizki18 5:3aa203218306 706
rahmadirizki18 5:3aa203218306 707
rahmadirizki18 7:d138c56dab20 708 void speedLauncher()
rahmadirizki18 7:d138c56dab20 709 {
franshendri 12:e07c59c28c29 710 if (joystick.R3_click and speedL < 0.8){
franshendri 12:e07c59c28c29 711 speedL = speedL + 0.01;}
rahmadirizki18 7:d138c56dab20 712 if (joystick.L3_click and speedL > 0.1){
franshendri 12:e07c59c28c29 713 speedL = speedL - 0.01;}
rahmadirizki18 13:8ab42383a2ca 714 if (joystick.R2_click and speedB < 0.8 ){
rahmadirizki18 13:8ab42383a2ca 715 speedB = speedB + 0.01;}
rahmadirizki18 13:8ab42383a2ca 716 if (joystick.L2_click and speedB > 0.1 ){
rahmadirizki18 13:8ab42383a2ca 717 speedB = speedB - 0.01;}
rahmadirizki18 15:98f0d56b14f0 718 //pc.printf("Pwm depan = %.3f\t Pwm belakang = %.3f\n", speedL, speedB);
rahmadirizki18 7:d138c56dab20 719 }
franshendri 12:e07c59c28c29 720
franshendri 12:e07c59c28c29 721
franshendri 12:e07c59c28c29 722
fanny868 0:9072e932503c 723 int main (void)
fanny868 0:9072e932503c 724 {
fanny868 0:9072e932503c 725 // Set baud rate - 115200
fanny868 0:9072e932503c 726 joystick.setup();
rahmadirizki18 3:1287fccc11be 727 pc.baud(115200);
rahmadirizki18 5:3aa203218306 728 wait_ms(500);
rahmadirizki18 5:3aa203218306 729
rahmadirizki18 3:1287fccc11be 730 pc.printf("Ready...\n");
rahmadirizki18 4:483c07cc22e1 731 kalibrasi();
fanny868 0:9072e932503c 732 while(1)
fanny868 0:9072e932503c 733 {
fanny868 0:9072e932503c 734 // Interrupt Serial
fanny868 0:9072e932503c 735 joystick.idle();
fanny868 0:9072e932503c 736 if(joystick.readable() ) {
fanny868 0:9072e932503c 737 // Panggil fungsi pembacaan joystik
fanny868 0:9072e932503c 738 joystick.baca_data();
fanny868 0:9072e932503c 739 // Panggil fungsi pengolahan data joystik
fanny868 0:9072e932503c 740 joystick.olah_data();
franshendri 12:e07c59c28c29 741 //pc.printf("%3d %3d\n\r",joystick.R2, joystick.L2);
fanny868 0:9072e932503c 742 //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 743 case_ger = case_gerak();
rahmadirizki18 3:1287fccc11be 744 aktuator();
rahmadirizki18 6:68293bed71ea 745
franshendri 12:e07c59c28c29 746 //pc.printf("bacaS = %.2f\tbacaB = %.2f\n",servoS.read(), servoB.read());
rahmadirizki18 6:68293bed71ea 747
rahmadirizki18 3:1287fccc11be 748 //kalibrasi
rahmadirizki18 4:483c07cc22e1 749 if (joystick.START){
rahmadirizki18 4:483c07cc22e1 750 kalibrasi();}
rahmadirizki18 4:483c07cc22e1 751 // analog switch mode
rahmadirizki18 6:68293bed71ea 752 if (joystick.SELECT_click) {analog = !analog;}
rahmadirizki18 6:68293bed71ea 753 if (joystick.segitiga_click) {Launcher = !Launcher;}
rahmadirizki18 6:68293bed71ea 754 if (joystick.lingkaran_click){
rahmadirizki18 6:68293bed71ea 755 ServoGo = true;
rahmadirizki18 6:68293bed71ea 756 }
rahmadirizki18 7:d138c56dab20 757 speedLauncher();
franshendri 12:e07c59c28c29 758
fanny868 0:9072e932503c 759 } else {
fanny868 0:9072e932503c 760 joystick.idle();
fanny868 0:9072e932503c 761
fanny868 0:9072e932503c 762 }
rahmadirizki18 16:89093194ccc2 763 // gotoXYT(XT,YT,Tetha);
rahmadirizki18 5:3aa203218306 764
fanny868 0:9072e932503c 765 }
fanny868 0:9072e932503c 766 }