library baru

Dependencies:   Motor PS_PAD mbed

Committer:
rizqicahyo
Date:
Fri Feb 05 18:15:10 2016 +0000
Revision:
0:e58595ad773c
Child:
1:0c9e4edadb8f
first commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rizqicahyo 0:e58595ad773c 1
rizqicahyo 0:e58595ad773c 2 #include "mbed.h"
rizqicahyo 0:e58595ad773c 3 #include "PS_PAD.h"
rizqicahyo 0:e58595ad773c 4 #include "Motor.h"
rizqicahyo 0:e58595ad773c 5
rizqicahyo 0:e58595ad773c 6 PS_PAD ps2(PB_15,PB_14,PB_13, PB_12); //mosi,miso,clk,ss
rizqicahyo 0:e58595ad773c 7 Serial pc(USBTX,USBRX);
rizqicahyo 0:e58595ad773c 8 Serial com(A0,A1);
rizqicahyo 0:e58595ad773c 9
rizqicahyo 0:e58595ad773c 10 #define kec_max 0.9 // konstanta kecepatan maksimum
rizqicahyo 0:e58595ad773c 11 #define kec_pivot 0.8 // konstanta kecepatan pivot maksimum
rizqicahyo 0:e58595ad773c 12 #define ax 0.1 // konstanta perceptan
rizqicahyo 0:e58595ad773c 13 #define kec_gripper_naik 0.85
rizqicahyo 0:e58595ad773c 14 #define kec_gripper_turun 0.15
rizqicahyo 0:e58595ad773c 15 //Deklarasi pin motor
rizqicahyo 0:e58595ad773c 16 //Motor NAME (PinName pwm, PinName fwd, PinName rev):
rizqicahyo 0:e58595ad773c 17
rizqicahyo 0:e58595ad773c 18 Motor gripper(PB_6, PB_8, PB_9);
rizqicahyo 0:e58595ad773c 19 Motor motor3(PC_6, PC_8, PC_9);
rizqicahyo 0:e58595ad773c 20 Motor motor2(PB_3, PB_5, PB_4);
rizqicahyo 0:e58595ad773c 21 Motor motor1(PA_8, PA_9, PC_7);
rizqicahyo 0:e58595ad773c 22
rizqicahyo 0:e58595ad773c 23 bool maju=false;
rizqicahyo 0:e58595ad773c 24 bool mundur=false;
rizqicahyo 0:e58595ad773c 25 bool pivki=false;
rizqicahyo 0:e58595ad773c 26 bool pivka=false;
rizqicahyo 0:e58595ad773c 27 bool naik = false;
rizqicahyo 0:e58595ad773c 28 bool turun = false;
rizqicahyo 0:e58595ad773c 29 bool stop = true;
rizqicahyo 0:e58595ad773c 30 char case_jalan, prev_case_jalan;
rizqicahyo 0:e58595ad773c 31
rizqicahyo 0:e58595ad773c 32
rizqicahyo 0:e58595ad773c 33 // variabel kecepatan motor
rizqicahyo 0:e58595ad773c 34 double kec_motor;
rizqicahyo 0:e58595ad773c 35
rizqicahyo 0:e58595ad773c 36 void prosedurmotor()
rizqicahyo 0:e58595ad773c 37 {
rizqicahyo 0:e58595ad773c 38 double koef;
rizqicahyo 0:e58595ad773c 39 double s1=0,s2=0;
rizqicahyo 0:e58595ad773c 40 //Kondisi Motor
rizqicahyo 0:e58595ad773c 41 switch (case_jalan)
rizqicahyo 0:e58595ad773c 42 {
rizqicahyo 0:e58595ad773c 43 case (1): // pivot kanan
rizqicahyo 0:e58595ad773c 44 {
rizqicahyo 0:e58595ad773c 45 if (pivka)
rizqicahyo 0:e58595ad773c 46 {
rizqicahyo 0:e58595ad773c 47
rizqicahyo 0:e58595ad773c 48 if(kec_motor<0.1)
rizqicahyo 0:e58595ad773c 49 {
rizqicahyo 0:e58595ad773c 50 kec_motor=0.1;
rizqicahyo 0:e58595ad773c 51 }
rizqicahyo 0:e58595ad773c 52 else
rizqicahyo 0:e58595ad773c 53 {
rizqicahyo 0:e58595ad773c 54 kec_motor+=ax;
rizqicahyo 0:e58595ad773c 55 }
rizqicahyo 0:e58595ad773c 56
rizqicahyo 0:e58595ad773c 57 //perlambatan=0;
rizqicahyo 0:e58595ad773c 58 }
rizqicahyo 0:e58595ad773c 59 else
rizqicahyo 0:e58595ad773c 60 {
rizqicahyo 0:e58595ad773c 61 //perlambatan=1;
rizqicahyo 0:e58595ad773c 62 }
rizqicahyo 0:e58595ad773c 63 if (kec_motor>=kec_pivot)
rizqicahyo 0:e58595ad773c 64 {
rizqicahyo 0:e58595ad773c 65 kec_motor=kec_pivot;
rizqicahyo 0:e58595ad773c 66 }
rizqicahyo 0:e58595ad773c 67 //mode lambat dan cepat pada tombol R2 dan L2
rizqicahyo 0:e58595ad773c 68 if((ps2.read(PS_PAD::PAD_R2)==1) && (ps2.read(PS_PAD::PAD_L2)==0))
rizqicahyo 0:e58595ad773c 69 {
rizqicahyo 0:e58595ad773c 70 koef=2;
rizqicahyo 0:e58595ad773c 71 }
rizqicahyo 0:e58595ad773c 72 else if ((ps2.read(PS_PAD::PAD_R2)==0) && (ps2.read(PS_PAD::PAD_L2)==1))
rizqicahyo 0:e58595ad773c 73 {
rizqicahyo 0:e58595ad773c 74 koef=0.5;
rizqicahyo 0:e58595ad773c 75 }
rizqicahyo 0:e58595ad773c 76 else
rizqicahyo 0:e58595ad773c 77 {
rizqicahyo 0:e58595ad773c 78 koef=1;
rizqicahyo 0:e58595ad773c 79 }
rizqicahyo 0:e58595ad773c 80
rizqicahyo 0:e58595ad773c 81 s1 = (float)(-0.5*koef*kec_motor);
rizqicahyo 0:e58595ad773c 82 s2 = (float)(0.5*koef*kec_motor);
rizqicahyo 0:e58595ad773c 83
rizqicahyo 0:e58595ad773c 84 pivka=true;
rizqicahyo 0:e58595ad773c 85 maju=mundur=pivki=naik=turun=false;
rizqicahyo 0:e58595ad773c 86
rizqicahyo 0:e58595ad773c 87 pc.printf("\rPivot Kanan Cuy\n");
rizqicahyo 0:e58595ad773c 88
rizqicahyo 0:e58595ad773c 89 motor1.speed(s1);
rizqicahyo 0:e58595ad773c 90 motor2.speed(s2);
rizqicahyo 0:e58595ad773c 91 gripper.brake(1);
rizqicahyo 0:e58595ad773c 92
rizqicahyo 0:e58595ad773c 93 break;
rizqicahyo 0:e58595ad773c 94 }
rizqicahyo 0:e58595ad773c 95 case (2): // pivot kiri
rizqicahyo 0:e58595ad773c 96 {
rizqicahyo 0:e58595ad773c 97 if (pivki)
rizqicahyo 0:e58595ad773c 98 {
rizqicahyo 0:e58595ad773c 99 if(kec_motor<0.1)
rizqicahyo 0:e58595ad773c 100 {
rizqicahyo 0:e58595ad773c 101 kec_motor=0.1;
rizqicahyo 0:e58595ad773c 102 }
rizqicahyo 0:e58595ad773c 103 else
rizqicahyo 0:e58595ad773c 104 {
rizqicahyo 0:e58595ad773c 105 kec_motor+=ax;
rizqicahyo 0:e58595ad773c 106 }
rizqicahyo 0:e58595ad773c 107 //perlambatan=0;
rizqicahyo 0:e58595ad773c 108 }
rizqicahyo 0:e58595ad773c 109 else
rizqicahyo 0:e58595ad773c 110 {
rizqicahyo 0:e58595ad773c 111 //perlambatan=1;
rizqicahyo 0:e58595ad773c 112 }
rizqicahyo 0:e58595ad773c 113
rizqicahyo 0:e58595ad773c 114 if (kec_motor>=kec_pivot)
rizqicahyo 0:e58595ad773c 115 {
rizqicahyo 0:e58595ad773c 116 kec_motor=kec_pivot;
rizqicahyo 0:e58595ad773c 117 }
rizqicahyo 0:e58595ad773c 118 //mode lambat dan cepat pada tombol R2 dan L2
rizqicahyo 0:e58595ad773c 119 if((ps2.read(PS_PAD::PAD_R2)==1) && (ps2.read(PS_PAD::PAD_L2)==0))
rizqicahyo 0:e58595ad773c 120 {
rizqicahyo 0:e58595ad773c 121 koef=2;
rizqicahyo 0:e58595ad773c 122 } else if ((ps2.read(PS_PAD::PAD_R2)==0) && (ps2.read(PS_PAD::PAD_L2)==1))
rizqicahyo 0:e58595ad773c 123 {
rizqicahyo 0:e58595ad773c 124 koef=0.5;
rizqicahyo 0:e58595ad773c 125 }
rizqicahyo 0:e58595ad773c 126 else
rizqicahyo 0:e58595ad773c 127 {
rizqicahyo 0:e58595ad773c 128 koef=1;
rizqicahyo 0:e58595ad773c 129 }
rizqicahyo 0:e58595ad773c 130
rizqicahyo 0:e58595ad773c 131 s1 = (float)( 0.5*koef*kec_motor);
rizqicahyo 0:e58595ad773c 132 s2 = (float)(-0.5*koef*kec_motor);
rizqicahyo 0:e58595ad773c 133
rizqicahyo 0:e58595ad773c 134
rizqicahyo 0:e58595ad773c 135 pivki=true;
rizqicahyo 0:e58595ad773c 136 maju=mundur=pivka=naik=turun=false;
rizqicahyo 0:e58595ad773c 137
rizqicahyo 0:e58595ad773c 138 pc.printf("\rPivot Kiri Cuy\n");
rizqicahyo 0:e58595ad773c 139
rizqicahyo 0:e58595ad773c 140 motor1.speed(s1);
rizqicahyo 0:e58595ad773c 141 motor2.speed(s2);
rizqicahyo 0:e58595ad773c 142 gripper.brake(1);
rizqicahyo 0:e58595ad773c 143
rizqicahyo 0:e58595ad773c 144
rizqicahyo 0:e58595ad773c 145 break;
rizqicahyo 0:e58595ad773c 146 }
rizqicahyo 0:e58595ad773c 147 case (3): // maju
rizqicahyo 0:e58595ad773c 148 {
rizqicahyo 0:e58595ad773c 149 if (maju)
rizqicahyo 0:e58595ad773c 150 {
rizqicahyo 0:e58595ad773c 151 if(kec_motor<0.1)
rizqicahyo 0:e58595ad773c 152 {
rizqicahyo 0:e58595ad773c 153 kec_motor=0.1;
rizqicahyo 0:e58595ad773c 154 }
rizqicahyo 0:e58595ad773c 155 else
rizqicahyo 0:e58595ad773c 156 {
rizqicahyo 0:e58595ad773c 157 kec_motor+=ax;
rizqicahyo 0:e58595ad773c 158 }
rizqicahyo 0:e58595ad773c 159 //perlambatan=0;
rizqicahyo 0:e58595ad773c 160 }
rizqicahyo 0:e58595ad773c 161 else
rizqicahyo 0:e58595ad773c 162 {
rizqicahyo 0:e58595ad773c 163 //perlambatan=1;
rizqicahyo 0:e58595ad773c 164 }
rizqicahyo 0:e58595ad773c 165
rizqicahyo 0:e58595ad773c 166 if (kec_motor>=kec_max)
rizqicahyo 0:e58595ad773c 167 {
rizqicahyo 0:e58595ad773c 168 kec_motor=kec_max;
rizqicahyo 0:e58595ad773c 169 }
rizqicahyo 0:e58595ad773c 170 //mode lambat dan cepat pada tombol R2 dan L2
rizqicahyo 0:e58595ad773c 171 if((ps2.read(PS_PAD::PAD_R2)==1) && (ps2.read(PS_PAD::PAD_L2)==0))
rizqicahyo 0:e58595ad773c 172 {
rizqicahyo 0:e58595ad773c 173 koef=2;
rizqicahyo 0:e58595ad773c 174 }
rizqicahyo 0:e58595ad773c 175 else if ((ps2.read(PS_PAD::PAD_R2)==0) && (ps2.read(PS_PAD::PAD_L2)==1))
rizqicahyo 0:e58595ad773c 176 {
rizqicahyo 0:e58595ad773c 177 koef=0.5;
rizqicahyo 0:e58595ad773c 178 }
rizqicahyo 0:e58595ad773c 179 else
rizqicahyo 0:e58595ad773c 180 {
rizqicahyo 0:e58595ad773c 181 koef=1;
rizqicahyo 0:e58595ad773c 182 }
rizqicahyo 0:e58595ad773c 183
rizqicahyo 0:e58595ad773c 184 //Case s1 untuk mode L2 lebih lambat
rizqicahyo 0:e58595ad773c 185 s1 = (float)(1*koef*(kec_motor));
rizqicahyo 0:e58595ad773c 186 s2 = (float)(1*koef*(kec_motor));
rizqicahyo 0:e58595ad773c 187
rizqicahyo 0:e58595ad773c 188
rizqicahyo 0:e58595ad773c 189 maju=true;
rizqicahyo 0:e58595ad773c 190 mundur=pivka=pivki=naik=turun=false;
rizqicahyo 0:e58595ad773c 191
rizqicahyo 0:e58595ad773c 192 pc.printf("\rMaju Cuy\n");
rizqicahyo 0:e58595ad773c 193
rizqicahyo 0:e58595ad773c 194 motor1.speed(s1);
rizqicahyo 0:e58595ad773c 195 motor2.speed(s2);
rizqicahyo 0:e58595ad773c 196 gripper.brake(1);
rizqicahyo 0:e58595ad773c 197
rizqicahyo 0:e58595ad773c 198
rizqicahyo 0:e58595ad773c 199 break;
rizqicahyo 0:e58595ad773c 200 }
rizqicahyo 0:e58595ad773c 201 case (4): // mundur
rizqicahyo 0:e58595ad773c 202 {
rizqicahyo 0:e58595ad773c 203 if (mundur)
rizqicahyo 0:e58595ad773c 204 {
rizqicahyo 0:e58595ad773c 205 if(kec_motor<0.1)
rizqicahyo 0:e58595ad773c 206 {
rizqicahyo 0:e58595ad773c 207 kec_motor=0.1;
rizqicahyo 0:e58595ad773c 208 }
rizqicahyo 0:e58595ad773c 209 else
rizqicahyo 0:e58595ad773c 210 {
rizqicahyo 0:e58595ad773c 211 kec_motor+=ax;
rizqicahyo 0:e58595ad773c 212 }
rizqicahyo 0:e58595ad773c 213 //perlambatan=0;
rizqicahyo 0:e58595ad773c 214 }
rizqicahyo 0:e58595ad773c 215 else
rizqicahyo 0:e58595ad773c 216 {
rizqicahyo 0:e58595ad773c 217 //perlambatan=1;
rizqicahyo 0:e58595ad773c 218 }
rizqicahyo 0:e58595ad773c 219
rizqicahyo 0:e58595ad773c 220 if (kec_motor>=kec_max)
rizqicahyo 0:e58595ad773c 221 {
rizqicahyo 0:e58595ad773c 222 kec_motor=kec_max;
rizqicahyo 0:e58595ad773c 223 }
rizqicahyo 0:e58595ad773c 224 //mode lambat dan cepat pada tombol R2 dan L2
rizqicahyo 0:e58595ad773c 225 if((ps2.read(PS_PAD::PAD_R2)==1) && (ps2.read(PS_PAD::PAD_L2)==0))
rizqicahyo 0:e58595ad773c 226 {
rizqicahyo 0:e58595ad773c 227 koef=2;
rizqicahyo 0:e58595ad773c 228 }
rizqicahyo 0:e58595ad773c 229 else if ((ps2.read(PS_PAD::PAD_R2)==0) && (ps2.read(PS_PAD::PAD_L2)==1))
rizqicahyo 0:e58595ad773c 230 {
rizqicahyo 0:e58595ad773c 231 koef=0.5;
rizqicahyo 0:e58595ad773c 232 }
rizqicahyo 0:e58595ad773c 233 else
rizqicahyo 0:e58595ad773c 234 {
rizqicahyo 0:e58595ad773c 235 koef=1;
rizqicahyo 0:e58595ad773c 236 }
rizqicahyo 0:e58595ad773c 237 //Motor 4 telat mulai
rizqicahyo 0:e58595ad773c 238 s1 = (float)(-1*koef*(kec_motor));
rizqicahyo 0:e58595ad773c 239 s2 = (float)(-1*koef*(kec_motor));
rizqicahyo 0:e58595ad773c 240
rizqicahyo 0:e58595ad773c 241
rizqicahyo 0:e58595ad773c 242 mundur=true;
rizqicahyo 0:e58595ad773c 243 maju=pivka=pivki=naik=turun=false;
rizqicahyo 0:e58595ad773c 244
rizqicahyo 0:e58595ad773c 245 pc.printf("\rMundur Cuy\n");
rizqicahyo 0:e58595ad773c 246
rizqicahyo 0:e58595ad773c 247 motor1.speed(s1);
rizqicahyo 0:e58595ad773c 248 motor2.speed(s2);
rizqicahyo 0:e58595ad773c 249 gripper.brake(1);
rizqicahyo 0:e58595ad773c 250
rizqicahyo 0:e58595ad773c 251
rizqicahyo 0:e58595ad773c 252 break;
rizqicahyo 0:e58595ad773c 253 }
rizqicahyo 0:e58595ad773c 254 case (5) : //naik
rizqicahyo 0:e58595ad773c 255 {
rizqicahyo 0:e58595ad773c 256 //motor1.brake(1);
rizqicahyo 0:e58595ad773c 257 //motor2.brake(1);
rizqicahyo 0:e58595ad773c 258 gripper.speed(-kec_gripper_naik);
rizqicahyo 0:e58595ad773c 259 break;
rizqicahyo 0:e58595ad773c 260 }
rizqicahyo 0:e58595ad773c 261 case (6) : //turun
rizqicahyo 0:e58595ad773c 262 {
rizqicahyo 0:e58595ad773c 263 //motor1.brake(1);
rizqicahyo 0:e58595ad773c 264 //motor2.brake(1);
rizqicahyo 0:e58595ad773c 265 gripper.speed(kec_gripper_turun);
rizqicahyo 0:e58595ad773c 266 break;
rizqicahyo 0:e58595ad773c 267 }
rizqicahyo 0:e58595ad773c 268
rizqicahyo 0:e58595ad773c 269 //TAMBAHAN POWER
rizqicahyo 0:e58595ad773c 270 case (7) : //turun
rizqicahyo 0:e58595ad773c 271 {
rizqicahyo 0:e58595ad773c 272 //motor1.brake(1);
rizqicahyo 0:e58595ad773c 273 //motor2.brake(1);
rizqicahyo 0:e58595ad773c 274 com.putc('1');
rizqicahyo 0:e58595ad773c 275 break;
rizqicahyo 0:e58595ad773c 276 }
rizqicahyo 0:e58595ad773c 277 case (8) : //turun
rizqicahyo 0:e58595ad773c 278 {
rizqicahyo 0:e58595ad773c 279 //motor1.brake(1);
rizqicahyo 0:e58595ad773c 280 //motor2.brake(1);
rizqicahyo 0:e58595ad773c 281 com.putc('2');
rizqicahyo 0:e58595ad773c 282 break;
rizqicahyo 0:e58595ad773c 283 }
rizqicahyo 0:e58595ad773c 284
rizqicahyo 0:e58595ad773c 285
rizqicahyo 0:e58595ad773c 286 default :
rizqicahyo 0:e58595ad773c 287 {
rizqicahyo 0:e58595ad773c 288
rizqicahyo 0:e58595ad773c 289 motor1.brake(1);
rizqicahyo 0:e58595ad773c 290 motor2.brake(1);
rizqicahyo 0:e58595ad773c 291 gripper.brake(1);
rizqicahyo 0:e58595ad773c 292
rizqicahyo 0:e58595ad773c 293 //}
rizqicahyo 0:e58595ad773c 294
rizqicahyo 0:e58595ad773c 295 maju=mundur=pivki=pivka=false;
rizqicahyo 0:e58595ad773c 296 stop = true;
rizqicahyo 0:e58595ad773c 297
rizqicahyo 0:e58595ad773c 298 //s1 = 0;s2 =0;
rizqicahyo 0:e58595ad773c 299
rizqicahyo 0:e58595ad773c 300 pc.printf("\rStop\n");
rizqicahyo 0:e58595ad773c 301 }
rizqicahyo 0:e58595ad773c 302 }
rizqicahyo 0:e58595ad773c 303 }
rizqicahyo 0:e58595ad773c 304
rizqicahyo 0:e58595ad773c 305 int mode_jalan ()
rizqicahyo 0:e58595ad773c 306 {
rizqicahyo 0:e58595ad773c 307 int jalan_ke;
rizqicahyo 0:e58595ad773c 308 if ((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0))
rizqicahyo 0:e58595ad773c 309 {
rizqicahyo 0:e58595ad773c 310 // Pivot Kanan
rizqicahyo 0:e58595ad773c 311 jalan_ke = 1;
rizqicahyo 0:e58595ad773c 312 }
rizqicahyo 0:e58595ad773c 313 else if ((ps2.read(PS_PAD::PAD_R1)==0) && (ps2.read(PS_PAD::PAD_L1)==1))
rizqicahyo 0:e58595ad773c 314 {
rizqicahyo 0:e58595ad773c 315 // Pivot Kiri
rizqicahyo 0:e58595ad773c 316 jalan_ke = 2;
rizqicahyo 0:e58595ad773c 317 }
rizqicahyo 0:e58595ad773c 318 else if ((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0))
rizqicahyo 0:e58595ad773c 319 {
rizqicahyo 0:e58595ad773c 320 // Maju
rizqicahyo 0:e58595ad773c 321 jalan_ke = 3;
rizqicahyo 0:e58595ad773c 322 }
rizqicahyo 0:e58595ad773c 323 else if ((ps2.read(PS_PAD::PAD_TOP)==0) && (ps2.read(PS_PAD::PAD_BOTTOM)==1))
rizqicahyo 0:e58595ad773c 324 {
rizqicahyo 0:e58595ad773c 325 // Mundur
rizqicahyo 0:e58595ad773c 326 jalan_ke = 4;
rizqicahyo 0:e58595ad773c 327 }
rizqicahyo 0:e58595ad773c 328 else if((ps2.read(PS_PAD::PAD_X)==1) && (ps2.read(PS_PAD::PAD_TRIANGLE)==0))
rizqicahyo 0:e58595ad773c 329 {
rizqicahyo 0:e58595ad773c 330 jalan_ke = 5;
rizqicahyo 0:e58595ad773c 331 }
rizqicahyo 0:e58595ad773c 332 else if((ps2.read(PS_PAD::PAD_X)==0) && (ps2.read(PS_PAD::PAD_TRIANGLE)==1))
rizqicahyo 0:e58595ad773c 333 {
rizqicahyo 0:e58595ad773c 334 jalan_ke = 6;
rizqicahyo 0:e58595ad773c 335 }
rizqicahyo 0:e58595ad773c 336
rizqicahyo 0:e58595ad773c 337 //tambahan power
rizqicahyo 0:e58595ad773c 338 else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_LEFT)==0))
rizqicahyo 0:e58595ad773c 339 {
rizqicahyo 0:e58595ad773c 340 jalan_ke = 7;
rizqicahyo 0:e58595ad773c 341 }
rizqicahyo 0:e58595ad773c 342 else if((ps2.read(PS_PAD::PAD_LEFT)==1) && (ps2.read(PS_PAD::PAD_RIGHT)==0))
rizqicahyo 0:e58595ad773c 343 {
rizqicahyo 0:e58595ad773c 344 jalan_ke = 8;
rizqicahyo 0:e58595ad773c 345 }
rizqicahyo 0:e58595ad773c 346 else
rizqicahyo 0:e58595ad773c 347 {
rizqicahyo 0:e58595ad773c 348 jalan_ke = 0;
rizqicahyo 0:e58595ad773c 349 }
rizqicahyo 0:e58595ad773c 350 return(jalan_ke);
rizqicahyo 0:e58595ad773c 351 }
rizqicahyo 0:e58595ad773c 352
rizqicahyo 0:e58595ad773c 353 int main ()
rizqicahyo 0:e58595ad773c 354 {
rizqicahyo 0:e58595ad773c 355 // Set baud rate - 115200
rizqicahyo 0:e58595ad773c 356 pc.baud(115200);
rizqicahyo 0:e58595ad773c 357 com.baud(9600);
rizqicahyo 0:e58595ad773c 358 pc.printf("Ready...\n");
rizqicahyo 0:e58595ad773c 359 ps2.init();
rizqicahyo 0:e58595ad773c 360
rizqicahyo 0:e58595ad773c 361 while (1)
rizqicahyo 0:e58595ad773c 362 {
rizqicahyo 0:e58595ad773c 363 ps2.poll();
rizqicahyo 0:e58595ad773c 364 case_jalan= mode_jalan();
rizqicahyo 0:e58595ad773c 365 if (case_jalan != prev_case_jalan)
rizqicahyo 0:e58595ad773c 366 {
rizqicahyo 0:e58595ad773c 367 kec_motor=0;
rizqicahyo 0:e58595ad773c 368 }
rizqicahyo 0:e58595ad773c 369
rizqicahyo 0:e58595ad773c 370 prosedurmotor();
rizqicahyo 0:e58595ad773c 371 prev_case_jalan = mode_jalan();
rizqicahyo 0:e58595ad773c 372 }
rizqicahyo 0:e58595ad773c 373 }