KRAI 2017 / Mbed 2 deprecated MainProgram_BaseBaru_17feb

Update 18 Februari 2017, PID laucnher dan Base sudah diperbarui

Dependencies:   Motor PID Joystick_OrdoV5 mbed millis

Fork of MainProgram_BaseBaru_otomatis-reloader by KRAI 2017

main.cpp@3:1287fccc11be, 2016-10-16 (annotated)

Committer:
rahmadirizki18
Date:
Sun Oct 16 06:27:39 2016 +0000
Revision:
3:1287fccc11be
Parent:
1:56bd3e8f38c5
Child:
4:483c07cc22e1
Program base bawah, dengan perbaikan untuk mode analog yang lebih leluasa

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