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@4:7a7a8aa33fd5, 2016-04-27 (annotated)

Committer:
rizqicahyo
Date:
Wed Apr 27 14:28:05 2016 +0000
Revision:
4:7a7a8aa33fd5
Parent:
3:43d4cb3ece1b
Child:
5:256f6eac0358
tambah sensor ping, ganti tombol line follower

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 4:7a7a8aa33fd5 177 }
rizqicahyo 3:43d4cb3ece1b 178 }
rizqicahyo 0:edddd373a163 179 motor1.brake(1);
rizqicahyo 0:edddd373a163 180 motor2.brake(1);
rizqicahyo 0:edddd373a163 181 motor3.brake(1);
rizqicahyo 0:edddd373a163 182 motor4.brake(1);
rizqicahyo 0:edddd373a163 183
rizqicahyo 3:43d4cb3ece1b 184 timer.detach();
rizqicahyo 3:43d4cb3ece1b 185
rizqicahyo 0:edddd373a163 186 pnuematik1=0;
rizqicahyo 0:edddd373a163 187 wait_ms(1500);
rizqicahyo 4:7a7a8aa33fd5 188
rizqicahyo 0:edddd373a163 189 while(limit4!=0){
rizqicahyo 4:7a7a8aa33fd5 190 motorC1.speed(0.95);
rizqicahyo 0:edddd373a163 191 motorC2.speed(-1);
rizqicahyo 0:edddd373a163 192 }
rizqicahyo 0:edddd373a163 193
rizqicahyo 0:edddd373a163 194 motorC1.brake(1);
rizqicahyo 0:edddd373a163 195 motorC2.brake(1);
rizqicahyo 0:edddd373a163 196
rizqicahyo 3:43d4cb3ece1b 197 if(field==1){
rizqicahyo 3:43d4cb3ece1b 198 pnuematik3 = 0;
rizqicahyo 3:43d4cb3ece1b 199 wait_ms(1500);
rizqicahyo 3:43d4cb3ece1b 200 pnuematik2 = 1;
rizqicahyo 3:43d4cb3ece1b 201 wait_ms(500);
rizqicahyo 3:43d4cb3ece1b 202 pnuematik3 = 1;
rizqicahyo 3:43d4cb3ece1b 203 }
rizqicahyo 3:43d4cb3ece1b 204 else{
rizqicahyo 3:43d4cb3ece1b 205 pnuematik4 = 0;
rizqicahyo 3:43d4cb3ece1b 206 wait_ms(1500);
rizqicahyo 3:43d4cb3ece1b 207 pnuematik2 = 1;
rizqicahyo 3:43d4cb3ece1b 208 wait_ms(500);
rizqicahyo 3:43d4cb3ece1b 209 pnuematik4 = 1;
rizqicahyo 3:43d4cb3ece1b 210 }
rizqicahyo 0:edddd373a163 211
rizqicahyo 0:edddd373a163 212 return 0;
rizqicahyo 0:edddd373a163 213 }
rizqicahyo 0:edddd373a163 214
rizqicahyo 0:edddd373a163 215 /*********************************************************************************************/
rizqicahyo 0:edddd373a163 216 /** ALGORITMA PROSEDUR DAN FUNGSI **/
rizqicahyo 0:edddd373a163 217 /*********************************************************************************************/
rizqicahyo 3:43d4cb3ece1b 218 void aktuator(int f){
rizqicahyo 2:df6c49846367 219 float speed = vcurr;
rizqicahyo 2:df6c49846367 220
rizqicahyo 2:df6c49846367 221 if(vcurr >= gMax_speed) vcurr = gMax_speed;
Najib_irvani 1:fc1535231c0d 222
rizqicahyo 3:43d4cb3ece1b 223 if(f == 1){
rizqicahyo 0:edddd373a163 224 if((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){
rizqicahyo 0:edddd373a163 225 //pivot kiri
rizqicahyo 3:43d4cb3ece1b 226 motor2.speed((float)0.5*(speed-tuning2));
rizqicahyo 3:43d4cb3ece1b 227 motor4.speed((float)-0.5*(speed-tuning4));
rizqicahyo 3:43d4cb3ece1b 228 motor1.speed((float)0.5*(speed-tuning1));
rizqicahyo 3:43d4cb3ece1b 229 motor3.speed((float)-0.5*(speed-tuning3));
rizqicahyo 0:edddd373a163 230 pc.printf("pivot kiri \n");
rizqicahyo 0:edddd373a163 231
rizqicahyo 2:df6c49846367 232 vcurr += ax;
rizqicahyo 0:edddd373a163 233 }
rizqicahyo 0:edddd373a163 234 else if((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){
rizqicahyo 0:edddd373a163 235 //pivot kanan
rizqicahyo 3:43d4cb3ece1b 236 motor2.speed((float)-0.5*(speed-tuning2));
rizqicahyo 3:43d4cb3ece1b 237 motor4.speed((float)0.5*(speed-tuning4));
rizqicahyo 3:43d4cb3ece1b 238 motor1.speed((float)-0.5*(speed-tuning1));
rizqicahyo 3:43d4cb3ece1b 239 motor3.speed((float)0.5*(speed-tuning3));
rizqicahyo 0:edddd373a163 240 pc.printf("pivot kanan \n");
rizqicahyo 0:edddd373a163 241
rizqicahyo 2:df6c49846367 242 vcurr += ax;
rizqicahyo 0:edddd373a163 243 }
rizqicahyo 0:edddd373a163 244 else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_TOP)==1)){
rizqicahyo 0:edddd373a163 245 //serong atas kanan
Najib_irvani 1:fc1535231c0d 246 motor2.speed(speed-tuning2);
rizqicahyo 0:edddd373a163 247 motor4.brake(1);
rizqicahyo 0:edddd373a163 248 motor1.brake(1);
Najib_irvani 1:fc1535231c0d 249 motor3.speed(speed-tuning3);
rizqicahyo 0:edddd373a163 250 pc.printf("serong atas kanan \n");
rizqicahyo 0:edddd373a163 251
rizqicahyo 2:df6c49846367 252 vcurr += ax;
rizqicahyo 0:edddd373a163 253 }
rizqicahyo 0:edddd373a163 254 else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
rizqicahyo 0:edddd373a163 255 //serong atas kiri
rizqicahyo 0:edddd373a163 256 motor2.brake(1);
Najib_irvani 1:fc1535231c0d 257 motor4.speed(-(speed-tuning4));
Najib_irvani 1:fc1535231c0d 258 motor1.speed(-(speed-tuning1));
rizqicahyo 0:edddd373a163 259 motor3.brake(1);
rizqicahyo 0:edddd373a163 260 pc.printf("serong atas kiri \n");
rizqicahyo 0:edddd373a163 261
rizqicahyo 2:df6c49846367 262 vcurr += ax;
rizqicahyo 0:edddd373a163 263 }
rizqicahyo 0:edddd373a163 264 else if((ps2.read(PS_PAD::PAD_LEFT)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==1)){
rizqicahyo 0:edddd373a163 265 //serong bawah kiri
Najib_irvani 1:fc1535231c0d 266 motor2.speed(-(speed-tuning2));
rizqicahyo 0:edddd373a163 267 motor4.brake(1);
rizqicahyo 0:edddd373a163 268 motor1.brake(1);
Najib_irvani 1:fc1535231c0d 269 motor3.speed(-(speed-tuning3));
rizqicahyo 0:edddd373a163 270 pc.printf("serong bawah kiri \n");
rizqicahyo 0:edddd373a163 271
rizqicahyo 2:df6c49846367 272 vcurr += ax;
rizqicahyo 0:edddd373a163 273 }
rizqicahyo 0:edddd373a163 274 else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_RIGHT)==1)){
rizqicahyo 0:edddd373a163 275 //serong bawah kanan
rizqicahyo 0:edddd373a163 276 motor2.brake(1);
Najib_irvani 1:fc1535231c0d 277 motor4.speed(speed-tuning4);
Najib_irvani 1:fc1535231c0d 278 motor1.speed(speed-tuning1);
rizqicahyo 0:edddd373a163 279 motor3.brake(1);
rizqicahyo 0:edddd373a163 280 pc.printf("serong bawah kanan \n");
rizqicahyo 0:edddd373a163 281
rizqicahyo 2:df6c49846367 282 vcurr += ax;
rizqicahyo 0:edddd373a163 283 }
rizqicahyo 0:edddd373a163 284 else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){
rizqicahyo 0:edddd373a163 285 //maju
Najib_irvani 1:fc1535231c0d 286 motor1.speed(-(speed-tuning1));
Najib_irvani 1:fc1535231c0d 287 motor3.speed(speed-tuning3);
Najib_irvani 1:fc1535231c0d 288 motor2.speed(speed-tuning2);
Najib_irvani 1:fc1535231c0d 289 motor4.speed(-(speed-tuning4));
rizqicahyo 0:edddd373a163 290 pc.printf("maju \n");
rizqicahyo 0:edddd373a163 291
rizqicahyo 2:df6c49846367 292 vcurr += ax;
rizqicahyo 0:edddd373a163 293 }
rizqicahyo 0:edddd373a163 294 else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){
rizqicahyo 0:edddd373a163 295 //mundur
Najib_irvani 1:fc1535231c0d 296 motor1.speed(speed-tuning1);
Najib_irvani 1:fc1535231c0d 297 motor3.speed(-(speed-tuning3));
Najib_irvani 1:fc1535231c0d 298 motor2.speed(-(speed-tuning2));
Najib_irvani 1:fc1535231c0d 299 motor4.speed(speed-tuning4);
rizqicahyo 0:edddd373a163 300 pc.printf("mundur \n");
rizqicahyo 0:edddd373a163 301
rizqicahyo 2:df6c49846367 302 vcurr += ax;
rizqicahyo 0:edddd373a163 303 }
rizqicahyo 0:edddd373a163 304 else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_LEFT)==0)){
rizqicahyo 0:edddd373a163 305 //kanan
Najib_irvani 1:fc1535231c0d 306 motor2.speed(speed-tuning2);
Najib_irvani 1:fc1535231c0d 307 motor4.speed(speed-tuning4);
Najib_irvani 1:fc1535231c0d 308 motor1.speed(speed-tuning1);
Najib_irvani 1:fc1535231c0d 309 motor3.speed(speed-tuning3);
rizqicahyo 0:edddd373a163 310 pc.printf("kanan \n");
rizqicahyo 0:edddd373a163 311
rizqicahyo 2:df6c49846367 312 vcurr += ax;
rizqicahyo 0:edddd373a163 313 }
rizqicahyo 0:edddd373a163 314 else if((ps2.read(PS_PAD::PAD_RIGHT)==0) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
rizqicahyo 0:edddd373a163 315 //kiri
Najib_irvani 1:fc1535231c0d 316 motor2.speed(-(speed-tuning2));
Najib_irvani 1:fc1535231c0d 317 motor4.speed(-(speed-tuning4));
Najib_irvani 1:fc1535231c0d 318 motor1.speed(-(speed-tuning1));
Najib_irvani 1:fc1535231c0d 319 motor3.speed(-(speed-tuning3));
rizqicahyo 0:edddd373a163 320 pc.printf("kiri \n");
rizqicahyo 0:edddd373a163 321
rizqicahyo 2:df6c49846367 322 vcurr += ax;
rizqicahyo 0:edddd373a163 323 }
rizqicahyo 0:edddd373a163 324 else{
rizqicahyo 0:edddd373a163 325 motor1.brake(1);
rizqicahyo 0:edddd373a163 326 motor3.brake(1);
rizqicahyo 0:edddd373a163 327 motor2.brake(1);
rizqicahyo 0:edddd373a163 328 motor4.brake(1);
rizqicahyo 0:edddd373a163 329 pc.printf("diam \n");
rizqicahyo 0:edddd373a163 330
rizqicahyo 2:df6c49846367 331 vcurr = v0;
rizqicahyo 0:edddd373a163 332 }
rizqicahyo 3:43d4cb3ece1b 333 }
rizqicahyo 3:43d4cb3ece1b 334 else{
rizqicahyo 3:43d4cb3ece1b 335 if((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){
rizqicahyo 3:43d4cb3ece1b 336 //pivot kiri
rizqicahyo 3:43d4cb3ece1b 337 motor2.speed((float)0.5*(speed-tuning2));
rizqicahyo 3:43d4cb3ece1b 338 motor4.speed((float)-0.5*(speed-tuning4));
rizqicahyo 3:43d4cb3ece1b 339 motor1.speed((float)0.5*(speed-tuning1));
rizqicahyo 3:43d4cb3ece1b 340 motor3.speed((float)-0.5*(speed-tuning3));
rizqicahyo 3:43d4cb3ece1b 341 pc.printf("pivot kiri \n");
rizqicahyo 3:43d4cb3ece1b 342
rizqicahyo 3:43d4cb3ece1b 343 vcurr += ax;
rizqicahyo 3:43d4cb3ece1b 344 }
rizqicahyo 3:43d4cb3ece1b 345 else if((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){
rizqicahyo 3:43d4cb3ece1b 346 //pivot kanan
rizqicahyo 3:43d4cb3ece1b 347 motor2.speed((float)-0.5*(speed-tuning2));
rizqicahyo 3:43d4cb3ece1b 348 motor4.speed((float)0.5*(speed-tuning4));
rizqicahyo 3:43d4cb3ece1b 349 motor1.speed((float)-0.5*(speed-tuning1));
rizqicahyo 3:43d4cb3ece1b 350 motor3.speed((float)0.5*(speed-tuning3));
rizqicahyo 3:43d4cb3ece1b 351 pc.printf("pivot kanan \n");
rizqicahyo 2:df6c49846367 352
rizqicahyo 3:43d4cb3ece1b 353 vcurr += ax;
rizqicahyo 3:43d4cb3ece1b 354 }
rizqicahyo 3:43d4cb3ece1b 355 else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_TOP)==1)){
rizqicahyo 3:43d4cb3ece1b 356 //serong atas kanan
rizqicahyo 3:43d4cb3ece1b 357 motor2.speed(-(speed-tuning2));
rizqicahyo 3:43d4cb3ece1b 358 motor4.brake(1);
rizqicahyo 3:43d4cb3ece1b 359 motor1.brake(1);
rizqicahyo 3:43d4cb3ece1b 360 motor3.speed(-(speed-tuning3));
rizqicahyo 3:43d4cb3ece1b 361 pc.printf("serong atas kanan \n");
rizqicahyo 2:df6c49846367 362
rizqicahyo 3:43d4cb3ece1b 363 vcurr += ax;
rizqicahyo 3:43d4cb3ece1b 364 }
rizqicahyo 3:43d4cb3ece1b 365 else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
rizqicahyo 3:43d4cb3ece1b 366 //serong atas kiri
rizqicahyo 3:43d4cb3ece1b 367 motor2.brake(1);
rizqicahyo 3:43d4cb3ece1b 368 motor4.speed(speed-tuning4);
rizqicahyo 3:43d4cb3ece1b 369 motor1.speed(speed-tuning1);
rizqicahyo 3:43d4cb3ece1b 370 motor3.brake(1);
rizqicahyo 3:43d4cb3ece1b 371 pc.printf("serong atas kiri \n");
rizqicahyo 3:43d4cb3ece1b 372
rizqicahyo 3:43d4cb3ece1b 373 vcurr += ax;
rizqicahyo 3:43d4cb3ece1b 374 }
rizqicahyo 3:43d4cb3ece1b 375 else if((ps2.read(PS_PAD::PAD_LEFT)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==1)){
rizqicahyo 3:43d4cb3ece1b 376 //serong bawah kiri
rizqicahyo 3:43d4cb3ece1b 377 motor2.speed(speed-tuning2);
rizqicahyo 3:43d4cb3ece1b 378 motor4.brake(1);
rizqicahyo 3:43d4cb3ece1b 379 motor1.brake(1);
rizqicahyo 3:43d4cb3ece1b 380 motor3.speed(speed-tuning3);
rizqicahyo 3:43d4cb3ece1b 381 pc.printf("serong bawah kiri \n");
rizqicahyo 2:df6c49846367 382
rizqicahyo 3:43d4cb3ece1b 383 vcurr += ax;
rizqicahyo 0:edddd373a163 384 }
rizqicahyo 3:43d4cb3ece1b 385 else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_RIGHT)==1)){
rizqicahyo 3:43d4cb3ece1b 386 //serong bawah kanan
rizqicahyo 3:43d4cb3ece1b 387 motor2.brake(1);
rizqicahyo 3:43d4cb3ece1b 388 motor4.speed(-(speed-tuning4));
rizqicahyo 3:43d4cb3ece1b 389 motor1.speed(-(speed-tuning1));
rizqicahyo 3:43d4cb3ece1b 390 motor3.brake(1);
rizqicahyo 3:43d4cb3ece1b 391 pc.printf("serong bawah kanan \n");
rizqicahyo 3:43d4cb3ece1b 392
rizqicahyo 3:43d4cb3ece1b 393 vcurr += ax;
rizqicahyo 3:43d4cb3ece1b 394 }
rizqicahyo 3:43d4cb3ece1b 395 else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){
rizqicahyo 3:43d4cb3ece1b 396 //maju
rizqicahyo 3:43d4cb3ece1b 397 motor1.speed(speed-tuning1);
rizqicahyo 3:43d4cb3ece1b 398 motor3.speed(-(speed-tuning3));
rizqicahyo 3:43d4cb3ece1b 399 motor2.speed(-(speed-tuning2));
rizqicahyo 3:43d4cb3ece1b 400 motor4.speed(speed-tuning4);
rizqicahyo 3:43d4cb3ece1b 401 pc.printf("maju \n");
rizqicahyo 2:df6c49846367 402
rizqicahyo 3:43d4cb3ece1b 403 vcurr += ax;
rizqicahyo 3:43d4cb3ece1b 404 }
rizqicahyo 3:43d4cb3ece1b 405 else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){
rizqicahyo 3:43d4cb3ece1b 406 //mundur
rizqicahyo 3:43d4cb3ece1b 407 motor1.speed(-(speed-tuning1));
rizqicahyo 3:43d4cb3ece1b 408 motor3.speed(speed-tuning3);
rizqicahyo 3:43d4cb3ece1b 409 motor2.speed(speed-tuning2);
rizqicahyo 3:43d4cb3ece1b 410 motor4.speed(-(speed-tuning4));
rizqicahyo 3:43d4cb3ece1b 411 pc.printf("mundur \n");
rizqicahyo 2:df6c49846367 412
rizqicahyo 3:43d4cb3ece1b 413 vcurr += ax;
rizqicahyo 3:43d4cb3ece1b 414 }
rizqicahyo 3:43d4cb3ece1b 415 else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_LEFT)==0)){
rizqicahyo 3:43d4cb3ece1b 416 //kanan
rizqicahyo 3:43d4cb3ece1b 417 motor2.speed(-(speed-tuning2));
rizqicahyo 3:43d4cb3ece1b 418 motor4.speed(-(speed-tuning4));
rizqicahyo 3:43d4cb3ece1b 419 motor1.speed(-(speed-tuning1));
rizqicahyo 3:43d4cb3ece1b 420 motor3.speed(-(speed-tuning3));
rizqicahyo 3:43d4cb3ece1b 421 pc.printf("kanan \n");
rizqicahyo 3:43d4cb3ece1b 422
rizqicahyo 3:43d4cb3ece1b 423 vcurr += ax;
rizqicahyo 3:43d4cb3ece1b 424 }
rizqicahyo 3:43d4cb3ece1b 425 else if((ps2.read(PS_PAD::PAD_RIGHT)==0) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
rizqicahyo 3:43d4cb3ece1b 426 //kiri
rizqicahyo 3:43d4cb3ece1b 427 motor2.speed(speed-tuning2);
rizqicahyo 3:43d4cb3ece1b 428 motor4.speed(speed-tuning4);
rizqicahyo 3:43d4cb3ece1b 429 motor1.speed(speed-tuning1);
rizqicahyo 3:43d4cb3ece1b 430 motor3.speed(speed-tuning3);
rizqicahyo 3:43d4cb3ece1b 431 pc.printf("kiri \n");
rizqicahyo 3:43d4cb3ece1b 432
rizqicahyo 3:43d4cb3ece1b 433 vcurr += ax;
rizqicahyo 0:edddd373a163 434 }
rizqicahyo 0:edddd373a163 435 else{
rizqicahyo 3:43d4cb3ece1b 436 motor1.brake(1);
rizqicahyo 3:43d4cb3ece1b 437 motor3.brake(1);
rizqicahyo 3:43d4cb3ece1b 438 motor2.brake(1);
rizqicahyo 3:43d4cb3ece1b 439 motor4.brake(1);
rizqicahyo 3:43d4cb3ece1b 440 pc.printf("diam \n");
rizqicahyo 0:edddd373a163 441
rizqicahyo 3:43d4cb3ece1b 442 vcurr = v0;
rizqicahyo 3:43d4cb3ece1b 443 }
rizqicahyo 3:43d4cb3ece1b 444
rizqicahyo 3:43d4cb3ece1b 445 }
rizqicahyo 3:43d4cb3ece1b 446
rizqicahyo 3:43d4cb3ece1b 447 if((ps2.read(PS_PAD::PAD_CIRCLE)==0) && (ps2.read(PS_PAD::PAD_TRIANGLE)==1)){
rizqicahyo 3:43d4cb3ece1b 448 //POWER WINDOW ATAS
rizqicahyo 3:43d4cb3ece1b 449 motorS.speed(1);
rizqicahyo 3:43d4cb3ece1b 450 if (limit1 == 0){
rizqicahyo 3:43d4cb3ece1b 451 motorS.brake(1);
rizqicahyo 3:43d4cb3ece1b 452 }
rizqicahyo 3:43d4cb3ece1b 453
rizqicahyo 3:43d4cb3ece1b 454
rizqicahyo 3:43d4cb3ece1b 455 pc.printf("up \n");
rizqicahyo 3:43d4cb3ece1b 456 c++;
rizqicahyo 3:43d4cb3ece1b 457 }
rizqicahyo 3:43d4cb3ece1b 458 else if((ps2.read(PS_PAD::PAD_CIRCLE)==1) && (ps2.read(PS_PAD::PAD_TRIANGLE)==0)){
rizqicahyo 3:43d4cb3ece1b 459 //POWER WINDOW BAWAH
rizqicahyo 3:43d4cb3ece1b 460 motorS.speed(-0.5);
rizqicahyo 3:43d4cb3ece1b 461
rizqicahyo 3:43d4cb3ece1b 462 if (limit2 ==0){
rizqicahyo 2:df6c49846367 463 motorS.brake(1);
rizqicahyo 2:df6c49846367 464 }
rizqicahyo 3:43d4cb3ece1b 465
rizqicahyo 3:43d4cb3ece1b 466 pc.printf("down \n");
rizqicahyo 3:43d4cb3ece1b 467 c--;
rizqicahyo 3:43d4cb3ece1b 468 }
rizqicahyo 3:43d4cb3ece1b 469 else{
rizqicahyo 3:43d4cb3ece1b 470 motorS.brake(1);
rizqicahyo 3:43d4cb3ece1b 471 if ((c <= batas_delay) && (c>=-batas_delay)){
rizqicahyo 3:43d4cb3ece1b 472 c=0;
rizqicahyo 2:df6c49846367 473 }
rizqicahyo 3:43d4cb3ece1b 474
rizqicahyo 3:43d4cb3ece1b 475 pc.printf("diam \n");
rizqicahyo 3:43d4cb3ece1b 476 }
rizqicahyo 3:43d4cb3ece1b 477
rizqicahyo 3:43d4cb3ece1b 478 if((c > batas_delay) && (limit1 == 0)){
rizqicahyo 3:43d4cb3ece1b 479 c = 0;
rizqicahyo 3:43d4cb3ece1b 480 motorS.brake(1);
rizqicahyo 3:43d4cb3ece1b 481 }
rizqicahyo 3:43d4cb3ece1b 482 else if((c < -batas_delay) && (limit2 == 0)){
rizqicahyo 3:43d4cb3ece1b 483 c = 0;
rizqicahyo 3:43d4cb3ece1b 484 motorS.brake(1);
rizqicahyo 3:43d4cb3ece1b 485 }
rizqicahyo 3:43d4cb3ece1b 486 else if( (c > batas_delay) && (limit1 != 0)){
rizqicahyo 3:43d4cb3ece1b 487 motorS.speed(1);
rizqicahyo 3:43d4cb3ece1b 488 }
rizqicahyo 3:43d4cb3ece1b 489 else if ((c<-batas_delay) && (limit2 != 0)){
rizqicahyo 3:43d4cb3ece1b 490 motorS.speed(-0.7);
rizqicahyo 3:43d4cb3ece1b 491 }
rizqicahyo 2:df6c49846367 492
rizqicahyo 0:edddd373a163 493
rizqicahyo 3:43d4cb3ece1b 494 if ((ps2.read(PS_PAD::PAD_SELECT)==1))
rizqicahyo 3:43d4cb3ece1b 495 {
rizqicahyo 0:edddd373a163 496 //mekanisme ambil gripper
rizqicahyo 0:edddd373a163 497 pc.printf("mekanisme gripper");
rizqicahyo 0:edddd373a163 498 if (g==1){
rizqicahyo 0:edddd373a163 499 pc.printf("ambil 1");
rizqicahyo 0:edddd373a163 500 pnuematik2 = 0;
rizqicahyo 0:edddd373a163 501 g=2;
rizqicahyo 0:edddd373a163 502 wait_ms(400);
rizqicahyo 0:edddd373a163 503 }
rizqicahyo 0:edddd373a163 504 else
rizqicahyo 0:edddd373a163 505 {
rizqicahyo 0:edddd373a163 506 pnuematik2 = 1;
rizqicahyo 0:edddd373a163 507 wait_ms(400);
rizqicahyo 0:edddd373a163 508 g=1;
rizqicahyo 0:edddd373a163 509 }
rizqicahyo 3:43d4cb3ece1b 510 }
rizqicahyo 0:edddd373a163 511 }
rizqicahyo 0:edddd373a163 512
rizqicahyo 0:edddd373a163 513 void edf_servo(){
rizqicahyo 0:edddd373a163 514 if(ps2.read(PS_PAD::PAD_X)==1){
rizqicahyo 0:edddd373a163 515 //PWM ++
rizqicahyo 2:df6c49846367 516 pwm += 0.0007;
rizqicahyo 4:7a7a8aa33fd5 517 if( pwm > 0.8) pwm = 0.8;
rizqicahyo 0:edddd373a163 518 pc.printf("gaspol \n");
rizqicahyo 0:edddd373a163 519 }
rizqicahyo 0:edddd373a163 520 else if(ps2.read(PS_PAD::PAD_SQUARE)==1){
rizqicahyo 0:edddd373a163 521 //PWM--
rizqicahyo 2:df6c49846367 522 pwm -= 0.0007;
rizqicahyo 0:edddd373a163 523
rizqicahyo 0:edddd373a163 524 if(pwm < 0) pwm = 0.0;
rizqicahyo 0:edddd373a163 525 pc.printf("rem ndeng \n");
rizqicahyo 0:edddd373a163 526 }
rizqicahyo 0:edddd373a163 527
rizqicahyo 0:edddd373a163 528 if(ps2.read(PS_PAD::PAD_R2)==1){
rizqicahyo 0:edddd373a163 529 //SERVO --
rizqicahyo 0:edddd373a163 530 sudut += 0.5;
rizqicahyo 0:edddd373a163 531
rizqicahyo 0:edddd373a163 532 if(sudut > 90) sudut = 90;
rizqicahyo 0:edddd373a163 533 pc.printf("servo max \n");
rizqicahyo 0:edddd373a163 534 }
rizqicahyo 0:edddd373a163 535 else if(ps2.read(PS_PAD::PAD_L2)==1){
rizqicahyo 0:edddd373a163 536 //SERVO ++
rizqicahyo 0:edddd373a163 537 sudut -= 0.5;
rizqicahyo 0:edddd373a163 538
rizqicahyo 0:edddd373a163 539 if(sudut < -90) sudut = -90;
rizqicahyo 0:edddd373a163 540 pc.printf("servo min \n");
rizqicahyo 0:edddd373a163 541 }
rizqicahyo 0:edddd373a163 542
rizqicahyo 0:edddd373a163 543 if(ps2.read(PS_PAD::PAD_START)==1){
rizqicahyo 0:edddd373a163 544
rizqicahyo 0:edddd373a163 545 sudut = 0;
rizqicahyo 3:43d4cb3ece1b 546 pwm = 0.22;
rizqicahyo 0:edddd373a163 547 }
rizqicahyo 3:43d4cb3ece1b 548
rizqicahyo 3:43d4cb3ece1b 549
rizqicahyo 0:edddd373a163 550 servoEDF.position((float)sudut);
rizqicahyo 0:edddd373a163 551 edf.setThrottle((float)pwm);
rizqicahyo 0:edddd373a163 552 edf.pulse();
rizqicahyo 2:df6c49846367 553 }
rizqicahyo 2:df6c49846367 554
rizqicahyo 4:7a7a8aa33fd5 555 /////////////////////////////////////////LINE FOLLOWER/////////////////////////
rizqicahyo 3:43d4cb3ece1b 556
rizqicahyo 2:df6c49846367 557 void init_sensor(){
rizqicahyo 2:df6c49846367 558 char data;
rizqicahyo 2:df6c49846367 559 if(com.readable()){
rizqicahyo 2:df6c49846367 560 data = com.getc();
rizqicahyo 2:df6c49846367 561
rizqicahyo 2:df6c49846367 562 for(int i=0; i<6; i++){
rizqicahyo 2:df6c49846367 563 datasensor[i] = (data >> i) & 1;
rizqicahyo 2:df6c49846367 564 }
rizqicahyo 2:df6c49846367 565 }
rizqicahyo 2:df6c49846367 566 }
rizqicahyo 2:df6c49846367 567
rizqicahyo 3:43d4cb3ece1b 568 void linetracer(float speed){
rizqicahyo 2:df6c49846367 569 float speed1,speed2,speed3,speed4;
rizqicahyo 3:43d4cb3ece1b 570
rizqicahyo 2:df6c49846367 571 //////////////////logic dari PV (present Value)/////////////////////////
rizqicahyo 3:43d4cb3ece1b 572 if(datasensor[2] && datasensor[3]){
rizqicahyo 3:43d4cb3ece1b 573 speed1 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 574 speed2 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 575 speed3 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 576 speed4 = speed*driver0;
rizqicahyo 2:df6c49846367 577 }
rizqicahyo 3:43d4cb3ece1b 578 else if(datasensor[2]){
rizqicahyo 3:43d4cb3ece1b 579 speed1 = speed*driver1;
rizqicahyo 3:43d4cb3ece1b 580 speed2 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 581 speed3 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 582 speed4 = speed*driver1;
rizqicahyo 2:df6c49846367 583 }
rizqicahyo 3:43d4cb3ece1b 584 else if(datasensor[3]){
rizqicahyo 3:43d4cb3ece1b 585 speed1 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 586 speed2 = speed*driver1;
rizqicahyo 3:43d4cb3ece1b 587 speed3 = speed*driver1;
rizqicahyo 3:43d4cb3ece1b 588 speed4 = speed*driver0;
rizqicahyo 2:df6c49846367 589 }
rizqicahyo 2:df6c49846367 590 else if(datasensor[1]){
rizqicahyo 3:43d4cb3ece1b 591 speed1 = speed*driver2;
rizqicahyo 3:43d4cb3ece1b 592 speed2 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 593 speed3 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 594 speed4 = speed*driver2;
rizqicahyo 2:df6c49846367 595 }
rizqicahyo 2:df6c49846367 596 else if(datasensor[4]){
rizqicahyo 3:43d4cb3ece1b 597 speed1 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 598 speed2 = speed*driver2;
rizqicahyo 3:43d4cb3ece1b 599 speed3 = speed*driver2;
rizqicahyo 3:43d4cb3ece1b 600 speed4 = speed*driver0;
rizqicahyo 2:df6c49846367 601 }
rizqicahyo 3:43d4cb3ece1b 602 else if(datasensor[0]){
rizqicahyo 3:43d4cb3ece1b 603 speed1 = speed*driver3;
rizqicahyo 3:43d4cb3ece1b 604 speed2 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 605 speed3 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 606 speed4 = speed*driver3;
rizqicahyo 2:df6c49846367 607 }
rizqicahyo 3:43d4cb3ece1b 608 else if(datasensor[5]){
rizqicahyo 3:43d4cb3ece1b 609 speed1 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 610 speed2 = speed*driver3;
rizqicahyo 3:43d4cb3ece1b 611 speed3 = speed*driver3;
rizqicahyo 3:43d4cb3ece1b 612 speed4 = speed*driver0;
rizqicahyo 2:df6c49846367 613 }
rizqicahyo 3:43d4cb3ece1b 614
rizqicahyo 2:df6c49846367 615 else
rizqicahyo 2:df6c49846367 616 {
rizqicahyo 3:43d4cb3ece1b 617 if(g_flag == 0){
rizqicahyo 3:43d4cb3ece1b 618 speed1 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 619 speed2 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 620 speed3 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 621 speed4 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 622 }
rizqicahyo 3:43d4cb3ece1b 623 else if(g_flag == 3){
rizqicahyo 3:43d4cb3ece1b 624 speed1 = speed*driver1;
rizqicahyo 3:43d4cb3ece1b 625 speed2 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 626 speed3 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 627 speed4 = speed*driver1;
rizqicahyo 3:43d4cb3ece1b 628 }
rizqicahyo 3:43d4cb3ece1b 629 else if(g_flag == 4){
rizqicahyo 3:43d4cb3ece1b 630 speed1 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 631 speed2 = speed*driver1;
rizqicahyo 3:43d4cb3ece1b 632 speed3 = speed*driver1;
rizqicahyo 3:43d4cb3ece1b 633 speed4 = speed*driver0;
rizqicahyo 2:df6c49846367 634 }
rizqicahyo 3:43d4cb3ece1b 635 else if(g_flag == 2){
rizqicahyo 3:43d4cb3ece1b 636 speed1 = speed*driver2;
rizqicahyo 3:43d4cb3ece1b 637 speed2 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 638 speed3 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 639 speed4 = speed*driver2;
rizqicahyo 3:43d4cb3ece1b 640 }
rizqicahyo 3:43d4cb3ece1b 641 else if(g_flag == 5){
rizqicahyo 3:43d4cb3ece1b 642 speed1 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 643 speed2 = speed*driver2;
rizqicahyo 3:43d4cb3ece1b 644 speed3 = speed*driver2;
rizqicahyo 3:43d4cb3ece1b 645 speed4 = speed*driver0;
rizqicahyo 3:43d4cb3ece1b 646 }
rizqicahyo 3:43d4cb3ece1b 647 else if(g_flag == 1){
rizqicahyo 3:43d4cb3ece1b 648 speed1 = -speed*driver2;
rizqicahyo 3:43d4cb3ece1b 649 speed2 = speed*driver2;
rizqicahyo 3:43d4cb3ece1b 650 speed3 = speed*driver2;
rizqicahyo 3:43d4cb3ece1b 651 speed4 = -speed*driver2;
rizqicahyo 3:43d4cb3ece1b 652 }
rizqicahyo 3:43d4cb3ece1b 653 else if(g_flag == 6){
rizqicahyo 3:43d4cb3ece1b 654 speed1 = speed*driver2;
rizqicahyo 3:43d4cb3ece1b 655 speed2 = -speed*driver2;
rizqicahyo 3:43d4cb3ece1b 656 speed3 = -speed*driver2;
rizqicahyo 3:43d4cb3ece1b 657 speed4 = speed*driver2;
rizqicahyo 2:df6c49846367 658 }
rizqicahyo 2:df6c49846367 659 }
rizqicahyo 2:df6c49846367 660
rizqicahyo 4:7a7a8aa33fd5 661 motor1.speed(-(float)(speed1-0.04));
rizqicahyo 4:7a7a8aa33fd5 662 motor2.speed(-(float)(speed2-0.04));
rizqicahyo 4:7a7a8aa33fd5 663 motor3.speed(-(float)(speed3-0.0));
rizqicahyo 4:7a7a8aa33fd5 664 motor4.speed(-(float)(speed4-0.0));
rizqicahyo 3:43d4cb3ece1b 665
rizqicahyo 3:43d4cb3ece1b 666 }
rizqicahyo 3:43d4cb3ece1b 667
rizqicahyo 3:43d4cb3ece1b 668 void flag_sensor(){
rizqicahyo 3:43d4cb3ece1b 669 if((datasensor[2] == 1) && (datasensor[3] == 1)) g_flag = 0;
rizqicahyo 3:43d4cb3ece1b 670 else if(datasensor[2] == 1) g_flag = 3;
rizqicahyo 3:43d4cb3ece1b 671 else if(datasensor[3] == 1) g_flag = 4;
rizqicahyo 3:43d4cb3ece1b 672 else if(datasensor[1] == 1) g_flag = 2;
rizqicahyo 3:43d4cb3ece1b 673 else if(datasensor[4] == 1) g_flag = 5;
rizqicahyo 3:43d4cb3ece1b 674 else if(datasensor[0] == 1) g_flag = 1;
rizqicahyo 3:43d4cb3ece1b 675 else if(datasensor[5] == 1) g_flag = 6;
rizqicahyo 4:7a7a8aa33fd5 676 }
rizqicahyo 4:7a7a8aa33fd5 677
rizqicahyo 4:7a7a8aa33fd5 678
rizqicahyo 4:7a7a8aa33fd5 679 ////////////////////////SENSOR PING///////////////////////////////////////
rizqicahyo 4:7a7a8aa33fd5 680 float read_jarak() {
rizqicahyo 4:7a7a8aa33fd5 681 float jarak;
rizqicahyo 4:7a7a8aa33fd5 682
rizqicahyo 4:7a7a8aa33fd5 683 ping.Send();
rizqicahyo 4:7a7a8aa33fd5 684 wait_ms(45);
rizqicahyo 4:7a7a8aa33fd5 685 jarak = ping.Read_cm()/2;
rizqicahyo 4:7a7a8aa33fd5 686 return jarak;
rizqicahyo 0:edddd373a163 687 }