library baru
Dependencies: Motor PS_PAD mbed
main.cpp@1:0c9e4edadb8f, 2016-02-09 (annotated)
- Committer:
- rizqicahyo
- Date:
- Tue Feb 09 14:14:32 2016 +0000
- Revision:
- 1:0c9e4edadb8f
- Parent:
- 0:e58595ad773c
- Child:
- 2:35c396166e13
first commit
Who changed what in which revision?
User | Revision | Line number | New 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 | 1:0c9e4edadb8f | 272 | motor1.speed(0.01); |
rizqicahyo | 1:0c9e4edadb8f | 273 | motor2.brake(0.01); |
rizqicahyo | 1:0c9e4edadb8f | 274 | //com.putc('1'); |
rizqicahyo | 1:0c9e4edadb8f | 275 | //break; |
rizqicahyo | 0:e58595ad773c | 276 | } |
rizqicahyo | 0:e58595ad773c | 277 | case (8) : //turun |
rizqicahyo | 0:e58595ad773c | 278 | { |
rizqicahyo | 1:0c9e4edadb8f | 279 | motor1.speed(0.01); |
rizqicahyo | 1:0c9e4edadb8f | 280 | motor2.speed(0.01); |
rizqicahyo | 1:0c9e4edadb8f | 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 | } |