KRAI 2016 / Mbed 2 deprecated Ultimate_Hybrid_LF

base ultimate + line following HYBRID

Dependencies:   ESC Motor PS_PAD hadah mbed Ping

Fork of Ultimate_Hybrid by KRAI 2016

main.cpp@6:7fce519e4976, 2016-04-28 (annotated)

Committer:
rizqicahyo
Date:
Thu Apr 28 14:59:21 2016 +0000
Revision:
6:7fce519e4976
Parent:
5:256f6eac0358 
tambah kodingan sensor arduino

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rizqicahyo 0:edddd373a163 1 /*********************************************************************************************/
rizqicahyo 0:edddd373a163 2 /** FILE HEADER **/
rizqicahyo 0:edddd373a163 3 /*********************************************************************************************/
rizqicahyo 0:edddd373a163 4 #include "mbed.h"
rizqicahyo 0:edddd373a163 5 #include "Motor.h"
rizqicahyo 0:edddd373a163 6 #include "PS_PAD.h"
rizqicahyo 0:edddd373a163 7 #include "esc.h"
rizqicahyo 0:edddd373a163 8 #include "Servo.h"
rizqicahyo 4:7a7a8aa33fd5 9 #include "Ping.h"
rizqicahyo 0:edddd373a163 10
rizqicahyo 0:edddd373a163 11 /*********************************************************************************************/
rizqicahyo 0:edddd373a163 12 /** DEKLARASI INPUT OUTPUT **/
rizqicahyo 0:edddd373a163 13 /*********************************************************************************************/
rizqicahyo 0:edddd373a163 14 //serial
rizqicahyo 0:edddd373a163 15 Serial pc(USBTX,USBRX); //debug
rizqicahyo 0:edddd373a163 16 Serial com(PA_0,PA_1); //sensor
rizqicahyo 0:edddd373a163 17
rizqicahyo 0:edddd373a163 18 //joystick PS2
rizqicahyo 0:edddd373a163 19 PS_PAD ps2(PB_15,PB_14,PB_13, PC_4); //(mosi, miso, sck, ss)
rizqicahyo 0:edddd373a163 20
rizqicahyo 0:edddd373a163 21 //motor (PWM, forward, reverse)
rizqicahyo 0:edddd373a163 22 Motor motor1(PA_8, PB_0, PC_15);
rizqicahyo 0:edddd373a163 23 Motor motor2(PA_11, PA_6, PA_5);
rizqicahyo 0:edddd373a163 24 Motor motor3(PA_9, PC_2, PC_3);
rizqicahyo 3:43d4cb3ece1b 25 Motor motor4(PA_10, PB_5, PB_4);
rizqicahyo 4:7a7a8aa33fd5 26 Motor motorC2(PB_7, PA_14, PA_15);
rizqicahyo 0:edddd373a163 27 Motor motorC1(PB_9, PA_12, PC_5);
rizqicahyo 4:7a7a8aa33fd5 28 Motor motorS(PB_8, PB_1, PA_13);
rizqicahyo 3:43d4cb3ece1b 29 //Motor motor4(PB_6, PA_7, PB_12);
Najib_irvani 1:fc1535231c0d 30
rizqicahyo 0:edddd373a163 31 //pnuematik
rizqicahyo 0:edddd373a163 32 DigitalOut pnuematik1(PC_11);
rizqicahyo 0:edddd373a163 33 DigitalOut pnuematik2(PC_10);
rizqicahyo 0:edddd373a163 34 DigitalOut pnuematik3(PD_2);
rizqicahyo 0:edddd373a163 35 DigitalOut pnuematik4(PC_12);
rizqicahyo 0:edddd373a163 36
rizqicahyo 0:edddd373a163 37 //Limit Switch
rizqicahyo 0:edddd373a163 38 DigitalIn limit1(PC_13 ,PullUp);
rizqicahyo 0:edddd373a163 39 DigitalIn limit2(PC_14 ,PullUp);
rizqicahyo 0:edddd373a163 40 DigitalIn limit3(PC_1 ,PullUp);
rizqicahyo 0:edddd373a163 41 DigitalIn limit4(PC_0 ,PullUp);
rizqicahyo 0:edddd373a163 42
rizqicahyo 3:43d4cb3ece1b 43 DigitalIn field(PB_10 ,PullUp);
rizqicahyo 3:43d4cb3ece1b 44
rizqicahyo 3:43d4cb3ece1b 45 //laser pointer
rizqicahyo 3:43d4cb3ece1b 46 DigitalOut laser(PB_3);
rizqicahyo 0:edddd373a163 47
rizqicahyo 0:edddd373a163 48 //servo(PWM)
rizqicahyo 0:edddd373a163 49 Servo servoEDF(PC_8);
rizqicahyo 0:edddd373a163 50
rizqicahyo 0:edddd373a163 51 //EDF(PWM, timer)
rizqicahyo 0:edddd373a163 52 ESC edf(PC_6,20);
rizqicahyo 0:edddd373a163 53
rizqicahyo 3:43d4cb3ece1b 54 //Timer
rizqicahyo 3:43d4cb3ece1b 55 Ticker timer;
rizqicahyo 2:df6c49846367 56
rizqicahyo 4:7a7a8aa33fd5 57 //Sensor Ping
rizqicahyo 4:7a7a8aa33fd5 58 Ping ping(PB_2);
rizqicahyo 0:edddd373a163 59
rizqicahyo 0:edddd373a163 60 /*********************************************************************************************/
rizqicahyo 0:edddd373a163 61 /** DEKLARASI VARIABEL, PROSEDUR, DAN FUNGSI **/
rizqicahyo 0:edddd373a163 62 /*********************************************************************************************/
rizqicahyo 3:43d4cb3ece1b 63 const float gMax_speed = 0.85;
rizqicahyo 3:43d4cb3ece1b 64 //const float gMin_speed = 0.1;
rizqicahyo 3:43d4cb3ece1b 65 const float v0 = 0.4;
rizqicahyo 3:43d4cb3ece1b 66 const float ax = 0.0007;
rizqicahyo 3:43d4cb3ece1b 67 float vcurr = v0;
rizqicahyo 2:df6c49846367 68
rizqicahyo 2:df6c49846367 69 const float tuning1 = 0.0;
rizqicahyo 3:43d4cb3ece1b 70 const float tuning2 = 0.04;
rizqicahyo 3:43d4cb3ece1b 71 const float tuning3 = 0.0;
rizqicahyo 3:43d4cb3ece1b 72 const float tuning4 = 0.04;
rizqicahyo 2:df6c49846367 73
rizqicahyo 3:43d4cb3ece1b 74 const float driver0 = 1;
rizqicahyo 4:7a7a8aa33fd5 75 const float driver1 = 0.82;
rizqicahyo 4:7a7a8aa33fd5 76 const float driver2 = 0.6;
rizqicahyo 4:7a7a8aa33fd5 77 const float driver3 = 0.15;
rizqicahyo 2:df6c49846367 78
rizqicahyo 0:edddd373a163 79
rizqicahyo 0:edddd373a163 80 // inisialisasi pwm awal servo
rizqicahyo 0:edddd373a163 81 double pwm = 0.00;
rizqicahyo 0:edddd373a163 82
rizqicahyo 0:edddd373a163 83 // inisialisasi sudut awal
rizqicahyo 0:edddd373a163 84 double sudut = -85;
rizqicahyo 0:edddd373a163 85
rizqicahyo 0:edddd373a163 86 // variabel kondisi pnuematik
rizqicahyo 0:edddd373a163 87 int g = 1;
rizqicahyo 0:edddd373a163 88
rizqicahyo 2:df6c49846367 89 //slider auto
rizqicahyo 2:df6c49846367 90 int c =0;
rizqicahyo 2:df6c49846367 91 int batas_delay = 270;
rizqicahyo 2:df6c49846367 92
rizqicahyo 2:df6c49846367 93 //data sensor garis dan line following
rizqicahyo 2:df6c49846367 94 int datasensor[6];
rizqicahyo 2:df6c49846367 95 int over;
rizqicahyo 3:43d4cb3ece1b 96 int g_flag;
rizqicahyo 2:df6c49846367 97
rizqicahyo 4:7a7a8aa33fd5 98
rizqicahyo 3:43d4cb3ece1b 99 void aktuator(int f);
rizqicahyo 0:edddd373a163 100 void edf_servo();
rizqicahyo 4:7a7a8aa33fd5 101
rizqicahyo 2:df6c49846367 102 void init_sensor();
rizqicahyo 3:43d4cb3ece1b 103 void linetracer(float speed);
rizqicahyo 3:43d4cb3ece1b 104 void flag_sensor();
rizqicahyo 2:df6c49846367 105
rizqicahyo 4:7a7a8aa33fd5 106 float read_jarak();
rizqicahyo 4:7a7a8aa33fd5 107
rizqicahyo 0:edddd373a163 108 /*********************************************************************************************/
rizqicahyo 0:edddd373a163 109 /*********************************************************************************************/
rizqicahyo 0:edddd373a163 110 /** PROGRAM UTAMA **/
rizqicahyo 0:edddd373a163 111 /*********************************************************************************************/
rizqicahyo 0:edddd373a163 112 /*********************************************************************************************/
rizqicahyo 0:edddd373a163 113 int main() {
rizqicahyo 0:edddd373a163 114 //inisiasi serial
rizqicahyo 0:edddd373a163 115 pc.baud(115200);
rizqicahyo 0:edddd373a163 116 com.baud(115200);
rizqicahyo 0:edddd373a163 117
rizqicahyo 3:43d4cb3ece1b 118 if(field == 1) sudut = -85;
rizqicahyo 3:43d4cb3ece1b 119 else sudut = 85;
rizqicahyo 3:43d4cb3ece1b 120
rizqicahyo 0:edddd373a163 121 //inisiasi joystick
rizqicahyo 0:edddd373a163 122 ps2.init();
rizqicahyo 0:edddd373a163 123
rizqicahyo 0:edddd373a163 124 //set inisiasi servo pada posisi 0
rizqicahyo 0:edddd373a163 125 servoEDF.position(sudut);
rizqicahyo 0:edddd373a163 126
rizqicahyo 0:edddd373a163 127 //set edf pada posisi bukan kalibrasi, yaitu set edf 0
rizqicahyo 0:edddd373a163 128 edf.setThrottle(0);
rizqicahyo 0:edddd373a163 129 edf.pulse();
rizqicahyo 0:edddd373a163 130
rizqicahyo 3:43d4cb3ece1b 131 //inisisasi laser
rizqicahyo 3:43d4cb3ece1b 132 laser = 1;
rizqicahyo 3:43d4cb3ece1b 133
rizqicahyo 0:edddd373a163 134 //inisiasi pnuematik
rizqicahyo 0:edddd373a163 135 pnuematik1 = 1;
rizqicahyo 0:edddd373a163 136 pnuematik2 = 1;
rizqicahyo 4:7a7a8aa33fd5 137 pnuematik3 = 1;
rizqicahyo 4:7a7a8aa33fd5 138 pnuematik4 = 1;
rizqicahyo 0:edddd373a163 139
rizqicahyo 0:edddd373a163 140 //inisisasi TIMER
rizqicahyo 3:43d4cb3ece1b 141 timer.attach(&flag_sensor,0.0005);
rizqicahyo 3:43d4cb3ece1b 142 timer.detach();
rizqicahyo 0:edddd373a163 143
rizqicahyo 0:edddd373a163 144 //kondisi robot
rizqicahyo 2:df6c49846367 145 bool manual=true;
rizqicahyo 3:43d4cb3ece1b 146 bool pool= false;
rizqicahyo 4:7a7a8aa33fd5 147
rizqicahyo 4:7a7a8aa33fd5 148 //running manual
rizqicahyo 0:edddd373a163 149 while(manual){
rizqicahyo 0:edddd373a163 150
rizqicahyo 0:edddd373a163 151 ps2.poll();
rizqicahyo 0:edddd373a163 152
rizqicahyo 3:43d4cb3ece1b 153 aktuator(field);
rizqicahyo 0:edddd373a163 154 edf_servo();
rizqicahyo 0:edddd373a163 155
rizqicahyo 4:7a7a8aa33fd5 156 if(limit4==0) manual = false;
rizqicahyo 0:edddd373a163 157
rizqicahyo 0:edddd373a163 158 }
rizqicahyo 0:edddd373a163 159
rizqicahyo 4:7a7a8aa33fd5 160 //running otomatis
rizqicahyo 3:43d4cb3ece1b 161 timer.attach(&flag_sensor,0.0005);
rizqicahyo 3:43d4cb3ece1b 162
rizqicahyo 3:43d4cb3ece1b 163 while(!pool){
rizqicahyo 3:43d4cb3ece1b 164 init_sensor();
rizqicahyo 3:43d4cb3ece1b 165
rizqicahyo 4:7a7a8aa33fd5 166 if(vcurr > 0.4) vcurr = (float) 0.4;
rizqicahyo 4:7a7a8aa33fd5 167 //else if (vcurr < 0.2) vcurr = (float) 0.2;
rizqicahyo 3:43d4cb3ece1b 168
rizqicahyo 3:43d4cb3ece1b 169 linetracer(vcurr);
rizqicahyo 3:43d4cb3ece1b 170 //laser = 1;
rizqicahyo 3:43d4cb3ece1b 171
rizqicahyo 4:7a7a8aa33fd5 172 if((limit3==0) || (read_jarak() <= 9.0)) pool=true;
rizqicahyo 3:43d4cb3ece1b 173
rizqicahyo 4:7a7a8aa33fd5 174 //if(read_jarak() <= 45.0) vcurr -= ax;
rizqicahyo 4:7a7a8aa33fd5 175 //else vcurr += ax;
rizqicahyo 4:7a7a8aa33fd5 176 vcurr += ax;
rizqicahyo 5:256f6eac0358 177
rizqicahyo 5:256f6eac0358 178 for(int i=0; i<6; i++){
rizqicahyo 5:256f6eac0358 179 printf("%d ",datasensor[i]);
rizqicahyo 5:256f6eac0358 180 }
rizqicahyo 5:256f6eac0358 181 printf("%d \n",g_flag);
rizqicahyo 5:256f6eac0358 182
rizqicahyo 3:43d4cb3ece1b 183 }
rizqicahyo 5:256f6eac0358 184 motor1.brake(0);
rizqicahyo 5:256f6eac0358 185 motor2.brake(0);
rizqicahyo 5:256f6eac0358 186 motor3.brake(0);
rizqicahyo 5:256f6eac0358 187 motor4.brake(0);
rizqicahyo 0:edddd373a163 188
rizqicahyo 3:43d4cb3ece1b 189 timer.detach();
rizqicahyo 3:43d4cb3ece1b 190
rizqicahyo 0:edddd373a163 191 pnuematik1=0;
rizqicahyo 0:edddd373a163 192 wait_ms(1500);
rizqicahyo 4:7a7a8aa33fd5 193
rizqicahyo 0:edddd373a163 194 while(limit4!=0){
rizqicahyo 4:7a7a8aa33fd5 195 motorC1.speed(0.95);
rizqicahyo 0:edddd373a163 196 motorC2.speed(-1);
rizqicahyo 0:edddd373a163 197 }
rizqicahyo 0:edddd373a163 198
rizqicahyo 0:edddd373a163 199 motorC1.brake(1);
rizqicahyo 0:edddd373a163 200 motorC2.brake(1);
rizqicahyo 0:edddd373a163 201
rizqicahyo 3:43d4cb3ece1b 202 if(field==1){
rizqicahyo 3:43d4cb3ece1b 203 pnuematik3 = 0;
rizqicahyo 3:43d4cb3ece1b 204 wait_ms(1500);
rizqicahyo 3:43d4cb3ece1b 205 pnuematik2 = 1;
rizqicahyo 3:43d4cb3ece1b 206 wait_ms(500);
rizqicahyo 3:43d4cb3ece1b 207 pnuematik3 = 1;
rizqicahyo 3:43d4cb3ece1b 208 }
rizqicahyo 3:43d4cb3ece1b 209 else{
rizqicahyo 3:43d4cb3ece1b 210 pnuematik4 = 0;
rizqicahyo 3:43d4cb3ece1b 211 wait_ms(1500);
rizqicahyo 3:43d4cb3ece1b 212 pnuematik2 = 1;
rizqicahyo 3:43d4cb3ece1b 213 wait_ms(500);
rizqicahyo 3:43d4cb3ece1b 214 pnuematik4 = 1;
rizqicahyo 3:43d4cb3ece1b 215 }
rizqicahyo 0:edddd373a163 216
rizqicahyo 0:edddd373a163 217 return 0;
rizqicahyo 0:edddd373a163 218 }
rizqicahyo 0:edddd373a163 219
rizqicahyo 0:edddd373a163 220 /*********************************************************************************************/
rizqicahyo 0:edddd373a163 221 /** ALGORITMA PROSEDUR DAN FUNGSI **/
rizqicahyo 0:edddd373a163 222 /*********************************************************************************************/
rizqicahyo 3:43d4cb3ece1b 223 void aktuator(int f){
rizqicahyo 2:df6c49846367 224 float speed = vcurr;
rizqicahyo 2:df6c49846367 225
rizqicahyo 2:df6c49846367 226 if(vcurr >= gMax_speed) vcurr = gMax_speed;
Najib_irvani 1:fc1535231c0d 227
rizqicahyo 3:43d4cb3ece1b 228 if(f == 1){
rizqicahyo 0:edddd373a163 229 if((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){
rizqicahyo 0:edddd373a163 230 //pivot kiri
rizqicahyo 3:43d4cb3ece1b 231 motor2.speed((float)0.5*(speed-tuning2));
rizqicahyo 3:43d4cb3ece1b 232 motor4.speed((float)-0.5*(speed-tuning4));
rizqicahyo 3:43d4cb3ece1b 233 motor1.speed((float)0.5*(speed-tuning1));
rizqicahyo 3:43d4cb3ece1b 234 motor3.speed((float)-0.5*(speed-tuning3));
rizqicahyo 0:edddd373a163 235 pc.printf("pivot kiri \n");
rizqicahyo 0:edddd373a163 236
rizqicahyo 2:df6c49846367 237 vcurr += ax;
rizqicahyo 0:edddd373a163 238 }
rizqicahyo 0:edddd373a163 239 else if((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){
rizqicahyo 0:edddd373a163 240 //pivot kanan
rizqicahyo 3:43d4cb3ece1b 241 motor2.speed((float)-0.5*(speed-tuning2));
rizqicahyo 3:43d4cb3ece1b 242 motor4.speed((float)0.5*(speed-tuning4));
rizqicahyo 3:43d4cb3ece1b 243 motor1.speed((float)-0.5*(speed-tuning1));
rizqicahyo 3:43d4cb3ece1b 244 motor3.speed((float)0.5*(speed-tuning3));
rizqicahyo 0:edddd373a163 245 pc.printf("pivot kanan \n");
rizqicahyo 0:edddd373a163 246
rizqicahyo 2:df6c49846367 247 vcurr += ax;
rizqicahyo 0:edddd373a163 248 }
rizqicahyo 0:edddd373a163 249 else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_TOP)==1)){
rizqicahyo 0:edddd373a163 250 //serong atas kanan
Najib_irvani 1:fc1535231c0d 251 motor2.speed(speed-tuning2);
rizqicahyo 0:edddd373a163 252 motor4.brake(1);
rizqicahyo 0:edddd373a163 253 motor1.brake(1);
Najib_irvani 1:fc1535231c0d 254 motor3.speed(speed-tuning3);
rizqicahyo 0:edddd373a163 255 pc.printf("serong atas kanan \n");
rizqicahyo 0:edddd373a163 256
rizqicahyo 2:df6c49846367 257 vcurr += ax;
rizqicahyo 0:edddd373a163 258 }
rizqicahyo 0:edddd373a163 259 else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
rizqicahyo 0:edddd373a163 260 //serong atas kiri
rizqicahyo 0:edddd373a163 261 motor2.brake(1);
Najib_irvani 1:fc1535231c0d 262 motor4.speed(-(speed-tuning4));
Najib_irvani 1:fc1535231c0d 263 motor1.speed(-(speed-tuning1));
rizqicahyo 0:edddd373a163 264 motor3.brake(1);
rizqicahyo 0:edddd373a163 265 pc.printf("serong atas kiri \n");
rizqicahyo 0:edddd373a163 266
rizqicahyo 2:df6c49846367 267 vcurr += ax;
rizqicahyo 0:edddd373a163 268 }
rizqicahyo 0:edddd373a163 269 else if((ps2.read(PS_PAD::PAD_LEFT)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==1)){
rizqicahyo 0:edddd373a163 270 //serong bawah kiri
Najib_irvani 1:fc1535231c0d 271 motor2.speed(-(speed-tuning2));
rizqicahyo 0:edddd373a163 272 motor4.brake(1);
rizqicahyo 0:edddd373a163 273 motor1.brake(1);
Najib_irvani 1:fc1535231c0d 274 motor3.speed(-(speed-tuning3));
rizqicahyo 0:edddd373a163 275 pc.printf("serong bawah kiri \n");
rizqicahyo 0:edddd373a163 276
rizqicahyo 2:df6c49846367 277 vcurr += ax;
rizqicahyo 0:edddd373a163 278 }
rizqicahyo 0:edddd373a163 279 else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_RIGHT)==1)){
rizqicahyo 0:edddd373a163 280 //serong bawah kanan
rizqicahyo 0:edddd373a163 281 motor2.brake(1);
Najib_irvani 1:fc1535231c0d 282 motor4.speed(speed-tuning4);
Najib_irvani 1:fc1535231c0d 283 motor1.speed(speed-tuning1);
rizqicahyo 0:edddd373a163 284 motor3.brake(1);
rizqicahyo 0:edddd373a163 285 pc.printf("serong bawah kanan \n");
rizqicahyo 0:edddd373a163 286
rizqicahyo 2:df6c49846367 287 vcurr += ax;
rizqicahyo 0:edddd373a163 288 }
rizqicahyo 0:edddd373a163 289 else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){
rizqicahyo 0:edddd373a163 290 //maju
Najib_irvani 1:fc1535231c0d 291 motor1.speed(-(speed-tuning1));
Najib_irvani 1:fc1535231c0d 292 motor3.speed(speed-tuning3);
Najib_irvani 1:fc1535231c0d 293 motor2.speed(speed-tuning2);
Najib_irvani 1:fc1535231c0d 294 motor4.speed(-(speed-tuning4));
rizqicahyo 0:edddd373a163 295 pc.printf("maju \n");
rizqicahyo 0:edddd373a163 296
rizqicahyo 2:df6c49846367 297 vcurr += ax;
rizqicahyo 0:edddd373a163 298 }
rizqicahyo 0:edddd373a163 299 else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){
rizqicahyo 0:edddd373a163 300 //mundur
Najib_irvani 1:fc1535231c0d 301 motor1.speed(speed-tuning1);
Najib_irvani 1:fc1535231c0d 302 motor3.speed(-(speed-tuning3));
Najib_irvani 1:fc1535231c0d 303 motor2.speed(-(speed-tuning2));
Najib_irvani 1:fc1535231c0d 304 motor4.speed(speed-tuning4);
rizqicahyo 0:edddd373a163 305 pc.printf("mundur \n");
rizqicahyo 0:edddd373a163 306
rizqicahyo 2:df6c49846367 307 vcurr += ax;
rizqicahyo 0:edddd373a163 308 }
rizqicahyo 0:edddd373a163 309 else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_LEFT)==0)){
rizqicahyo 0:edddd373a163 310 //kanan
Najib_irvani 1:fc1535231c0d 311 motor2.speed(speed-tuning2);
Najib_irvani 1:fc1535231c0d 312 motor4.speed(speed-tuning4);
Najib_irvani 1:fc1535231c0d 313 motor1.speed(speed-tuning1);
Najib_irvani 1:fc1535231c0d 314 motor3.speed(speed-tuning3);
rizqicahyo 0:edddd373a163 315 pc.printf("kanan \n");
rizqicahyo 0:edddd373a163 316
rizqicahyo 2:df6c49846367 317 vcurr += ax;
rizqicahyo 0:edddd373a163 318 }
rizqicahyo 0:edddd373a163 319 else if((ps2.read(PS_PAD::PAD_RIGHT)==0) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
rizqicahyo 0:edddd373a163 320 //kiri
Najib_irvani 1:fc1535231c0d 321 motor2.speed(-(speed-tuning2));
Najib_irvani 1:fc1535231c0d 322 motor4.speed(-(speed-tuning4));
Najib_irvani 1:fc1535231c0d 323 motor1.speed(-(speed-tuning1));
Najib_irvani 1:fc1535231c0d 324 motor3.speed(-(speed-tuning3));
rizqicahyo 0:edddd373a163 325 pc.printf("kiri \n");
rizqicahyo 0:edddd373a163 326
rizqicahyo 2:df6c49846367 327 vcurr += ax;
rizqicahyo 0:edddd373a163 328 }
rizqicahyo 0:edddd373a163 329 else{
rizqicahyo 0:edddd373a163 330 motor1.brake(1);
rizqicahyo 0:edddd373a163 331 motor3.brake(1);
rizqicahyo 0:edddd373a163 332 motor2.brake(1);
rizqicahyo 0:edddd373a163 333 motor4.brake(1);
rizqicahyo 0:edddd373a163 334 pc.printf("diam \n");
rizqicahyo 0:edddd373a163 335
rizqicahyo 2:df6c49846367 336 vcurr = v0;
rizqicahyo 0:edddd373a163 337 }
rizqicahyo 3:43d4cb3ece1b 338 }
rizqicahyo 3:43d4cb3ece1b 339 else{
rizqicahyo 3:43d4cb3ece1b 340 if((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){
rizqicahyo 3:43d4cb3ece1b 341 //pivot kiri
rizqicahyo 3:43d4cb3ece1b 342 motor2.speed((float)0.5*(speed-tuning2));
rizqicahyo 3:43d4cb3ece1b 343 motor4.speed((float)-0.5*(speed-tuning4));
rizqicahyo 3:43d4cb3ece1b 344 motor1.speed((float)0.5*(speed-tuning1));
rizqicahyo 3:43d4cb3ece1b 345 motor3.speed((float)-0.5*(speed-tuning3));
rizqicahyo 3:43d4cb3ece1b 346 pc.printf("pivot kiri \n");
rizqicahyo 3:43d4cb3ece1b 347
rizqicahyo 3:43d4cb3ece1b 348 vcurr += ax;
rizqicahyo 3:43d4cb3ece1b 349 }
rizqicahyo 3:43d4cb3ece1b 350 else if((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){
rizqicahyo 3:43d4cb3ece1b 351 //pivot kanan
rizqicahyo 3:43d4cb3ece1b 352 motor2.speed((float)-0.5*(speed-tuning2));
rizqicahyo 3:43d4cb3ece1b 353 motor4.speed((float)0.5*(speed-tuning4));
rizqicahyo 3:43d4cb3ece1b 354 motor1.speed((float)-0.5*(speed-tuning1));
rizqicahyo 3:43d4cb3ece1b 355 motor3.speed((float)0.5*(speed-tuning3));
rizqicahyo 3:43d4cb3ece1b 356 pc.printf("pivot kanan \n");
rizqicahyo 2:df6c49846367 357
rizqicahyo 3:43d4cb3ece1b 358 vcurr += ax;
rizqicahyo 3:43d4cb3ece1b 359 }
rizqicahyo 3:43d4cb3ece1b 360 else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_TOP)==1)){
rizqicahyo 3:43d4cb3ece1b 361 //serong atas kanan
rizqicahyo 3:43d4cb3ece1b 362 motor2.speed(-(speed-tuning2));
rizqicahyo 3:43d4cb3ece1b 363 motor4.brake(1);
rizqicahyo 3:43d4cb3ece1b 364 motor1.brake(1);
rizqicahyo 3:43d4cb3ece1b 365 motor3.speed(-(speed-tuning3));
rizqicahyo 3:43d4cb3ece1b 366 pc.printf("serong atas kanan \n");
rizqicahyo 2:df6c49846367 367
rizqicahyo 3:43d4cb3ece1b 368 vcurr += ax;
rizqicahyo 3:43d4cb3ece1b 369 }
rizqicahyo 3:43d4cb3ece1b 370 else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
rizqicahyo 3:43d4cb3ece1b 371 //serong atas kiri
rizqicahyo 3:43d4cb3ece1b 372 motor2.brake(1);
rizqicahyo 3:43d4cb3ece1b 373 motor4.speed(speed-tuning4);
rizqicahyo 3:43d4cb3ece1b 374 motor1.speed(speed-tuning1);
rizqicahyo 3:43d4cb3ece1b 375 motor3.brake(1);
rizqicahyo 3:43d4cb3ece1b 376 pc.printf("serong atas kiri \n");
rizqicahyo 3:43d4cb3ece1b 377
rizqicahyo 3:43d4cb3ece1b 378 vcurr += ax;
rizqicahyo 3:43d4cb3ece1b 379 }
rizqicahyo 3:43d4cb3ece1b 380 else if((ps2.read(PS_PAD::PAD_LEFT)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==1)){
rizqicahyo 3:43d4cb3ece1b 381 //serong bawah kiri
rizqicahyo 3:43d4cb3ece1b 382 motor2.speed(speed-tuning2);
rizqicahyo 3:43d4cb3ece1b 383 motor4.brake(1);
rizqicahyo 3:43d4cb3ece1b 384 motor1.brake(1);
rizqicahyo 3:43d4cb3ece1b 385 motor3.speed(speed-tuning3);
rizqicahyo 3:43d4cb3ece1b 386 pc.printf("serong bawah kiri \n");
rizqicahyo 2:df6c49846367 387
rizqicahyo 3:43d4cb3ece1b 388 vcurr += ax;
rizqicahyo 0:edddd373a163 389 }
rizqicahyo 3:43d4cb3ece1b 390 else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_RIGHT)==1)){
rizqicahyo 3:43d4cb3ece1b 391 //serong bawah kanan
rizqicahyo 3:43d4cb3ece1b 392 motor2.brake(1);
rizqicahyo 3:43d4cb3ece1b 393 motor4.speed(-(speed-tuning4));
rizqicahyo 3:43d4cb3ece1b 394 motor1.speed(-(speed-tuning1));
rizqicahyo 3:43d4cb3ece1b 395 motor3.brake(1);
rizqicahyo 3:43d4cb3ece1b 396 pc.printf("serong bawah kanan \n");
rizqicahyo 3:43d4cb3ece1b 397
rizqicahyo 3:43d4cb3ece1b 398 vcurr += ax;
rizqicahyo 3:43d4cb3ece1b 399 }
rizqicahyo 3:43d4cb3ece1b 400 else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){
rizqicahyo 3:43d4cb3ece1b 401 //maju
rizqicahyo 3:43d4cb3ece1b 402 motor1.speed(speed-tuning1);
rizqicahyo 3:43d4cb3ece1b 403 motor3.speed(-(speed-tuning3));
rizqicahyo 3:43d4cb3ece1b 404 motor2.speed(-(speed-tuning2));
rizqicahyo 3:43d4cb3ece1b 405 motor4.speed(speed-tuning4);
rizqicahyo 3:43d4cb3ece1b 406 pc.printf("maju \n");
rizqicahyo 2:df6c49846367 407
rizqicahyo 3:43d4cb3ece1b 408 vcurr += ax;
rizqicahyo 3:43d4cb3ece1b 409 }
rizqicahyo 3:43d4cb3ece1b 410 else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){
rizqicahyo 3:43d4cb3ece1b 411 //mundur
rizqicahyo 3:43d4cb3ece1b 412 motor1.speed(-(speed-tuning1));
rizqicahyo 3:43d4cb3ece1b 413 motor3.speed(speed-tuning3);
rizqicahyo 3:43d4cb3ece1b 414 motor2.speed(speed-tuning2);
rizqicahyo 3:43d4cb3ece1b 415 motor4.speed(-(speed-tuning4));
rizqicahyo 3:43d4cb3ece1b 416 pc.printf("mundur \n");
rizqicahyo 2:df6c49846367 417
rizqicahyo 3:43d4cb3ece1b 418 vcurr += ax;
rizqicahyo 3:43d4cb3ece1b 419 }
rizqicahyo 3:43d4cb3ece1b 420 else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_LEFT)==0)){
rizqicahyo 3:43d4cb3ece1b 421 //kanan
rizqicahyo 3:43d4cb3ece1b 422 motor2.speed(-(speed-tuning2));
rizqicahyo 3:43d4cb3ece1b 423 motor4.speed(-(speed-tuning4));
rizqicahyo 3:43d4cb3ece1b 424 motor1.speed(-(speed-tuning1));
rizqicahyo 3:43d4cb3ece1b 425 motor3.speed(-(speed-tuning3));
rizqicahyo 3:43d4cb3ece1b 426 pc.printf("kanan \n");
rizqicahyo 3:43d4cb3ece1b 427
rizqicahyo 3:43d4cb3ece1b 428 vcurr += ax;
rizqicahyo 3:43d4cb3ece1b 429 }
rizqicahyo 3:43d4cb3ece1b 430 else if((ps2.read(PS_PAD::PAD_RIGHT)==0) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
rizqicahyo 3:43d4cb3ece1b 431 //kiri
rizqicahyo 3:43d4cb3ece1b 432 motor2.speed(speed-tuning2);
rizqicahyo 3:43d4cb3ece1b 433 motor4.speed(speed-tuning4);
rizqicahyo 3:43d4cb3ece1b 434 motor1.speed(speed-tuning1);
rizqicahyo 3:43d4cb3ece1b 435 motor3.speed(speed-tuning3);
rizqicahyo 3:43d4cb3ece1b 436 pc.printf("kiri \n");
rizqicahyo 3:43d4cb3ece1b 437
rizqicahyo 3:43d4cb3ece1b 438 vcurr += ax;
rizqicahyo 0:edddd373a163 439 }
rizqicahyo 0:edddd373a163 440 else{
rizqicahyo 3:43d4cb3ece1b 441 motor1.brake(1);
rizqicahyo 3:43d4cb3ece1b 442 motor3.brake(1);
rizqicahyo 3:43d4cb3ece1b 443 motor2.brake(1);
rizqicahyo 3:43d4cb3ece1b 444 motor4.brake(1);
rizqicahyo 3:43d4cb3ece1b 445 pc.printf("diam \n");
rizqicahyo 0:edddd373a163 446
rizqicahyo 3:43d4cb3ece1b 447 vcurr = v0;
rizqicahyo 3:43d4cb3ece1b 448 }
rizqicahyo 3:43d4cb3ece1b 449
rizqicahyo 3:43d4cb3ece1b 450 }
rizqicahyo 3:43d4cb3ece1b 451
rizqicahyo 3:43d4cb3ece1b 452 if((ps2.read(PS_PAD::PAD_CIRCLE)==0) && (ps2.read(PS_PAD::PAD_TRIANGLE)==1)){
rizqicahyo 3:43d4cb3ece1b 453 //POWER WINDOW ATAS
rizqicahyo 3:43d4cb3ece1b 454 motorS.speed(1);
rizqicahyo 3:43d4cb3ece1b 455 if (limit1 == 0){
rizqicahyo 3:43d4cb3ece1b 456 motorS.brake(1);
rizqicahyo 3:43d4cb3ece1b 457 }
rizqicahyo 3:43d4cb3ece1b 458
rizqicahyo 3:43d4cb3ece1b 459
rizqicahyo 3:43d4cb3ece1b 460 pc.printf("up \n");
rizqicahyo 3:43d4cb3ece1b 461 c++;
rizqicahyo 3:43d4cb3ece1b 462 }
rizqicahyo 3:43d4cb3ece1b 463 else if((ps2.read(PS_PAD::PAD_CIRCLE)==1) && (ps2.read(PS_PAD::PAD_TRIANGLE)==0)){
rizqicahyo 3:43d4cb3ece1b 464 //POWER WINDOW BAWAH
rizqicahyo 3:43d4cb3ece1b 465 motorS.speed(-0.5);
rizqicahyo 3:43d4cb3ece1b 466
rizqicahyo 3:43d4cb3ece1b 467 if (limit2 ==0){
rizqicahyo 2:df6c49846367 468 motorS.brake(1);
rizqicahyo 2:df6c49846367 469 }
rizqicahyo 3:43d4cb3ece1b 470
rizqicahyo 3:43d4cb3ece1b 471 pc.printf("down \n");
rizqicahyo 3:43d4cb3ece1b 472 c--;
rizqicahyo 3:43d4cb3ece1b 473 }
rizqicahyo 3:43d4cb3ece1b 474 else{
rizqicahyo 3:43d4cb3ece1b 475 motorS.brake(1);
rizqicahyo 3:43d4cb3ece1b 476 if ((c <= batas_delay) && (c>=-batas_delay)){
rizqicahyo 3:43d4cb3ece1b 477 c=0;
rizqicahyo 2:df6c49846367 478 }
rizqicahyo 3:43d4cb3ece1b 479
rizqicahyo 3:43d4cb3ece1b 480 pc.printf("diam \n");
rizqicahyo 3:43d4cb3ece1b 481 }
rizqicahyo 3:43d4cb3ece1b 482
rizqicahyo 3:43d4cb3ece1b 483 if((c > batas_delay) && (limit1 == 0)){
rizqicahyo 3:43d4cb3ece1b 484 c = 0;
rizqicahyo 3:43d4cb3ece1b 485 motorS.brake(1);
rizqicahyo 3:43d4cb3ece1b 486 }
rizqicahyo 3:43d4cb3ece1b 487 else if((c < -batas_delay) && (limit2 == 0)){
rizqicahyo 3:43d4cb3ece1b 488 c = 0;
rizqicahyo 3:43d4cb3ece1b 489 motorS.brake(1);
rizqicahyo 3:43d4cb3ece1b 490 }
rizqicahyo 3:43d4cb3ece1b 491 else if( (c > batas_delay) && (limit1 != 0)){
rizqicahyo 3:43d4cb3ece1b 492 motorS.speed(1);
rizqicahyo 3:43d4cb3ece1b 493 }
rizqicahyo 3:43d4cb3ece1b 494 else if ((c<-batas_delay) && (limit2 != 0)){
rizqicahyo 3:43d4cb3ece1b 495 motorS.speed(-0.7);
rizqicahyo 3:43d4cb3ece1b 496 }
rizqicahyo 2:df6c49846367 497
rizqicahyo 0:edddd373a163 498
rizqicahyo 3:43d4cb3ece1b 499 if ((ps2.read(PS_PAD::PAD_SELECT)==1))
rizqicahyo 3:43d4cb3ece1b 500 {
rizqicahyo 0:edddd373a163 501 //mekanisme ambil gripper
rizqicahyo 0:edddd373a163 502 pc.printf("mekanisme gripper");
rizqicahyo 0:edddd373a163 503 if (g==1){
rizqicahyo 0:edddd373a163 504 pc.printf("ambil 1");
rizqicahyo 0:edddd373a163 505 pnuematik2 = 0;
rizqicahyo 0:edddd373a163 506 g=2;
rizqicahyo 0:edddd373a163 507 wait_ms(400);
rizqicahyo 0:edddd373a163 508 }
rizqicahyo 0:edddd373a163 509 else
rizqicahyo 0:edddd373a163 510 {
rizqicahyo 0:edddd373a163 511 pnuematik2 = 1;
rizqicahyo 0:edddd373a163 512 wait_ms(400);
rizqicahyo 0:edddd373a163 513 g=1;
rizqicahyo 0:edddd373a163 514 }
rizqicahyo 3:43d4cb3ece1b 515 }
rizqicahyo 0:edddd373a163 516 }
rizqicahyo 0:edddd373a163 517
rizqicahyo 0:edddd373a163 518 void edf_servo(){
rizqicahyo 0:edddd373a163 519 if(ps2.read(PS_PAD::PAD_X)==1){
rizqicahyo 0:edddd373a163 520 //PWM ++
rizqicahyo 2:df6c49846367 521 pwm += 0.0007;
rizqicahyo 4:7a7a8aa33fd5 522 if( pwm > 0.8) pwm = 0.8;
rizqicahyo 0:edddd373a163 523 pc.printf("gaspol \n");
rizqicahyo 0:edddd373a163 524 }
rizqicahyo 0:edddd373a163 525 else if(ps2.read(PS_PAD::PAD_SQUARE)==1){
rizqicahyo 0:edddd373a163 526 //PWM--
rizqicahyo 2:df6c49846367 527 pwm -= 0.0007;
rizqicahyo 0:edddd373a163 528
rizqicahyo 0:edddd373a163 529 if(pwm < 0) pwm = 0.0;
rizqicahyo 0:edddd373a163 530 pc.printf("rem ndeng \n");
rizqicahyo 0:edddd373a163 531 }
rizqicahyo 0:edddd373a163 532
rizqicahyo 0:edddd373a163 533 if(ps2.read(PS_PAD::PAD_R2)==1){
rizqicahyo 0:edddd373a163 534 //SERVO --
rizqicahyo 0:edddd373a163 535 sudut += 0.5;
rizqicahyo 0:edddd373a163 536
rizqicahyo 0:edddd373a163 537 if(sudut > 90) sudut = 90;
rizqicahyo 0:edddd373a163 538 pc.printf("servo max \n");
rizqicahyo 0:edddd373a163 539 }
rizqicahyo 0:edddd373a163 540 else if(ps2.read(PS_PAD::PAD_L2)==1){
rizqicahyo 0:edddd373a163 541 //SERVO ++
rizqicahyo 0:edddd373a163 542 sudut -= 0.5;
rizqicahyo 0:edddd373a163 543
rizqicahyo 0:edddd373a163 544 if(sudut < -90) sudut = -90;
rizqicahyo 0:edddd373a163 545 pc.printf("servo min \n");
rizqicahyo 0:edddd373a163 546 }
rizqicahyo 0:edddd373a163 547
rizqicahyo 0:edddd373a163 548 if(ps2.read(PS_PAD::PAD_START)==1){
rizqicahyo 0:edddd373a163 549
rizqicahyo 0:edddd373a163 550 sudut = 0;
rizqicahyo 3:43d4cb3ece1b 551 pwm = 0.22;
rizqicahyo 0:edddd373a163 552 }
rizqicahyo 3:43d4cb3ece1b 553
rizqicahyo 3:43d4cb3ece1b 554
rizqicahyo 0:edddd373a163 555 servoEDF.position((float)sudut);
rizqicahyo 0:edddd373a163 556 edf.setThrottle((float)pwm);
rizqicahyo 0:edddd373a163 557 edf.pulse();
rizqicahyo 2:df6c49846367 558 }
rizqicahyo 2:df6c49846367 559
rizqicahyo 4:7a7a8aa33fd5 560 /////////////////////////////////////////LINE FOLLOWER/////////////////////////
rizqicahyo 3:43d4cb3ece1b 561
rizqicahyo 2:df6c49846367 562 void init_sensor(){
rizqicahyo 2:df6c49846367 563 char data;
rizqicahyo 2:df6c49846367 564 if(com.readable()){
rizqicahyo 2:df6c49846367 565 data = com.getc();
rizqicahyo 2:df6c49846367 566
rizqicahyo 2:df6c49846367 567 for(int i=0; i<6; i++){
rizqicahyo 2:df6c49846367 568 datasensor[i] = (data >> i) & 1;
rizqicahyo 2:df6c49846367 569 }
rizqicahyo 2:df6c49846367 570 }
rizqicahyo 2:df6c49846367 571 }
rizqicahyo 2:df6c49846367 572
rizqicahyo 3:43d4cb3ece1b 573 void linetracer(float speed){
rizqicahyo 2:df6c49846367 574 float speed1,speed2,speed3,speed4;
rizqicahyo 3:43d4cb3ece1b 575
rizqicahyo 2:df6c49846367 576 //////////////////logic dari PV (present Value)/////////////////////////
rizqicahyo 3:43d4cb3ece1b 577 if(datasensor[2] && datasensor[3]){
rizqicahyo 3:43d4cb3ece1b 578 speed1 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 579 speed2 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 580 speed3 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 581 speed4 = speed*driver0;
rizqicahyo 2:df6c49846367 582 }
rizqicahyo 3:43d4cb3ece1b 583 else if(datasensor[2]){
rizqicahyo 3:43d4cb3ece1b 584 speed1 = speed*driver1;
rizqicahyo 3:43d4cb3ece1b 585 speed2 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 586 speed3 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 587 speed4 = speed*driver1;
rizqicahyo 2:df6c49846367 588 }
rizqicahyo 3:43d4cb3ece1b 589 else if(datasensor[3]){
rizqicahyo 3:43d4cb3ece1b 590 speed1 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 591 speed2 = speed*driver1;
rizqicahyo 3:43d4cb3ece1b 592 speed3 = speed*driver1;
rizqicahyo 3:43d4cb3ece1b 593 speed4 = speed*driver0;
rizqicahyo 2:df6c49846367 594 }
rizqicahyo 2:df6c49846367 595 else if(datasensor[1]){
rizqicahyo 3:43d4cb3ece1b 596 speed1 = speed*driver2;
rizqicahyo 3:43d4cb3ece1b 597 speed2 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 598 speed3 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 599 speed4 = speed*driver2;
rizqicahyo 2:df6c49846367 600 }
rizqicahyo 2:df6c49846367 601 else if(datasensor[4]){
rizqicahyo 3:43d4cb3ece1b 602 speed1 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 603 speed2 = speed*driver2;
rizqicahyo 3:43d4cb3ece1b 604 speed3 = speed*driver2;
rizqicahyo 3:43d4cb3ece1b 605 speed4 = speed*driver0;
rizqicahyo 2:df6c49846367 606 }
rizqicahyo 3:43d4cb3ece1b 607 else if(datasensor[0]){
rizqicahyo 3:43d4cb3ece1b 608 speed1 = speed*driver3;
rizqicahyo 3:43d4cb3ece1b 609 speed2 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 610 speed3 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 611 speed4 = speed*driver3;
rizqicahyo 2:df6c49846367 612 }
rizqicahyo 3:43d4cb3ece1b 613 else if(datasensor[5]){
rizqicahyo 3:43d4cb3ece1b 614 speed1 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 615 speed2 = speed*driver3;
rizqicahyo 3:43d4cb3ece1b 616 speed3 = speed*driver3;
rizqicahyo 3:43d4cb3ece1b 617 speed4 = speed*driver0;
rizqicahyo 2:df6c49846367 618 }
rizqicahyo 3:43d4cb3ece1b 619
rizqicahyo 2:df6c49846367 620 else
rizqicahyo 2:df6c49846367 621 {
rizqicahyo 3:43d4cb3ece1b 622 if(g_flag == 0){
rizqicahyo 3:43d4cb3ece1b 623 speed1 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 624 speed2 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 625 speed3 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 626 speed4 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 627 }
rizqicahyo 3:43d4cb3ece1b 628 else if(g_flag == 3){
rizqicahyo 3:43d4cb3ece1b 629 speed1 = speed*driver1;
rizqicahyo 3:43d4cb3ece1b 630 speed2 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 631 speed3 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 632 speed4 = speed*driver1;
rizqicahyo 3:43d4cb3ece1b 633 }
rizqicahyo 3:43d4cb3ece1b 634 else if(g_flag == 4){
rizqicahyo 3:43d4cb3ece1b 635 speed1 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 636 speed2 = speed*driver1;
rizqicahyo 3:43d4cb3ece1b 637 speed3 = speed*driver1;
rizqicahyo 3:43d4cb3ece1b 638 speed4 = speed*driver0;
rizqicahyo 2:df6c49846367 639 }
rizqicahyo 3:43d4cb3ece1b 640 else if(g_flag == 2){
rizqicahyo 3:43d4cb3ece1b 641 speed1 = speed*driver2;
rizqicahyo 3:43d4cb3ece1b 642 speed2 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 643 speed3 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 644 speed4 = speed*driver2;
rizqicahyo 3:43d4cb3ece1b 645 }
rizqicahyo 3:43d4cb3ece1b 646 else if(g_flag == 5){
rizqicahyo 3:43d4cb3ece1b 647 speed1 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 648 speed2 = speed*driver2;
rizqicahyo 3:43d4cb3ece1b 649 speed3 = speed*driver2;
rizqicahyo 3:43d4cb3ece1b 650 speed4 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 651 }
rizqicahyo 3:43d4cb3ece1b 652 else if(g_flag == 1){
rizqicahyo 3:43d4cb3ece1b 653 speed1 = -speed*driver2;
rizqicahyo 3:43d4cb3ece1b 654 speed2 = speed*driver2;
rizqicahyo 3:43d4cb3ece1b 655 speed3 = speed*driver2;
rizqicahyo 3:43d4cb3ece1b 656 speed4 = -speed*driver2;
rizqicahyo 3:43d4cb3ece1b 657 }
rizqicahyo 3:43d4cb3ece1b 658 else if(g_flag == 6){
rizqicahyo 3:43d4cb3ece1b 659 speed1 = speed*driver2;
rizqicahyo 3:43d4cb3ece1b 660 speed2 = -speed*driver2;
rizqicahyo 3:43d4cb3ece1b 661 speed3 = -speed*driver2;
rizqicahyo 3:43d4cb3ece1b 662 speed4 = speed*driver2;
rizqicahyo 2:df6c49846367 663 }
rizqicahyo 2:df6c49846367 664 }
rizqicahyo 2:df6c49846367 665
rizqicahyo 4:7a7a8aa33fd5 666 motor1.speed(-(float)(speed1-0.04));
rizqicahyo 4:7a7a8aa33fd5 667 motor2.speed(-(float)(speed2-0.04));
rizqicahyo 4:7a7a8aa33fd5 668 motor3.speed(-(float)(speed3-0.0));
rizqicahyo 4:7a7a8aa33fd5 669 motor4.speed(-(float)(speed4-0.0));
rizqicahyo 3:43d4cb3ece1b 670
rizqicahyo 3:43d4cb3ece1b 671 }
rizqicahyo 3:43d4cb3ece1b 672
rizqicahyo 3:43d4cb3ece1b 673 void flag_sensor(){
rizqicahyo 3:43d4cb3ece1b 674 if((datasensor[2] == 1) && (datasensor[3] == 1)) g_flag = 0;
rizqicahyo 3:43d4cb3ece1b 675 else if(datasensor[2] == 1) g_flag = 3;
rizqicahyo 3:43d4cb3ece1b 676 else if(datasensor[3] == 1) g_flag = 4;
rizqicahyo 3:43d4cb3ece1b 677 else if(datasensor[1] == 1) g_flag = 2;
rizqicahyo 3:43d4cb3ece1b 678 else if(datasensor[4] == 1) g_flag = 5;
rizqicahyo 3:43d4cb3ece1b 679 else if(datasensor[0] == 1) g_flag = 1;
rizqicahyo 3:43d4cb3ece1b 680 else if(datasensor[5] == 1) g_flag = 6;
rizqicahyo 4:7a7a8aa33fd5 681 }
rizqicahyo 4:7a7a8aa33fd5 682
rizqicahyo 4:7a7a8aa33fd5 683
rizqicahyo 4:7a7a8aa33fd5 684 ////////////////////////SENSOR PING///////////////////////////////////////
rizqicahyo 4:7a7a8aa33fd5 685 float read_jarak() {
rizqicahyo 4:7a7a8aa33fd5 686 float jarak;
rizqicahyo 4:7a7a8aa33fd5 687
rizqicahyo 4:7a7a8aa33fd5 688 ping.Send();
rizqicahyo 4:7a7a8aa33fd5 689 wait_ms(45);
rizqicahyo 4:7a7a8aa33fd5 690 jarak = ping.Read_cm()/2;
rizqicahyo 4:7a7a8aa33fd5 691 return jarak;
rizqicahyo 0:edddd373a163 692 }