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@0:9072e932503c, 2015-05-28 (annotated)

Committer:
fanny868
Date:
Thu May 28 14:50:05 2015 +0000
Revision:
0:9072e932503c
Child:
1:56bd3e8f38c5
.

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
fanny868 0:9072e932503c 272 s1 = (float)(-1*koef*vcurr);
fanny868 0:9072e932503c 273 s2 = (float)(1.0*koef*vcurr);
fanny868 0:9072e932503c 274 s3 = (float)(1.0*koef*vcurr);
fanny868 0:9072e932503c 275 s4 = (float)(-1*koef*vcurr);
fanny868 0:9072e932503c 276
fanny868 0:9072e932503c 277 //s1 =-0.8*koef*vcurr;
fanny868 0:9072e932503c 278 //s2 =koef*vcurr;
fanny868 0:9072e932503c 279 //s3 =-koef*vcurr;
fanny868 0:9072e932503c 280 //s4 =koef*vcurr;
fanny868 0:9072e932503c 281
fanny868 0:9072e932503c 282 maju=true;
fanny868 0:9072e932503c 283 mundur=kiri=kanan=saka=saki=sbka=sbki=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 284
fanny868 0:9072e932503c 285 //pc.printf("maju\n");
fanny868 0:9072e932503c 286
fanny868 0:9072e932503c 287 motor1.speed(s1);
fanny868 0:9072e932503c 288 motor2.speed(s2);
fanny868 0:9072e932503c 289 motor3.speed(s3);
fanny868 0:9072e932503c 290 motor4.speed(s4);
fanny868 0:9072e932503c 291
fanny868 0:9072e932503c 292 break;
fanny868 0:9072e932503c 293 }
fanny868 0:9072e932503c 294 case (4):
fanny868 0:9072e932503c 295 {
fanny868 0:9072e932503c 296 if (mundur) {
fanny868 0:9072e932503c 297 if(vcurr<0.1) {
fanny868 0:9072e932503c 298 vcurr=0.1;
fanny868 0:9072e932503c 299 } else {
fanny868 0:9072e932503c 300 vcurr+=ax;
fanny868 0:9072e932503c 301 }
fanny868 0:9072e932503c 302 //perlambatan=0;
fanny868 0:9072e932503c 303 } else {
fanny868 0:9072e932503c 304 //perlambatan=1;
fanny868 0:9072e932503c 305 }
fanny868 0:9072e932503c 306
fanny868 0:9072e932503c 307 if (vcurr>=vmax) {
fanny868 0:9072e932503c 308 vcurr=vmax;
fanny868 0:9072e932503c 309 }
fanny868 0:9072e932503c 310
fanny868 0:9072e932503c 311 if(joystick.R2==255 && joystick.L2==0) {
fanny868 0:9072e932503c 312 koef=2;
fanny868 0:9072e932503c 313 } else if (joystick.L2==255 && joystick.R2==0) {
fanny868 0:9072e932503c 314 koef=0.5;
fanny868 0:9072e932503c 315 } else {
fanny868 0:9072e932503c 316 koef=1;
fanny868 0:9072e932503c 317 }
fanny868 0:9072e932503c 318
fanny868 0:9072e932503c 319 s1 = (float)(1*koef*vcurr);
fanny868 0:9072e932503c 320 s2 = (float)(-1*koef*vcurr);
fanny868 0:9072e932503c 321 s3 = (float)(-1*koef*vcurr);
fanny868 0:9072e932503c 322 s4 = (float)(1*koef*vcurr);
fanny868 0:9072e932503c 323
fanny868 0:9072e932503c 324 mundur=true;
fanny868 0:9072e932503c 325 maju=kiri=kanan=saka=saki=sbka=sbki=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 326
fanny868 0:9072e932503c 327 //pc.printf("mundur\n");
fanny868 0:9072e932503c 328
fanny868 0:9072e932503c 329 motor1.speed(s1);
fanny868 0:9072e932503c 330 motor2.speed(s2);
fanny868 0:9072e932503c 331 motor3.speed(s3);
fanny868 0:9072e932503c 332 motor4.speed(s4);
fanny868 0:9072e932503c 333
fanny868 0:9072e932503c 334 break;
fanny868 0:9072e932503c 335 }
fanny868 0:9072e932503c 336 case (5) :
fanny868 0:9072e932503c 337 {
fanny868 0:9072e932503c 338 if (saka) {
fanny868 0:9072e932503c 339 if(vcurr<0.1) {
fanny868 0:9072e932503c 340 vcurr=0.1;
fanny868 0:9072e932503c 341 } else {
fanny868 0:9072e932503c 342 vcurr+=ax;
fanny868 0:9072e932503c 343 }
fanny868 0:9072e932503c 344 //perlambatan=0;
fanny868 0:9072e932503c 345 } else {
fanny868 0:9072e932503c 346 //perlambatan=1;
fanny868 0:9072e932503c 347 }
fanny868 0:9072e932503c 348
fanny868 0:9072e932503c 349 if (vcurr>=vmax) {
fanny868 0:9072e932503c 350 vcurr=vmax;
fanny868 0:9072e932503c 351 } if(joystick.R2==255 && joystick.L2==0) {
fanny868 0:9072e932503c 352 koef=2;
fanny868 0:9072e932503c 353 } else if (joystick.L2==255 && joystick.R2==0) {
fanny868 0:9072e932503c 354 koef=0.5;
fanny868 0:9072e932503c 355 } else {
fanny868 0:9072e932503c 356 koef=1;
fanny868 0:9072e932503c 357 }
fanny868 0:9072e932503c 358
fanny868 0:9072e932503c 359 s1 = (float)(-koef*vcurr);
fanny868 0:9072e932503c 360 s2 = (float)(0); //koef*0.1*vcurr;
fanny868 0:9072e932503c 361 s3 = (float)(koef*vcurr);
fanny868 0:9072e932503c 362 s4 = (float)(0); //-koef*0.1*vcurr;
fanny868 0:9072e932503c 363
fanny868 0:9072e932503c 364 saka=true;
fanny868 0:9072e932503c 365 maju=mundur=kiri=kanan=sbka=saki=sbki=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 366
fanny868 0:9072e932503c 367 //pc.printf("saka\n");
fanny868 0:9072e932503c 368
fanny868 0:9072e932503c 369 motor1.speed(s1);
fanny868 0:9072e932503c 370 motor2.brake(1);
fanny868 0:9072e932503c 371 //motor2.speed(s2);
fanny868 0:9072e932503c 372 motor3.speed(s3);
fanny868 0:9072e932503c 373 motor4.brake(1);
fanny868 0:9072e932503c 374 //motor4.speed(s4);
fanny868 0:9072e932503c 375
fanny868 0:9072e932503c 376 break;
fanny868 0:9072e932503c 377 }
fanny868 0:9072e932503c 378 case (6) :
fanny868 0:9072e932503c 379 {
fanny868 0:9072e932503c 380 if (sbka){
fanny868 0:9072e932503c 381 if(vcurr<0.1) {
fanny868 0:9072e932503c 382 vcurr=0.1;
fanny868 0:9072e932503c 383 } else {
fanny868 0:9072e932503c 384 vcurr+=ax;
fanny868 0:9072e932503c 385 }
fanny868 0:9072e932503c 386 //perlambatan=0;
fanny868 0:9072e932503c 387 } else {
fanny868 0:9072e932503c 388 //perlambatan=1;
fanny868 0:9072e932503c 389 }
fanny868 0:9072e932503c 390
fanny868 0:9072e932503c 391 if (vcurr>=vmaxserong) {
fanny868 0:9072e932503c 392 vcurr=vmaxserong;
fanny868 0:9072e932503c 393 }
fanny868 0:9072e932503c 394
fanny868 0:9072e932503c 395 if(joystick.R2==255 && joystick.L2==0) {
fanny868 0:9072e932503c 396 koef=2;
fanny868 0:9072e932503c 397 } else if (joystick.L2==255 && joystick.R2==0) {
fanny868 0:9072e932503c 398 koef=0.5;
fanny868 0:9072e932503c 399 } else {
fanny868 0:9072e932503c 400 koef=1;
fanny868 0:9072e932503c 401 }
fanny868 0:9072e932503c 402
fanny868 0:9072e932503c 403 s1 = (float)(0); //koef*0.1*vcurr;
fanny868 0:9072e932503c 404 s2 = (float)(-koef*vcurr);
fanny868 0:9072e932503c 405 s3 = (float)(0); //-koef*0.1*vcurr;
fanny868 0:9072e932503c 406 s4 = (float)(koef*vcurr);
fanny868 0:9072e932503c 407
fanny868 0:9072e932503c 408 sbka=true;
fanny868 0:9072e932503c 409 maju=mundur=kiri=kanan=saka=saki=sbki=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 410
fanny868 0:9072e932503c 411 //pc.printf("sbka\n");
fanny868 0:9072e932503c 412
fanny868 0:9072e932503c 413 //motor1.speed(s1);
fanny868 0:9072e932503c 414 motor1.brake(1);
fanny868 0:9072e932503c 415 motor2.speed(s2);
fanny868 0:9072e932503c 416 //motor3.speed(s3);
fanny868 0:9072e932503c 417 motor3.brake(1);
fanny868 0:9072e932503c 418 motor4.speed(s4);
fanny868 0:9072e932503c 419
fanny868 0:9072e932503c 420 break;
fanny868 0:9072e932503c 421 }
fanny868 0:9072e932503c 422 case (7) :
fanny868 0:9072e932503c 423 {
fanny868 0:9072e932503c 424 if (saki) {
fanny868 0:9072e932503c 425 if(vcurr<0.1) {
fanny868 0:9072e932503c 426 vcurr=0.1;
fanny868 0:9072e932503c 427 } else {
fanny868 0:9072e932503c 428 vcurr+=ax;
fanny868 0:9072e932503c 429 }
fanny868 0:9072e932503c 430 //perlambatan=0;
fanny868 0:9072e932503c 431 } else {
fanny868 0:9072e932503c 432 //perlambatan=1;
fanny868 0:9072e932503c 433 }
fanny868 0:9072e932503c 434
fanny868 0:9072e932503c 435 if (vcurr>=vmaxserong) {
fanny868 0:9072e932503c 436 vcurr=vmaxserong;
fanny868 0:9072e932503c 437 }
fanny868 0:9072e932503c 438
fanny868 0:9072e932503c 439 if(joystick.R2==255 && joystick.L2==0) {
fanny868 0:9072e932503c 440 koef=2;
fanny868 0:9072e932503c 441 } else if (joystick.L2==255 && joystick.R2==0) {
fanny868 0:9072e932503c 442 koef=0.5;
fanny868 0:9072e932503c 443 } else {
fanny868 0:9072e932503c 444 koef=1;
fanny868 0:9072e932503c 445 }
fanny868 0:9072e932503c 446
fanny868 0:9072e932503c 447 s1 = (float)(0); //-koef*0.1*vcurr;
fanny868 0:9072e932503c 448 s2 = (float)(koef*vcurr);
fanny868 0:9072e932503c 449 s3 = (float)(0); //koef*0.1*vcurr;
fanny868 0:9072e932503c 450 s4 = (float)(-koef*vcurr);
fanny868 0:9072e932503c 451
fanny868 0:9072e932503c 452 saki=true;
fanny868 0:9072e932503c 453 maju=kiri=kanan=saka=mundur=sbka=sbki=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 454
fanny868 0:9072e932503c 455 //pc.printf("saki\n");
fanny868 0:9072e932503c 456
fanny868 0:9072e932503c 457 //motor1.speed(s1);
fanny868 0:9072e932503c 458 motor1.brake(1);
fanny868 0:9072e932503c 459 motor2.speed(s2);
fanny868 0:9072e932503c 460 //motor3.speed(s3);
fanny868 0:9072e932503c 461 motor3.brake(1);
fanny868 0:9072e932503c 462 motor4.speed(s4);
fanny868 0:9072e932503c 463
fanny868 0:9072e932503c 464 break;
fanny868 0:9072e932503c 465 }
fanny868 0:9072e932503c 466 case (8) :
fanny868 0:9072e932503c 467 {
fanny868 0:9072e932503c 468 if (sbki) {
fanny868 0:9072e932503c 469 if(vcurr<0.1) {
fanny868 0:9072e932503c 470 vcurr=0.1;
fanny868 0:9072e932503c 471 } else {
fanny868 0:9072e932503c 472 vcurr+=ax;
fanny868 0:9072e932503c 473 }
fanny868 0:9072e932503c 474 //perlambatan=0;
fanny868 0:9072e932503c 475 } else {
fanny868 0:9072e932503c 476 //perlambatan=1;
fanny868 0:9072e932503c 477 }
fanny868 0:9072e932503c 478
fanny868 0:9072e932503c 479 if (vcurr>=vmaxserong) {
fanny868 0:9072e932503c 480 vcurr=vmaxserong;
fanny868 0:9072e932503c 481 }
fanny868 0:9072e932503c 482
fanny868 0:9072e932503c 483 if(joystick.R2==255 && joystick.L2==0) {
fanny868 0:9072e932503c 484 koef=2;
fanny868 0:9072e932503c 485 } else if (joystick.L2==255 && joystick.R2==0) {
fanny868 0:9072e932503c 486 koef=0.5;
fanny868 0:9072e932503c 487 } else {
fanny868 0:9072e932503c 488 koef=1;
fanny868 0:9072e932503c 489 }
fanny868 0:9072e932503c 490
fanny868 0:9072e932503c 491 s1 = (float)(koef*vcurr);
fanny868 0:9072e932503c 492 s2 = (float)(0); //-koef*0.1*vcurr;
fanny868 0:9072e932503c 493 s3 = (float)(-koef*vcurr);
fanny868 0:9072e932503c 494 s4 = (float)(0); //koef*0.1*vcurr;
fanny868 0:9072e932503c 495
fanny868 0:9072e932503c 496 sbki=true;
fanny868 0:9072e932503c 497 maju=kiri=kanan=saka=saki=sbka=mundur=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 498
fanny868 0:9072e932503c 499 //pc.printf("sbki\n");
fanny868 0:9072e932503c 500
fanny868 0:9072e932503c 501 motor1.speed(s1);
fanny868 0:9072e932503c 502 //motor2.speed(s2);
fanny868 0:9072e932503c 503 motor2.brake(1);
fanny868 0:9072e932503c 504 motor3.speed(s3);
fanny868 0:9072e932503c 505 //motor4.speed(s4);
fanny868 0:9072e932503c 506 motor4.brake(1);
fanny868 0:9072e932503c 507
fanny868 0:9072e932503c 508 break;
fanny868 0:9072e932503c 509 }
fanny868 0:9072e932503c 510 case (9) :
fanny868 0:9072e932503c 511 {
fanny868 0:9072e932503c 512 if (kanan) {
fanny868 0:9072e932503c 513 if(vcurr<0.1) {
fanny868 0:9072e932503c 514 vcurr=0.1;
fanny868 0:9072e932503c 515 } else {
fanny868 0:9072e932503c 516 vcurr+=ax;
fanny868 0:9072e932503c 517 }
fanny868 0:9072e932503c 518 //perlambatan=0;
fanny868 0:9072e932503c 519 } else {
fanny868 0:9072e932503c 520 //perlambatan=1;
fanny868 0:9072e932503c 521 }
fanny868 0:9072e932503c 522
fanny868 0:9072e932503c 523 if (vcurr>=vmax) {
fanny868 0:9072e932503c 524 vcurr=vmax;
fanny868 0:9072e932503c 525 }
fanny868 0:9072e932503c 526
fanny868 0:9072e932503c 527 if(joystick.R2==255 && joystick.L2==0) {
fanny868 0:9072e932503c 528 koef=2;
fanny868 0:9072e932503c 529 } else if (joystick.L2==255 && joystick.R2==0) {
fanny868 0:9072e932503c 530 koef=0.5;
fanny868 0:9072e932503c 531 } else {
fanny868 0:9072e932503c 532 koef=1;
fanny868 0:9072e932503c 533 }
fanny868 0:9072e932503c 534
fanny868 0:9072e932503c 535 s1 =(float)(-1*koef*vcurr);
fanny868 0:9072e932503c 536 s2 =(float)(-1.0*koef*vcurr);
fanny868 0:9072e932503c 537 s3 =(float)(1*koef*vcurr);
fanny868 0:9072e932503c 538 s4 =(float)(1.0*koef*vcurr);
fanny868 0:9072e932503c 539
fanny868 0:9072e932503c 540 kanan=true;
fanny868 0:9072e932503c 541 maju=kiri=mundur=saka=saki=sbka=sbki=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 542
fanny868 0:9072e932503c 543 //pc.printf("Kanan\n");
fanny868 0:9072e932503c 544
fanny868 0:9072e932503c 545 motor1.speed(s1);
fanny868 0:9072e932503c 546 motor2.speed(s2);
fanny868 0:9072e932503c 547 motor3.speed(s3);
fanny868 0:9072e932503c 548 motor4.speed(s4);
fanny868 0:9072e932503c 549 break;
fanny868 0:9072e932503c 550 }
fanny868 0:9072e932503c 551 case (10) :
fanny868 0:9072e932503c 552 {
fanny868 0:9072e932503c 553 if (kiri) {
fanny868 0:9072e932503c 554 if(vcurr<0.1) {
fanny868 0:9072e932503c 555 vcurr=0.1;
fanny868 0:9072e932503c 556 } else {
fanny868 0:9072e932503c 557 vcurr+=ax;
fanny868 0:9072e932503c 558 }
fanny868 0:9072e932503c 559 //perlambatan=1;
fanny868 0:9072e932503c 560 } else {
fanny868 0:9072e932503c 561 //perlambatan=1;
fanny868 0:9072e932503c 562 }
fanny868 0:9072e932503c 563
fanny868 0:9072e932503c 564 if (vcurr>=vmax) {
fanny868 0:9072e932503c 565 vcurr=vmax;
fanny868 0:9072e932503c 566 }
fanny868 0:9072e932503c 567
fanny868 0:9072e932503c 568 if(joystick.R2==255 && joystick.L2==0) {
fanny868 0:9072e932503c 569 koef=2;
fanny868 0:9072e932503c 570 } else if (joystick.L2==255 && joystick.R2==0) {
fanny868 0:9072e932503c 571 koef=0.5;
fanny868 0:9072e932503c 572 } else {
fanny868 0:9072e932503c 573 koef=1;
fanny868 0:9072e932503c 574 }
fanny868 0:9072e932503c 575
fanny868 0:9072e932503c 576 s1 =(float)(1*koef*vcurr);
fanny868 0:9072e932503c 577 s2 =(float)(1*koef*vcurr);
fanny868 0:9072e932503c 578 s3 =(float)(-1*koef*vcurr);
fanny868 0:9072e932503c 579 s4 =(float)(-1.0*koef*vcurr);
fanny868 0:9072e932503c 580
fanny868 0:9072e932503c 581 kiri=true;
fanny868 0:9072e932503c 582 maju=kanan=mundur=saka=saki=sbka=sbki=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 583
fanny868 0:9072e932503c 584 //pc.printf("Kiri\n");
fanny868 0:9072e932503c 585
fanny868 0:9072e932503c 586 motor1.speed(s1);
fanny868 0:9072e932503c 587 motor2.speed(s2);
fanny868 0:9072e932503c 588 motor3.speed(s3);
fanny868 0:9072e932503c 589 motor4.speed(s4);
fanny868 0:9072e932503c 590
fanny868 0:9072e932503c 591 break;
fanny868 0:9072e932503c 592 }
fanny868 0:9072e932503c 593 default :
fanny868 0:9072e932503c 594 {
fanny868 0:9072e932503c 595 //if (mundur||kiri||kanan||saka||saki||sbka||sbki||pivki||pivka||cw1||ccw1||cw2||ccw2||cw3||ccw3) wait_ms(100);
fanny868 0:9072e932503c 596 //if (maju && (vcurr>=0.5)) wait_ms(100);
fanny868 0:9072e932503c 597 //else if (maju && (vcurr<0.5)) wait_ms(50);
fanny868 0:9072e932503c 598 /*
fanny868 0:9072e932503c 599 if(s1>0.2 || s1<-0.2 || s2>0.2 || s2<-0.2) {
fanny868 0:9072e932503c 600 s1 = koefperlambatan * s1;
fanny868 0:9072e932503c 601 s2 = koefperlambatan * s2;
fanny868 0:9072e932503c 602 s3 = koefperlambatan * s3;
fanny868 0:9072e932503c 603 s4 = koefperlambatan * s4;
fanny868 0:9072e932503c 604
fanny868 0:9072e932503c 605 motor1.speed(s1);
fanny868 0:9072e932503c 606 motor2.speed(s2);
fanny868 0:9072e932503c 607 motor3.speed(s3);
fanny868 0:9072e932503c 608 motor4.speed(s4);
fanny868 0:9072e932503c 609
fanny868 0:9072e932503c 610
fanny868 0:9072e932503c 611 } else {
fanny868 0:9072e932503c 612 */
fanny868 0:9072e932503c 613 motor1.brake(1);
fanny868 0:9072e932503c 614 motor2.brake(1);
fanny868 0:9072e932503c 615 motor3.brake(1);
fanny868 0:9072e932503c 616 motor4.brake(1);
fanny868 0:9072e932503c 617 //}
fanny868 0:9072e932503c 618
fanny868 0:9072e932503c 619 maju=mundur=kiri=kanan=saka=saki=sbka=sbki=pivki=pivka=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 620 stop = true;
fanny868 0:9072e932503c 621
fanny868 0:9072e932503c 622 //s1 = 0;s2 =0; s3 =0; s4 =0;
fanny868 0:9072e932503c 623
fanny868 0:9072e932503c 624 //pc.printf("Stop\n");
fanny868 0:9072e932503c 625 }
fanny868 0:9072e932503c 626 }
fanny868 0:9072e932503c 627 }
fanny868 0:9072e932503c 628
fanny868 0:9072e932503c 629
fanny868 0:9072e932503c 630 int main (void)
fanny868 0:9072e932503c 631 {
fanny868 0:9072e932503c 632 // Set baud rate - 115200
fanny868 0:9072e932503c 633 joystick.setup();
fanny868 0:9072e932503c 634 //pc.baud(115200);
fanny868 0:9072e932503c 635 //pc.printf("Ready...\n");
fanny868 0:9072e932503c 636 timer_pneu.start();
fanny868 0:9072e932503c 637 pneumatik1=1;
fanny868 0:9072e932503c 638 pneumatik2=1;
fanny868 0:9072e932503c 639 t1=0;
fanny868 0:9072e932503c 640 t2=0;
fanny868 0:9072e932503c 641 while(1)
fanny868 0:9072e932503c 642 {
fanny868 0:9072e932503c 643 // Interrupt Serial
fanny868 0:9072e932503c 644 joystick.idle();
fanny868 0:9072e932503c 645 if(joystick.readable() ) {
fanny868 0:9072e932503c 646 // Panggil fungsi pembacaan joystik
fanny868 0:9072e932503c 647 joystick.baca_data();
fanny868 0:9072e932503c 648 // Panggil fungsi pengolahan data joystik
fanny868 0:9072e932503c 649 joystick.olah_data();
fanny868 0:9072e932503c 650 //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 651 case_ger = case_gerak();
fanny868 0:9072e932503c 652 aktuator();
fanny868 0:9072e932503c 653
fanny868 0:9072e932503c 654 } else {
fanny868 0:9072e932503c 655 joystick.idle();
fanny868 0:9072e932503c 656
fanny868 0:9072e932503c 657 }
fanny868 0:9072e932503c 658 }
fanny868 0:9072e932503c 659 }