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@1:56bd3e8f38c5, 2015-06-06 (annotated)

Committer:
fadhilginting
Date:
Sat Jun 06 06:19:06 2015 +0000
Revision:
1:56bd3e8f38c5
Parent:
0:9072e932503c
Child:
3:1287fccc11be
RobotAttack_Nasional_OpenLoop_Tuning1

Who changed what in which revision?

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