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@3:43d4cb3ece1b, 2016-04-26 (annotated)

Committer:
rizqicahyo
Date:
Tue Apr 26 14:57:35 2016 +0000
Revision:
3:43d4cb3ece1b
Parent:
2:df6c49846367
Child:
4:7a7a8aa33fd5
update LF + tuning, tambah switch lapangan

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