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@16:89093194ccc2, 2017-01-12 (annotated)

Committer:
rahmadirizki18
Date:
Thu Jan 12 11:15:01 2017 +0000
Revision:
16:89093194ccc2
Parent:
15:98f0d56b14f0
Child:
17:703072f5dce1
Manual_JoystickV1

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 16:89093194ccc2 33 #define vmax 1
rahmadirizki18 16:89093194ccc2 34 #define vmaxserong 0.9
rahmadirizki18 16:89093194ccc2 35 #define vmaxpivot 0.7
rahmadirizki18 16:89093194ccc2 36 #define vmaxanalog 0.9
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 14:6d389e99981c 51 Motor motor1(PB_7, PA_14 , PA_15); // pwm, fwd, rev
rahmadirizki18 14:6d389e99981c 52 Motor motor2(PB_8, PA_13 ,PB_0); // pwm, fwd, rev
rahmadirizki18 15:98f0d56b14f0 53 Motor motor3(PB_9, PA_12 ,PC_5); // pwm, fwd, rev
rahmadirizki18 14:6d389e99981c 54 Motor motor4(PB_6, PB_1 ,PB_12); // 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 16:89093194ccc2 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 16:89093194ccc2 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 16:89093194ccc2 333 s1 = (float)(-1*koef*(vcurr+0.005));
rahmadirizki18 16:89093194ccc2 334
rahmadirizki18 16:89093194ccc2 335 s2 = (float)(1.0*koef*vcurr);
rahmadirizki18 16:89093194ccc2 336 s3 = (float)(1.0*koef*vcurr);
rahmadirizki18 16:89093194ccc2 337 s4 = (float)(-1*koef*vcurr);
rahmadirizki18 16:89093194ccc2 338
rahmadirizki18 16:89093194ccc2 339 //s1 =-0.8*koef*vcurr;
rahmadirizki18 16:89093194ccc2 340 //s2 =koef*vcurr;
rahmadirizki18 16:89093194ccc2 341 //s3 =-koef*vcurr;
rahmadirizki18 16:89093194ccc2 342 //s4 =koef*vcurr;
fanny868 0:9072e932503c 343
fanny868 0:9072e932503c 344 maju=true;
rahmadirizki18 3:1287fccc11be 345 mundur=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 346
rahmadirizki18 16:89093194ccc2 347 pc.printf("maju\n");
fanny868 0:9072e932503c 348
rahmadirizki18 16:89093194ccc2 349 motor1.speed(s1);
rahmadirizki18 16:89093194ccc2 350 motor2.speed(s2);
rahmadirizki18 16:89093194ccc2 351 motor3.speed(s3);
rahmadirizki18 16:89093194ccc2 352 motor4.speed(s4);
rahmadirizki18 16:89093194ccc2 353
fanny868 0:9072e932503c 354 break;
fanny868 0:9072e932503c 355 }
fanny868 0:9072e932503c 356 case (4):
fanny868 0:9072e932503c 357 {
rahmadirizki18 16:89093194ccc2 358 if (mundur) {
rahmadirizki18 16:89093194ccc2 359 if(vcurr<0.1) {
rahmadirizki18 16:89093194ccc2 360 vcurr=0.1;
rahmadirizki18 16:89093194ccc2 361 } else {
rahmadirizki18 16:89093194ccc2 362 vcurr+=ax;
rahmadirizki18 16:89093194ccc2 363 }
rahmadirizki18 16:89093194ccc2 364 //perlambatan=0;
rahmadirizki18 16:89093194ccc2 365 } else {
rahmadirizki18 16:89093194ccc2 366 //perlambatan=1;
rahmadirizki18 16:89093194ccc2 367 }
rahmadirizki18 16:89093194ccc2 368
rahmadirizki18 16:89093194ccc2 369 if (vcurr>=vmax) {
rahmadirizki18 16:89093194ccc2 370 vcurr=vmax;
rahmadirizki18 16:89093194ccc2 371 }
rahmadirizki18 16:89093194ccc2 372
rahmadirizki18 16:89093194ccc2 373 if(joystick.R2==255 && joystick.L2==0) {
rahmadirizki18 16:89093194ccc2 374 koef=2;
rahmadirizki18 16:89093194ccc2 375 } else if (joystick.L2==255 && joystick.R2==0) {
rahmadirizki18 16:89093194ccc2 376 koef=0.5;
rahmadirizki18 16:89093194ccc2 377 } else {
rahmadirizki18 16:89093194ccc2 378 koef=1;
rahmadirizki18 16:89093194ccc2 379 }
rahmadirizki18 16:89093194ccc2 380 //Motor 4 telat mulai
rahmadirizki18 16:89093194ccc2 381 s1 = (float)(1*koef*(vcurr-0.008));
rahmadirizki18 16:89093194ccc2 382 s2 = (float)(-1*koef*(vcurr-0.005));
rahmadirizki18 16:89093194ccc2 383 s3 = (float)(-1*koef*(vcurr-0.005));
rahmadirizki18 16:89093194ccc2 384 s4 = (float)(1*koef*(vcurr+0.005));
fanny868 0:9072e932503c 385
fanny868 0:9072e932503c 386 mundur=true;
rahmadirizki18 3:1287fccc11be 387 maju=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 388
rahmadirizki18 16:89093194ccc2 389 pc.printf("mundur\n");
fanny868 0:9072e932503c 390
rahmadirizki18 16:89093194ccc2 391 motor1.speed(s1);
rahmadirizki18 16:89093194ccc2 392 motor2.speed(s2);
rahmadirizki18 16:89093194ccc2 393 motor3.speed(s3);
rahmadirizki18 16:89093194ccc2 394 motor4.speed(s4);
fanny868 0:9072e932503c 395
fanny868 0:9072e932503c 396 break;
fanny868 0:9072e932503c 397 }
fanny868 0:9072e932503c 398 case (5) :
fanny868 0:9072e932503c 399 {
rahmadirizki18 16:89093194ccc2 400 if (saka) {
rahmadirizki18 16:89093194ccc2 401 if(vcurr<0.1) {
rahmadirizki18 16:89093194ccc2 402 vcurr=0.1;
rahmadirizki18 16:89093194ccc2 403 } else {
rahmadirizki18 16:89093194ccc2 404 vcurr+=ax;
rahmadirizki18 16:89093194ccc2 405 }
rahmadirizki18 16:89093194ccc2 406 //perlambatan=0;
rahmadirizki18 16:89093194ccc2 407 } else {
rahmadirizki18 16:89093194ccc2 408 //perlambatan=1;
rahmadirizki18 16:89093194ccc2 409 }
rahmadirizki18 16:89093194ccc2 410
rahmadirizki18 16:89093194ccc2 411 if (vcurr>=vmax) {
rahmadirizki18 16:89093194ccc2 412 vcurr=vmax;
rahmadirizki18 16:89093194ccc2 413 } if(joystick.R2==255 && joystick.L2==0) {
rahmadirizki18 16:89093194ccc2 414 koef=2;
rahmadirizki18 16:89093194ccc2 415 } else if (joystick.L2==255 && joystick.R2==0) {
rahmadirizki18 16:89093194ccc2 416 koef=0.5;
rahmadirizki18 16:89093194ccc2 417 } else {
rahmadirizki18 16:89093194ccc2 418 koef=1;
rahmadirizki18 16:89093194ccc2 419 }
rahmadirizki18 16:89093194ccc2 420
rahmadirizki18 16:89093194ccc2 421 s1 = (float)(-koef*vcurr);
rahmadirizki18 16:89093194ccc2 422 s2 = (float)(0); //koef*0.1*vcurr;
rahmadirizki18 16:89093194ccc2 423 s3 = (float)(koef*vcurr);
rahmadirizki18 16:89093194ccc2 424 s4 = (float)(0); //-koef*0.1*vcurr;
fanny868 0:9072e932503c 425
fanny868 0:9072e932503c 426 saka=true;
rahmadirizki18 3:1287fccc11be 427 maju=mundur=kiri=kanan=sbka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 428
rahmadirizki18 16:89093194ccc2 429 pc.printf("saka\n");
fanny868 0:9072e932503c 430
rahmadirizki18 16:89093194ccc2 431 motor1.speed(s1);
rahmadirizki18 16:89093194ccc2 432 motor2.brake(1);
rahmadirizki18 16:89093194ccc2 433 //motor2.speed(s2);
rahmadirizki18 16:89093194ccc2 434 motor3.speed(s3);
rahmadirizki18 16:89093194ccc2 435 motor4.brake(1);
rahmadirizki18 16:89093194ccc2 436 //motor4.speed(s4);
fanny868 0:9072e932503c 437
fanny868 0:9072e932503c 438 break;
fanny868 0:9072e932503c 439 }
fanny868 0:9072e932503c 440 case (6) :
fanny868 0:9072e932503c 441 {
rahmadirizki18 16:89093194ccc2 442 if (sbka){
rahmadirizki18 16:89093194ccc2 443 if(vcurr<0.1) {
rahmadirizki18 16:89093194ccc2 444 vcurr=0.1;
rahmadirizki18 16:89093194ccc2 445 } else {
rahmadirizki18 16:89093194ccc2 446 vcurr+=ax;
rahmadirizki18 16:89093194ccc2 447 }
rahmadirizki18 16:89093194ccc2 448 //perlambatan=0;
rahmadirizki18 16:89093194ccc2 449 } else {
rahmadirizki18 16:89093194ccc2 450 //perlambatan=1;
rahmadirizki18 16:89093194ccc2 451 }
rahmadirizki18 16:89093194ccc2 452
rahmadirizki18 16:89093194ccc2 453 if (vcurr>=vmaxserong) {
rahmadirizki18 16:89093194ccc2 454 vcurr=vmaxserong;
rahmadirizki18 16:89093194ccc2 455 }
rahmadirizki18 16:89093194ccc2 456
rahmadirizki18 16:89093194ccc2 457 if(joystick.R2==255 && joystick.L2==0) {
rahmadirizki18 16:89093194ccc2 458 koef=2;
rahmadirizki18 16:89093194ccc2 459 } else if (joystick.L2==255 && joystick.R2==0) {
rahmadirizki18 16:89093194ccc2 460 koef=0.5;
rahmadirizki18 16:89093194ccc2 461 } else {
rahmadirizki18 16:89093194ccc2 462 koef=1;
rahmadirizki18 16:89093194ccc2 463 }
rahmadirizki18 16:89093194ccc2 464
rahmadirizki18 16:89093194ccc2 465 s1 = (float)(0); //koef*0.1*vcurr;
rahmadirizki18 16:89093194ccc2 466 s2 = (float)(-koef*vcurr);
rahmadirizki18 16:89093194ccc2 467 s3 = (float)(0); //-koef*0.1*vcurr;
rahmadirizki18 16:89093194ccc2 468 s4 = (float)(koef*vcurr);
fanny868 0:9072e932503c 469
fanny868 0:9072e932503c 470 sbka=true;
rahmadirizki18 3:1287fccc11be 471 maju=mundur=kiri=kanan=saka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 472
rahmadirizki18 16:89093194ccc2 473 pc.printf("sbka\n");
fanny868 0:9072e932503c 474
rahmadirizki18 16:89093194ccc2 475 //motor1.speed(s1);
rahmadirizki18 16:89093194ccc2 476 motor1.brake(1);
rahmadirizki18 16:89093194ccc2 477 motor2.speed(s2);
rahmadirizki18 16:89093194ccc2 478 //motor3.speed(s3);
rahmadirizki18 16:89093194ccc2 479 motor3.brake(1);
rahmadirizki18 16:89093194ccc2 480 motor4.speed(s4);
fanny868 0:9072e932503c 481
fanny868 0:9072e932503c 482 break;
fanny868 0:9072e932503c 483 }
fanny868 0:9072e932503c 484 case (7) :
fanny868 0:9072e932503c 485 {
rahmadirizki18 16:89093194ccc2 486 if (saki) {
rahmadirizki18 16:89093194ccc2 487 if(vcurr<0.1) {
rahmadirizki18 16:89093194ccc2 488 vcurr=0.1;
rahmadirizki18 16:89093194ccc2 489 } else {
rahmadirizki18 16:89093194ccc2 490 vcurr+=ax;
rahmadirizki18 16:89093194ccc2 491 }
rahmadirizki18 16:89093194ccc2 492 //perlambatan=0;
rahmadirizki18 16:89093194ccc2 493 } else {
rahmadirizki18 16:89093194ccc2 494 //perlambatan=1;
rahmadirizki18 16:89093194ccc2 495 }
rahmadirizki18 16:89093194ccc2 496
rahmadirizki18 16:89093194ccc2 497 if (vcurr>=vmaxserong) {
rahmadirizki18 16:89093194ccc2 498 vcurr=vmaxserong;
rahmadirizki18 16:89093194ccc2 499 }
rahmadirizki18 16:89093194ccc2 500
rahmadirizki18 16:89093194ccc2 501 if(joystick.R2==255 && joystick.L2==0) {
rahmadirizki18 16:89093194ccc2 502 koef=2;
rahmadirizki18 16:89093194ccc2 503 } else if (joystick.L2==255 && joystick.R2==0) {
rahmadirizki18 16:89093194ccc2 504 koef=0.5;
rahmadirizki18 16:89093194ccc2 505 } else {
rahmadirizki18 16:89093194ccc2 506 koef=1;
rahmadirizki18 16:89093194ccc2 507 }
rahmadirizki18 16:89093194ccc2 508
rahmadirizki18 16:89093194ccc2 509 s1 = (float)(0); //-koef*0.1*vcurr;
rahmadirizki18 16:89093194ccc2 510 s2 = (float)(koef*vcurr);
rahmadirizki18 16:89093194ccc2 511 s3 = (float)(0); //koef*0.1*vcurr;
rahmadirizki18 16:89093194ccc2 512 s4 = (float)(-koef*vcurr);
fanny868 0:9072e932503c 513
fanny868 0:9072e932503c 514 saki=true;
rahmadirizki18 3:1287fccc11be 515 maju=kiri=kanan=saka=mundur=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 516
rahmadirizki18 16:89093194ccc2 517 pc.printf("saki\n");
fanny868 0:9072e932503c 518
rahmadirizki18 16:89093194ccc2 519 //motor1.speed(s1);
rahmadirizki18 16:89093194ccc2 520 motor1.brake(1);
rahmadirizki18 16:89093194ccc2 521 motor2.speed(s2);
rahmadirizki18 16:89093194ccc2 522 //motor3.speed(s3);
rahmadirizki18 16:89093194ccc2 523 motor3.brake(1);
rahmadirizki18 16:89093194ccc2 524 motor4.speed(s4);
fanny868 0:9072e932503c 525
fanny868 0:9072e932503c 526 break;
fanny868 0:9072e932503c 527 }
fanny868 0:9072e932503c 528 case (8) :
fanny868 0:9072e932503c 529 {
rahmadirizki18 16:89093194ccc2 530 if (sbki) {
rahmadirizki18 16:89093194ccc2 531 if(vcurr<0.1) {
rahmadirizki18 16:89093194ccc2 532 vcurr=0.1;
rahmadirizki18 16:89093194ccc2 533 } else {
rahmadirizki18 16:89093194ccc2 534 vcurr+=ax;
rahmadirizki18 16:89093194ccc2 535 }
rahmadirizki18 16:89093194ccc2 536 //perlambatan=0;
rahmadirizki18 16:89093194ccc2 537 } else {
rahmadirizki18 16:89093194ccc2 538 //perlambatan=1;
rahmadirizki18 16:89093194ccc2 539 }
rahmadirizki18 16:89093194ccc2 540
rahmadirizki18 16:89093194ccc2 541 if (vcurr>=vmaxserong) {
rahmadirizki18 16:89093194ccc2 542 vcurr=vmaxserong;
rahmadirizki18 16:89093194ccc2 543 }
fanny868 0:9072e932503c 544
rahmadirizki18 16:89093194ccc2 545 if(joystick.R2==255 && joystick.L2==0) {
rahmadirizki18 16:89093194ccc2 546 koef=2;
rahmadirizki18 16:89093194ccc2 547 } else if (joystick.L2==255 && joystick.R2==0) {
rahmadirizki18 16:89093194ccc2 548 koef=0.5;
rahmadirizki18 16:89093194ccc2 549 } else {
rahmadirizki18 16:89093194ccc2 550 koef=1;
rahmadirizki18 16:89093194ccc2 551 }
rahmadirizki18 16:89093194ccc2 552
rahmadirizki18 16:89093194ccc2 553 s1 = (float)(koef*vcurr);
rahmadirizki18 16:89093194ccc2 554 s2 = (float)(0); //-koef*0.1*vcurr;
rahmadirizki18 16:89093194ccc2 555 s3 = (float)(-koef*vcurr);
rahmadirizki18 16:89093194ccc2 556 s4 = (float)(0); //koef*0.1*vcurr;
rahmadirizki18 16:89093194ccc2 557
rahmadirizki18 16:89093194ccc2 558 sbki=true;
rahmadirizki18 16:89093194ccc2 559 maju=kiri=kanan=saka=saki=sbka=mundur=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
rahmadirizki18 16:89093194ccc2 560
rahmadirizki18 16:89093194ccc2 561 pc.printf("sbki\n");
rahmadirizki18 16:89093194ccc2 562
rahmadirizki18 16:89093194ccc2 563 motor1.speed(s1);
rahmadirizki18 16:89093194ccc2 564 //motor2.speed(s2);
rahmadirizki18 16:89093194ccc2 565 motor2.brake(1);
rahmadirizki18 16:89093194ccc2 566 motor3.speed(s3);
rahmadirizki18 16:89093194ccc2 567 //motor4.speed(s4);
rahmadirizki18 16:89093194ccc2 568 motor4.brake(1);
fanny868 0:9072e932503c 569
fanny868 0:9072e932503c 570 break;
fanny868 0:9072e932503c 571 }
fanny868 0:9072e932503c 572 case (9) :
fanny868 0:9072e932503c 573 {
rahmadirizki18 16:89093194ccc2 574 if (kanan) {
rahmadirizki18 16:89093194ccc2 575 if(vcurr<0.1) {
rahmadirizki18 16:89093194ccc2 576 vcurr=0.1;
rahmadirizki18 16:89093194ccc2 577 } else {
rahmadirizki18 16:89093194ccc2 578 vcurr+=ax;
rahmadirizki18 16:89093194ccc2 579 }
rahmadirizki18 16:89093194ccc2 580 //perlambatan=0;
rahmadirizki18 16:89093194ccc2 581 } else {
rahmadirizki18 16:89093194ccc2 582 //perlambatan=1;
rahmadirizki18 16:89093194ccc2 583 }
rahmadirizki18 16:89093194ccc2 584
rahmadirizki18 16:89093194ccc2 585 if (vcurr>=vmax) {
rahmadirizki18 16:89093194ccc2 586 vcurr=vmax;
rahmadirizki18 16:89093194ccc2 587 }
rahmadirizki18 16:89093194ccc2 588
rahmadirizki18 16:89093194ccc2 589 if(joystick.R2==255 && joystick.L2==0) {
rahmadirizki18 16:89093194ccc2 590 koef=2;
rahmadirizki18 16:89093194ccc2 591 } else if (joystick.L2==255 && joystick.R2==0) {
rahmadirizki18 16:89093194ccc2 592 koef=0.5;
rahmadirizki18 16:89093194ccc2 593 } else {
rahmadirizki18 16:89093194ccc2 594 koef=1;
rahmadirizki18 16:89093194ccc2 595 }
rahmadirizki18 16:89093194ccc2 596
rahmadirizki18 16:89093194ccc2 597 s1 =(float)(-1*koef*vcurr);
rahmadirizki18 16:89093194ccc2 598 s2 =(float)(-1.0*koef*vcurr);
rahmadirizki18 16:89093194ccc2 599 s3 =(float)(1*koef*vcurr);
rahmadirizki18 16:89093194ccc2 600 s4 =(float)(1.0*koef*vcurr);
rahmadirizki18 16:89093194ccc2 601
fanny868 0:9072e932503c 602 kanan=true;
rahmadirizki18 3:1287fccc11be 603 maju=kiri=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 604
rahmadirizki18 16:89093194ccc2 605 pc.printf("Kanan\n");
fanny868 0:9072e932503c 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);
fanny868 0:9072e932503c 611 break;
fanny868 0:9072e932503c 612 }
fanny868 0:9072e932503c 613 case (10) :
fanny868 0:9072e932503c 614 {
rahmadirizki18 16:89093194ccc2 615 if (kiri) {
rahmadirizki18 16:89093194ccc2 616 if(vcurr<0.1) {
rahmadirizki18 16:89093194ccc2 617 vcurr=0.1;
rahmadirizki18 16:89093194ccc2 618 } else {
rahmadirizki18 16:89093194ccc2 619 vcurr+=ax;
rahmadirizki18 16:89093194ccc2 620 }
rahmadirizki18 16:89093194ccc2 621 //perlambatan=1;
rahmadirizki18 16:89093194ccc2 622 } else {
rahmadirizki18 16:89093194ccc2 623 //perlambatan=1;
rahmadirizki18 16:89093194ccc2 624 }
rahmadirizki18 16:89093194ccc2 625
rahmadirizki18 16:89093194ccc2 626 if (vcurr>=vmax) {
rahmadirizki18 16:89093194ccc2 627 vcurr=vmax;
rahmadirizki18 16:89093194ccc2 628 }
rahmadirizki18 16:89093194ccc2 629
rahmadirizki18 16:89093194ccc2 630 if(joystick.R2==255 && joystick.L2==0) {
rahmadirizki18 16:89093194ccc2 631 koef=2;
rahmadirizki18 16:89093194ccc2 632 } else if (joystick.L2==255 && joystick.R2==0) {
rahmadirizki18 16:89093194ccc2 633 koef=0.5;
rahmadirizki18 16:89093194ccc2 634 } else {
rahmadirizki18 16:89093194ccc2 635 koef=1;
rahmadirizki18 16:89093194ccc2 636 }
rahmadirizki18 16:89093194ccc2 637
rahmadirizki18 16:89093194ccc2 638 s1 =(float)(1*koef*vcurr);
rahmadirizki18 16:89093194ccc2 639 s2 =(float)(1*koef*vcurr);
rahmadirizki18 16:89093194ccc2 640 s3 =(float)(-1*koef*vcurr);
rahmadirizki18 16:89093194ccc2 641 s4 =(float)(-1.0*koef*vcurr);
fanny868 0:9072e932503c 642
fanny868 0:9072e932503c 643 kiri=true;
rahmadirizki18 3:1287fccc11be 644 maju=kanan=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 645
rahmadirizki18 16:89093194ccc2 646 pc.printf("Kiri\n");
fanny868 0:9072e932503c 647
rahmadirizki18 16:89093194ccc2 648 motor1.speed(s1);
rahmadirizki18 16:89093194ccc2 649 motor2.speed(s2);
rahmadirizki18 16:89093194ccc2 650 motor3.speed(s3);
rahmadirizki18 16:89093194ccc2 651 motor4.speed(s4);
fanny868 0:9072e932503c 652
fanny868 0:9072e932503c 653 break;
fanny868 0:9072e932503c 654 }
rahmadirizki18 3:1287fccc11be 655 case (11):
rahmadirizki18 3:1287fccc11be 656 {
rahmadirizki18 4:483c07cc22e1 657
rahmadirizki18 16:89093194ccc2 658 if(joystick.R2==255 && joystick.L2==0) {
rahmadirizki18 16:89093194ccc2 659 koef=2;
rahmadirizki18 16:89093194ccc2 660 } else if (joystick.L2==255 && joystick.R2==0) {
rahmadirizki18 16:89093194ccc2 661 koef=0.5;
rahmadirizki18 16:89093194ccc2 662 }
rahmadirizki18 16:89093194ccc2 663 else {
rahmadirizki18 16:89093194ccc2 664 koef=1;
rahmadirizki18 16:89093194ccc2 665 }
rahmadirizki18 16:89093194ccc2 666
rahmadirizki18 16:89093194ccc2 667 s1t = (vmaxanalog*(-x+y));
rahmadirizki18 16:89093194ccc2 668 s2t = (vmaxanalog*(-x-y));
rahmadirizki18 16:89093194ccc2 669 s3t = (vmaxanalog*(x-y));
rahmadirizki18 16:89093194ccc2 670 s4t = (vmaxanalog*(x+y));
rahmadirizki18 16:89093194ccc2 671
rahmadirizki18 16:89093194ccc2 672 s1 = (float)(0.5*koef*s1t);
rahmadirizki18 16:89093194ccc2 673 s2 = (float)(0.5*koef*s2t);
rahmadirizki18 16:89093194ccc2 674 s3 = (float)(0.5*koef*s3t);
rahmadirizki18 16:89093194ccc2 675 s4 = (float)(0.5*koef*s4t);
rahmadirizki18 16:89093194ccc2 676
rahmadirizki18 16:89093194ccc2 677
rahmadirizki18 3:1287fccc11be 678
rahmadirizki18 3:1287fccc11be 679 analog=true;
rahmadirizki18 3:1287fccc11be 680 maju=mundur=kiri=kanan=saka=saki=sbka=sbki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
rahmadirizki18 3:1287fccc11be 681
rahmadirizki18 16:89093194ccc2 682 pc.printf("analog X =%.2f Y =%.2f \n ",x,y);
rahmadirizki18 3:1287fccc11be 683
rahmadirizki18 16:89093194ccc2 684
rahmadirizki18 16:89093194ccc2 685 motor1.speed(s1);
rahmadirizki18 16:89093194ccc2 686 motor2.speed(s2);
rahmadirizki18 16:89093194ccc2 687 motor3.speed(s3);
rahmadirizki18 16:89093194ccc2 688 motor4.speed(s4);
rahmadirizki18 3:1287fccc11be 689 break;
rahmadirizki18 3:1287fccc11be 690 }
rahmadirizki18 3:1287fccc11be 691 default :
fanny868 0:9072e932503c 692 {
rahmadirizki18 16:89093194ccc2 693 motor1.brake(1);
rahmadirizki18 16:89093194ccc2 694 motor2.brake(1);
rahmadirizki18 16:89093194ccc2 695 motor3.brake(1);
rahmadirizki18 16:89093194ccc2 696 motor4.brake(1);
rahmadirizki18 16:89093194ccc2 697
rahmadirizki18 3:1287fccc11be 698 maju=mundur=kiri=kanan=saka=saki=sbka=sbki=analog=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 699 stop = true;
fanny868 0:9072e932503c 700
fanny868 0:9072e932503c 701
rahmadirizki18 16:89093194ccc2 702 pc.printf("Stop\n");
rahmadirizki18 16:89093194ccc2 703 }
rahmadirizki18 5:3aa203218306 704 }
rahmadirizki18 5:3aa203218306 705 }
rahmadirizki18 5:3aa203218306 706
rahmadirizki18 5:3aa203218306 707
rahmadirizki18 5:3aa203218306 708
rahmadirizki18 7:d138c56dab20 709 void speedLauncher()
rahmadirizki18 7:d138c56dab20 710 {
franshendri 12:e07c59c28c29 711 if (joystick.R3_click and speedL < 0.8){
franshendri 12:e07c59c28c29 712 speedL = speedL + 0.01;}
rahmadirizki18 7:d138c56dab20 713 if (joystick.L3_click and speedL > 0.1){
franshendri 12:e07c59c28c29 714 speedL = speedL - 0.01;}
rahmadirizki18 13:8ab42383a2ca 715 if (joystick.R2_click and speedB < 0.8 ){
rahmadirizki18 13:8ab42383a2ca 716 speedB = speedB + 0.01;}
rahmadirizki18 13:8ab42383a2ca 717 if (joystick.L2_click and speedB > 0.1 ){
rahmadirizki18 13:8ab42383a2ca 718 speedB = speedB - 0.01;}
rahmadirizki18 15:98f0d56b14f0 719 //pc.printf("Pwm depan = %.3f\t Pwm belakang = %.3f\n", speedL, speedB);
rahmadirizki18 7:d138c56dab20 720 }
franshendri 12:e07c59c28c29 721
franshendri 12:e07c59c28c29 722
franshendri 12:e07c59c28c29 723
fanny868 0:9072e932503c 724 int main (void)
fanny868 0:9072e932503c 725 {
fanny868 0:9072e932503c 726 // Set baud rate - 115200
fanny868 0:9072e932503c 727 joystick.setup();
rahmadirizki18 3:1287fccc11be 728 pc.baud(115200);
rahmadirizki18 5:3aa203218306 729 wait_ms(500);
rahmadirizki18 5:3aa203218306 730
rahmadirizki18 3:1287fccc11be 731 pc.printf("Ready...\n");
rahmadirizki18 4:483c07cc22e1 732 kalibrasi();
fanny868 0:9072e932503c 733 while(1)
fanny868 0:9072e932503c 734 {
fanny868 0:9072e932503c 735 // Interrupt Serial
fanny868 0:9072e932503c 736 joystick.idle();
fanny868 0:9072e932503c 737 if(joystick.readable() ) {
fanny868 0:9072e932503c 738 // Panggil fungsi pembacaan joystik
fanny868 0:9072e932503c 739 joystick.baca_data();
fanny868 0:9072e932503c 740 // Panggil fungsi pengolahan data joystik
fanny868 0:9072e932503c 741 joystick.olah_data();
franshendri 12:e07c59c28c29 742 //pc.printf("%3d %3d\n\r",joystick.R2, joystick.L2);
fanny868 0:9072e932503c 743 //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 744 case_ger = case_gerak();
rahmadirizki18 3:1287fccc11be 745 aktuator();
rahmadirizki18 6:68293bed71ea 746
franshendri 12:e07c59c28c29 747 //pc.printf("bacaS = %.2f\tbacaB = %.2f\n",servoS.read(), servoB.read());
rahmadirizki18 6:68293bed71ea 748
rahmadirizki18 3:1287fccc11be 749 //kalibrasi
rahmadirizki18 4:483c07cc22e1 750 if (joystick.START){
rahmadirizki18 4:483c07cc22e1 751 kalibrasi();}
rahmadirizki18 4:483c07cc22e1 752 // analog switch mode
rahmadirizki18 6:68293bed71ea 753 if (joystick.SELECT_click) {analog = !analog;}
rahmadirizki18 6:68293bed71ea 754 if (joystick.segitiga_click) {Launcher = !Launcher;}
rahmadirizki18 6:68293bed71ea 755 if (joystick.lingkaran_click){
rahmadirizki18 6:68293bed71ea 756 ServoGo = true;
rahmadirizki18 6:68293bed71ea 757 }
rahmadirizki18 5:3aa203218306 758 if (joystick.silang) {
rahmadirizki18 16:89093194ccc2 759
rahmadirizki18 15:98f0d56b14f0 760 //pc.printf("x..\n");
rahmadirizki18 5:3aa203218306 761 }
rahmadirizki18 7:d138c56dab20 762 speedLauncher();
franshendri 12:e07c59c28c29 763
fanny868 0:9072e932503c 764 } else {
fanny868 0:9072e932503c 765 joystick.idle();
fanny868 0:9072e932503c 766
fanny868 0:9072e932503c 767 }
rahmadirizki18 16:89093194ccc2 768 // gotoXYT(XT,YT,Tetha);
rahmadirizki18 5:3aa203218306 769
fanny868 0:9072e932503c 770 }
fanny868 0:9072e932503c 771 }