KRAI 2017 / Mbed 2 deprecated MainProgram_BaseBaru_fix_omni_20April

paling baru

Dependencies:   DigitDisplay Motor PID Ping mbed millis

Fork of MainProgram_BaseBaru_fix_omni_11April by KRAI 2017

main.cpp@4:483c07cc22e1, 2016-10-25 (annotated)

Committer:
rahmadirizki18
Date:
Tue Oct 25 13:21:57 2016 +0000
Revision:
4:483c07cc22e1
Parent:
3:1287fccc11be
Child:
5:3aa203218306
fixed error analog

Who changed what in which revision?

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