KRTMI_Titanium2022 / Mbed 2 deprecated KRTMI_Sigmoid10

Robot Sulistia, Full Kodingan Lomba KRTMI 2022

Dependencies:   MPU9250 Motor PID QEI SRF05 mbed

PIDremote.cpp@0:a2b784a0e760, 20 months ago (annotated)

Committer:
photonicbabe
Date:
Tue Oct 04 10:49:03 2022 +0000
Revision:
0:a2b784a0e760
Robot Sulistia, Kodingan Lomba KRTMI

Who changed what in which revision?

UserRevisionLine numberNew contents of line
photonicbabe 0:a2b784a0e760 1 #include "mbed.h"
photonicbabe 0:a2b784a0e760 2 #include "Motor.h"
photonicbabe 0:a2b784a0e760 3 #include "QEI.h"
photonicbabe 0:a2b784a0e760 4 #include "PID.h"
photonicbabe 0:a2b784a0e760 5
photonicbabe 0:a2b784a0e760 6 #define i2c_address 0x60
photonicbabe 0:a2b784a0e760 7
photonicbabe 0:a2b784a0e760 8 Serial pc(USBTX, USBRX);
photonicbabe 0:a2b784a0e760 9 Serial bluetooth(PB_6, PA_10);
photonicbabe 0:a2b784a0e760 10 I2C CMPS(PB_4, PA_8);
photonicbabe 0:a2b784a0e760 11 //SRF05 ultra(PA_9, PC_7);
photonicbabe 0:a2b784a0e760 12 float jarak;
photonicbabe 0:a2b784a0e760 13
photonicbabe 0:a2b784a0e760 14 DigitalOut trigPin1(PA_9); // sensor tengah
photonicbabe 0:a2b784a0e760 15 DigitalIn echoPin1(PC_7);
photonicbabe 0:a2b784a0e760 16 DigitalIn ir(PB_9);
photonicbabe 0:a2b784a0e760 17 int eksistensi_koin;
photonicbabe 0:a2b784a0e760 18 int ada = 0;
photonicbabe 0:a2b784a0e760 19 int tidak_ada = 1;
photonicbabe 0:a2b784a0e760 20 int i;
photonicbabe 0:a2b784a0e760 21 int kecepatan_state;
photonicbabe 0:a2b784a0e760 22
photonicbabe 0:a2b784a0e760 23 QEI DepanKananQEI(PB_8, PC_9, NC, 986); //roda depan kanan PB_1, PB_15, NC, 624
photonicbabe 0:a2b784a0e760 24 QEI BelakangKananQEI(PB_1, PB_15, NC, 986); //roda belakang kiri PB_14, PB_13, NC, 624
photonicbabe 0:a2b784a0e760 25 QEI DepanKiriQEI(PC_12, PC_10, NC, 986); //roda depan kiri
photonicbabe 0:a2b784a0e760 26 QEI BelakangKiriQEI(PB_7, PA_15, NC, 986); //roda belakang kiri
photonicbabe 0:a2b784a0e760 27
photonicbabe 0:a2b784a0e760 28 //Tuning Di alas
photonicbabe 0:a2b784a0e760 29 //PID DepanKananPID(0.40, 0.020, 0.00001, 0.02); //0000001 Kc, Ti, Td MOTOR baru nomor 1
photonicbabe 0:a2b784a0e760 30 //PID BelakangKananPID(0.37, 0.018, 0.0000000001, 0.02); //Kc, Ti, Td MOTOR baru nomor 2
photonicbabe 0:a2b784a0e760 31 //PID BelakangKiriPID(0.496, 0.0565, 0.0000000001, 0.02); //Kc, Ti, Td MOTOR baru nomor 3
photonicbabe 0:a2b784a0e760 32 //PID DepanKiriPID(0.85, 0.08, 0.000000000001, 0.02); //Kc, Ti, Td MOTOR baru nomor 4
photonicbabe 0:a2b784a0e760 33
photonicbabe 0:a2b784a0e760 34 //Tuning Di Lantai
photonicbabe 0:a2b784a0e760 35 //PID DepanKananPID(0.5, 0.020, 0.00001, 0.02); //0000001 Kc, Ti, Td MOTOR baru nomor 1
photonicbabe 0:a2b784a0e760 36 //PID BelakangKananPID(0.5, 0.022, 0.00000000001, 0.02);
photonicbabe 0:a2b784a0e760 37 //PID BelakangKiriPID(0.496, 0.0565, 0.0000000001, 0.02); //Kc, Ti, Td MOTOR baru nomor 3
photonicbabe 0:a2b784a0e760 38 //PID DepanKiriPID(0.85, 0.08, 0.000000000001, 0.02); //Kc, Ti, Td MOTOR baru nomor 4
photonicbabe 0:a2b784a0e760 39
photonicbabe 0:a2b784a0e760 40 PID DepanKananPID(0.8, 0.1, 0.00000001, 0.02); //0000001 Kc, Ti, Td MOTOR baru nomor 1
photonicbabe 0:a2b784a0e760 41 PID BelakangKananPID(0.78, 0.1, 0.0000001, 0.02);
photonicbabe 0:a2b784a0e760 42 PID BelakangKiriPID(0.78, 0.1, 0.000000001, 0.02); //Kc, Ti, Td MOTOR baru nomor 3
photonicbabe 0:a2b784a0e760 43 PID DepanKiriPID(0.87, 0.1, 0.000001, 0.02); //Kc, Ti, Td MOTOR baru nomor 4
photonicbabe 0:a2b784a0e760 44
photonicbabe 0:a2b784a0e760 45
photonicbabe 0:a2b784a0e760 46
photonicbabe 0:a2b784a0e760 47 Motor DepanKanan(PC_6, PA_11, PB_12);
photonicbabe 0:a2b784a0e760 48 Motor DepanKiri(PA_0, PA_4, PB_0);
photonicbabe 0:a2b784a0e760 49 Motor BelakangKanan(PC_8, PC_5, PA_12);
photonicbabe 0:a2b784a0e760 50 Motor BelakangKiri(PA_1, PC_1, PC_0);
photonicbabe 0:a2b784a0e760 51
photonicbabe 0:a2b784a0e760 52 PwmOut Servo(PA_7);
photonicbabe 0:a2b784a0e760 53 PwmOut Servo2(PB_10);
photonicbabe 0:a2b784a0e760 54
photonicbabe 0:a2b784a0e760 55 Timer t_jalan;
photonicbabe 0:a2b784a0e760 56 Timer t2;
photonicbabe 0:a2b784a0e760 57 Timer t;
photonicbabe 0:a2b784a0e760 58 Timer t_capit;
photonicbabe 0:a2b784a0e760 59
photonicbabe 0:a2b784a0e760 60 char nilai, huruf;
photonicbabe 0:a2b784a0e760 61 int Jalan;
photonicbabe 0:a2b784a0e760 62 int Jalan_Maju =1;
photonicbabe 0:a2b784a0e760 63 int Berhenti = 0;
photonicbabe 0:a2b784a0e760 64 int Jalan_Mundur = 2;
photonicbabe 0:a2b784a0e760 65 int Jalan_Kanan = 3;
photonicbabe 0:a2b784a0e760 66 int Jalan_Kiri = 4;
photonicbabe 0:a2b784a0e760 67 int ke_setpoint = 5;
photonicbabe 0:a2b784a0e760 68 int mode_inverse = 6;
photonicbabe 0:a2b784a0e760 69 int Rotasi_Kanan = 7;
photonicbabe 0:a2b784a0e760 70 int Rotasi_Kiri = 8;
photonicbabe 0:a2b784a0e760 71 int Belok_Kanan = 9;
photonicbabe 0:a2b784a0e760 72 int Belok_Kiri = 10;
photonicbabe 0:a2b784a0e760 73 int Muter_Kiri = 11;
photonicbabe 0:a2b784a0e760 74 int Muter_Kanan =12;
photonicbabe 0:a2b784a0e760 75 int Capit_Buka = 13;
photonicbabe 0:a2b784a0e760 76 int Capit_Muter = 14;
photonicbabe 0:a2b784a0e760 77 int Cari_Koin1 = 15;
photonicbabe 0:a2b784a0e760 78 int Cari_Koin2 = 16;
photonicbabe 0:a2b784a0e760 79 int Mundur_Belok_Kanan = 17;
photonicbabe 0:a2b784a0e760 80 int Mundur_Belok_Kiri = 18;
photonicbabe 0:a2b784a0e760 81 int Ganti_Kecepatan = 19;
photonicbabe 0:a2b784a0e760 82
photonicbabe 0:a2b784a0e760 83 int DepanKananPulses = 0;
photonicbabe 0:a2b784a0e760 84 int DepanKananPrevPulses = 0;
photonicbabe 0:a2b784a0e760 85 float DepanKananVelocity = 0.0;
photonicbabe 0:a2b784a0e760 86
photonicbabe 0:a2b784a0e760 87 int BelakangKananPulses = 0;
photonicbabe 0:a2b784a0e760 88 int BelakangKananPrevPulses = 0;
photonicbabe 0:a2b784a0e760 89 float BelakangKananVelocity = 0.0;
photonicbabe 0:a2b784a0e760 90
photonicbabe 0:a2b784a0e760 91 int BelakangKiriPulses = 0;
photonicbabe 0:a2b784a0e760 92 int BelakangKiriPrevPulses = 0;
photonicbabe 0:a2b784a0e760 93 float BelakangKiriVelocity = 0.0;
photonicbabe 0:a2b784a0e760 94
photonicbabe 0:a2b784a0e760 95 int DepanKiriPulses = 0;
photonicbabe 0:a2b784a0e760 96 int DepanKiriPrevPulses = 0;
photonicbabe 0:a2b784a0e760 97 float DepanKiriVelocity = 0.0;
photonicbabe 0:a2b784a0e760 98
photonicbabe 0:a2b784a0e760 99 int Depan_Kanan_Count;
photonicbabe 0:a2b784a0e760 100 int Depan_Kiri_Count;
photonicbabe 0:a2b784a0e760 101 int Belakang_Kanan_Count;
photonicbabe 0:a2b784a0e760 102 int Belakang_Kiri_Count;
photonicbabe 0:a2b784a0e760 103
photonicbabe 0:a2b784a0e760 104 int d_pulse_DepanKanan;
photonicbabe 0:a2b784a0e760 105 int d_pulse_DepanKiri;
photonicbabe 0:a2b784a0e760 106 int d_pulse_BelakangKanan;
photonicbabe 0:a2b784a0e760 107 int d_pulse_BelakangKiri;
photonicbabe 0:a2b784a0e760 108
photonicbabe 0:a2b784a0e760 109 int koordinat_x_DepanKiri;
photonicbabe 0:a2b784a0e760 110 int koordinat_x_DepanKanan;
photonicbabe 0:a2b784a0e760 111 int koordinat_x_BelakangKiri;
photonicbabe 0:a2b784a0e760 112 int koordinat_x_BelakangKanan;
photonicbabe 0:a2b784a0e760 113 int koordinat_y_DepanKiri;
photonicbabe 0:a2b784a0e760 114 int koordinat_y_DepanKanan;
photonicbabe 0:a2b784a0e760 115 int koordinat_y_BelakangKiri;
photonicbabe 0:a2b784a0e760 116 int koordinat_y_BelakangKanan;
photonicbabe 0:a2b784a0e760 117 float koordinat_x;
photonicbabe 0:a2b784a0e760 118 float koordinat_y;
photonicbabe 0:a2b784a0e760 119 int Resolusi = 986;
photonicbabe 0:a2b784a0e760 120 float pi = 3.141;
photonicbabe 0:a2b784a0e760 121 float diameter = 5.8f;
photonicbabe 0:a2b784a0e760 122 float rtd = pi/180.0f;
photonicbabe 0:a2b784a0e760 123 int setpoint_x;
photonicbabe 0:a2b784a0e760 124 int setpoint_y;
photonicbabe 0:a2b784a0e760 125 float radius = 0.029;
photonicbabe 0:a2b784a0e760 126 float orientasi;
photonicbabe 0:a2b784a0e760 127 int orientasi_2;
photonicbabe 0:a2b784a0e760 128 int target_orientasi;
photonicbabe 0:a2b784a0e760 129 float waktu;
photonicbabe 0:a2b784a0e760 130 int BEARING_Register;
photonicbabe 0:a2b784a0e760 131 char bits[2];
photonicbabe 0:a2b784a0e760 132 int _byteHigh;
photonicbabe 0:a2b784a0e760 133 int _byteLow;
photonicbabe 0:a2b784a0e760 134 float bearing;
photonicbabe 0:a2b784a0e760 135
photonicbabe 0:a2b784a0e760 136 float target_depan_kiri;
photonicbabe 0:a2b784a0e760 137 float target_depan_kanan;
photonicbabe 0:a2b784a0e760 138 float target_belakang_kiri;
photonicbabe 0:a2b784a0e760 139 float target_belakang_kanan;
photonicbabe 0:a2b784a0e760 140 float arah_depan_kiri;
photonicbabe 0:a2b784a0e760 141 float arah_depan_kanan;
photonicbabe 0:a2b784a0e760 142 float arah_belakang_kiri;
photonicbabe 0:a2b784a0e760 143 float arah_belakang_kanan;
photonicbabe 0:a2b784a0e760 144
photonicbabe 0:a2b784a0e760 145 float temp_x_DepanKiri;
photonicbabe 0:a2b784a0e760 146 float temp_x_DepanKanan;
photonicbabe 0:a2b784a0e760 147 float temp_x_BelakangKanan;
photonicbabe 0:a2b784a0e760 148 float temp_x_BelakangKiri;
photonicbabe 0:a2b784a0e760 149 float temp_y_DepanKiri;
photonicbabe 0:a2b784a0e760 150 float temp_y_DepanKanan;
photonicbabe 0:a2b784a0e760 151 float temp_y_BelakangKanan;
photonicbabe 0:a2b784a0e760 152 float temp_y_BelakangKiri;
photonicbabe 0:a2b784a0e760 153 float temp_x;
photonicbabe 0:a2b784a0e760 154 float temp_y;
photonicbabe 0:a2b784a0e760 155 float terhadap_x;
photonicbabe 0:a2b784a0e760 156 float terhadap_y;
photonicbabe 0:a2b784a0e760 157 int koordinat_x_bulat;
photonicbabe 0:a2b784a0e760 158 int koordinat_y_bulat;
photonicbabe 0:a2b784a0e760 159 float target_x_basis;
photonicbabe 0:a2b784a0e760 160 float target_y_basis;
photonicbabe 0:a2b784a0e760 161 float sudut_awal;
photonicbabe 0:a2b784a0e760 162 int capit_state;
photonicbabe 0:a2b784a0e760 163 int capit2_state;
photonicbabe 0:a2b784a0e760 164
photonicbabe 0:a2b784a0e760 165 void fungsi_bluetooth(void)
photonicbabe 0:a2b784a0e760 166 {
photonicbabe 0:a2b784a0e760 167 if(bluetooth.readable())
photonicbabe 0:a2b784a0e760 168 {
photonicbabe 0:a2b784a0e760 169 setpoint_x = bluetooth.getc();
photonicbabe 0:a2b784a0e760 170 setpoint_y = bluetooth.getc();
photonicbabe 0:a2b784a0e760 171 int target_sudut = bluetooth.getc();
photonicbabe 0:a2b784a0e760 172 nilai = bluetooth.getc();
photonicbabe 0:a2b784a0e760 173 if (nilai == 'S')
photonicbabe 0:a2b784a0e760 174 {
photonicbabe 0:a2b784a0e760 175 huruf = 'S';
photonicbabe 0:a2b784a0e760 176 Jalan = Berhenti;
photonicbabe 0:a2b784a0e760 177 }
photonicbabe 0:a2b784a0e760 178 else if (nilai == 'F')
photonicbabe 0:a2b784a0e760 179 {
photonicbabe 0:a2b784a0e760 180 huruf = 'F';
photonicbabe 0:a2b784a0e760 181 Jalan = Jalan_Maju;
photonicbabe 0:a2b784a0e760 182 }
photonicbabe 0:a2b784a0e760 183 else if(nilai =='B')
photonicbabe 0:a2b784a0e760 184 {
photonicbabe 0:a2b784a0e760 185 huruf = 'B';
photonicbabe 0:a2b784a0e760 186 Jalan = Jalan_Mundur;
photonicbabe 0:a2b784a0e760 187 }
photonicbabe 0:a2b784a0e760 188 else if(nilai =='L')
photonicbabe 0:a2b784a0e760 189 {
photonicbabe 0:a2b784a0e760 190 huruf = 'L';
photonicbabe 0:a2b784a0e760 191 Jalan = Jalan_Kiri;
photonicbabe 0:a2b784a0e760 192 }
photonicbabe 0:a2b784a0e760 193 else if(nilai =='K')
photonicbabe 0:a2b784a0e760 194 {
photonicbabe 0:a2b784a0e760 195 huruf = 'K';
photonicbabe 0:a2b784a0e760 196 Jalan = Belok_Kiri;
photonicbabe 0:a2b784a0e760 197 }
photonicbabe 0:a2b784a0e760 198 else if(nilai =='A')
photonicbabe 0:a2b784a0e760 199 {
photonicbabe 0:a2b784a0e760 200 huruf = 'A';
photonicbabe 0:a2b784a0e760 201 Jalan = Belok_Kanan;
photonicbabe 0:a2b784a0e760 202 }
photonicbabe 0:a2b784a0e760 203 else if(nilai =='R')
photonicbabe 0:a2b784a0e760 204 {
photonicbabe 0:a2b784a0e760 205 huruf = 'R';
photonicbabe 0:a2b784a0e760 206 Jalan = Jalan_Kanan;
photonicbabe 0:a2b784a0e760 207 }
photonicbabe 0:a2b784a0e760 208 else if(nilai =='Q')
photonicbabe 0:a2b784a0e760 209 {
photonicbabe 0:a2b784a0e760 210 huruf = 'Q';
photonicbabe 0:a2b784a0e760 211 Jalan = Muter_Kiri;
photonicbabe 0:a2b784a0e760 212 }
photonicbabe 0:a2b784a0e760 213 else if(nilai =='E')
photonicbabe 0:a2b784a0e760 214 {
photonicbabe 0:a2b784a0e760 215 huruf = 'E';
photonicbabe 0:a2b784a0e760 216 Jalan = Muter_Kanan;
photonicbabe 0:a2b784a0e760 217 }
photonicbabe 0:a2b784a0e760 218 else if(nilai =='M')
photonicbabe 0:a2b784a0e760 219 {
photonicbabe 0:a2b784a0e760 220 huruf = 'M';
photonicbabe 0:a2b784a0e760 221 Jalan = Rotasi_Kanan;
photonicbabe 0:a2b784a0e760 222 }
photonicbabe 0:a2b784a0e760 223 else if(nilai =='W')
photonicbabe 0:a2b784a0e760 224 {
photonicbabe 0:a2b784a0e760 225 huruf = 'W';
photonicbabe 0:a2b784a0e760 226 Jalan = Rotasi_Kiri;
photonicbabe 0:a2b784a0e760 227 }
photonicbabe 0:a2b784a0e760 228 else if(nilai == 'U')
photonicbabe 0:a2b784a0e760 229 {
photonicbabe 0:a2b784a0e760 230 huruf = 'U';
photonicbabe 0:a2b784a0e760 231 Jalan = Mundur_Belok_Kiri;
photonicbabe 0:a2b784a0e760 232 }
photonicbabe 0:a2b784a0e760 233 else if(nilai == 'V')
photonicbabe 0:a2b784a0e760 234 {
photonicbabe 0:a2b784a0e760 235 huruf = 'V';
photonicbabe 0:a2b784a0e760 236 Jalan = Mundur_Belok_Kanan;
photonicbabe 0:a2b784a0e760 237 }
photonicbabe 0:a2b784a0e760 238 else if(nilai =='T')
photonicbabe 0:a2b784a0e760 239 {
photonicbabe 0:a2b784a0e760 240 huruf = 'T';
photonicbabe 0:a2b784a0e760 241 Jalan = Capit_Buka;
photonicbabe 0:a2b784a0e760 242 }
photonicbabe 0:a2b784a0e760 243 else if(nilai =='O')
photonicbabe 0:a2b784a0e760 244 {
photonicbabe 0:a2b784a0e760 245 huruf = 'O';
photonicbabe 0:a2b784a0e760 246 Jalan = Capit_Muter;
photonicbabe 0:a2b784a0e760 247 }
photonicbabe 0:a2b784a0e760 248 else if(nilai =='J')
photonicbabe 0:a2b784a0e760 249 {
photonicbabe 0:a2b784a0e760 250 huruf = 'J';
photonicbabe 0:a2b784a0e760 251 Jalan = Cari_Koin1;
photonicbabe 0:a2b784a0e760 252 //Jalan = ke_setpoint;
photonicbabe 0:a2b784a0e760 253 }
photonicbabe 0:a2b784a0e760 254 else if(nilai =='H')
photonicbabe 0:a2b784a0e760 255 {
photonicbabe 0:a2b784a0e760 256 huruf = 'H';
photonicbabe 0:a2b784a0e760 257 Jalan = Cari_Koin2;
photonicbabe 0:a2b784a0e760 258 //Jalan = ke_setpoint;
photonicbabe 0:a2b784a0e760 259 }
photonicbabe 0:a2b784a0e760 260
photonicbabe 0:a2b784a0e760 261 else if(nilai =='Y')
photonicbabe 0:a2b784a0e760 262 {
photonicbabe 0:a2b784a0e760 263 huruf = 'G';
photonicbabe 0:a2b784a0e760 264 Jalan = ke_setpoint;
photonicbabe 0:a2b784a0e760 265 }
photonicbabe 0:a2b784a0e760 266 else if(nilai == 'G')
photonicbabe 0:a2b784a0e760 267 {
photonicbabe 0:a2b784a0e760 268 huruf = 'G';
photonicbabe 0:a2b784a0e760 269 Jalan = mode_inverse;
photonicbabe 0:a2b784a0e760 270 }
photonicbabe 0:a2b784a0e760 271 else if(nilai == 'X')
photonicbabe 0:a2b784a0e760 272 {
photonicbabe 0:a2b784a0e760 273 huruf = 'X';
photonicbabe 0:a2b784a0e760 274 Jalan = Ganti_Kecepatan;
photonicbabe 0:a2b784a0e760 275 }
photonicbabe 0:a2b784a0e760 276 else if(nilai == 'S')
photonicbabe 0:a2b784a0e760 277 {
photonicbabe 0:a2b784a0e760 278 Jalan = Berhenti;
photonicbabe 0:a2b784a0e760 279 }
photonicbabe 0:a2b784a0e760 280 else if(nilai == 'p')
photonicbabe 0:a2b784a0e760 281 {
photonicbabe 0:a2b784a0e760 282 NVIC_SystemReset();
photonicbabe 0:a2b784a0e760 283 }
photonicbabe 0:a2b784a0e760 284 }
photonicbabe 0:a2b784a0e760 285 }
photonicbabe 0:a2b784a0e760 286
photonicbabe 0:a2b784a0e760 287 void kosongkan()
photonicbabe 0:a2b784a0e760 288 {
photonicbabe 0:a2b784a0e760 289 BelakangKananQEI.reset();
photonicbabe 0:a2b784a0e760 290 DepanKananQEI.reset();
photonicbabe 0:a2b784a0e760 291 DepanKiriQEI.reset();
photonicbabe 0:a2b784a0e760 292 BelakangKiriQEI.reset();
photonicbabe 0:a2b784a0e760 293
photonicbabe 0:a2b784a0e760 294 DepanKananPulses = 0;
photonicbabe 0:a2b784a0e760 295 DepanKananPrevPulses = 0;
photonicbabe 0:a2b784a0e760 296 DepanKananVelocity = 0.0;
photonicbabe 0:a2b784a0e760 297
photonicbabe 0:a2b784a0e760 298 BelakangKananPulses = 0;
photonicbabe 0:a2b784a0e760 299 BelakangKananPrevPulses = 0;
photonicbabe 0:a2b784a0e760 300 BelakangKananVelocity = 0.0;
photonicbabe 0:a2b784a0e760 301
photonicbabe 0:a2b784a0e760 302 BelakangKiriPulses = 0;
photonicbabe 0:a2b784a0e760 303 BelakangKiriPrevPulses = 0;
photonicbabe 0:a2b784a0e760 304 BelakangKiriVelocity = 0.0;
photonicbabe 0:a2b784a0e760 305
photonicbabe 0:a2b784a0e760 306 DepanKiriPulses = 0;
photonicbabe 0:a2b784a0e760 307 DepanKiriPrevPulses = 0;
photonicbabe 0:a2b784a0e760 308 DepanKiriVelocity = 0.0;
photonicbabe 0:a2b784a0e760 309 }
photonicbabe 0:a2b784a0e760 310
photonicbabe 0:a2b784a0e760 311 void proses_kecepatan()
photonicbabe 0:a2b784a0e760 312 {
photonicbabe 0:a2b784a0e760 313 DepanKananPulses = DepanKananQEI.getPulses();
photonicbabe 0:a2b784a0e760 314 d_pulse_DepanKanan = DepanKananPulses - DepanKananPrevPulses;
photonicbabe 0:a2b784a0e760 315 DepanKananVelocity = (DepanKananPulses - DepanKananPrevPulses) / 0.02;
photonicbabe 0:a2b784a0e760 316 DepanKananPrevPulses = DepanKananPulses;
photonicbabe 0:a2b784a0e760 317 DepanKananPID.setProcessValue(fabs(DepanKananVelocity));
photonicbabe 0:a2b784a0e760 318
photonicbabe 0:a2b784a0e760 319 DepanKiriPulses = DepanKiriQEI.getPulses();
photonicbabe 0:a2b784a0e760 320 d_pulse_DepanKiri = DepanKiriPulses - DepanKiriPrevPulses;
photonicbabe 0:a2b784a0e760 321 DepanKiriVelocity = (DepanKiriPulses - DepanKiriPrevPulses) / 0.02;
photonicbabe 0:a2b784a0e760 322 DepanKiriPrevPulses = DepanKiriPulses;
photonicbabe 0:a2b784a0e760 323 DepanKiriPID.setProcessValue(fabs(DepanKiriVelocity));
photonicbabe 0:a2b784a0e760 324
photonicbabe 0:a2b784a0e760 325 BelakangKananPulses = BelakangKananQEI.getPulses();
photonicbabe 0:a2b784a0e760 326 d_pulse_BelakangKanan = BelakangKananPulses - BelakangKananPrevPulses;
photonicbabe 0:a2b784a0e760 327 BelakangKananVelocity = (BelakangKananPulses - BelakangKananPrevPulses) / 0.02;
photonicbabe 0:a2b784a0e760 328 BelakangKananPrevPulses = BelakangKananPulses;
photonicbabe 0:a2b784a0e760 329 BelakangKananPID.setProcessValue(fabs(BelakangKananVelocity));
photonicbabe 0:a2b784a0e760 330
photonicbabe 0:a2b784a0e760 331 BelakangKiriPulses = BelakangKiriQEI.getPulses();
photonicbabe 0:a2b784a0e760 332 d_pulse_BelakangKiri = BelakangKiriPulses - BelakangKiriPrevPulses;
photonicbabe 0:a2b784a0e760 333 BelakangKiriVelocity = (BelakangKiriPulses - BelakangKiriPrevPulses) / 0.02;
photonicbabe 0:a2b784a0e760 334 BelakangKiriPrevPulses = BelakangKiriPulses;
photonicbabe 0:a2b784a0e760 335 BelakangKiriPID.setProcessValue(fabs(BelakangKiriVelocity));
photonicbabe 0:a2b784a0e760 336 }
photonicbabe 0:a2b784a0e760 337
photonicbabe 0:a2b784a0e760 338 float Get_Yaw()
photonicbabe 0:a2b784a0e760 339 {
photonicbabe 0:a2b784a0e760 340 CMPS.unlock();
photonicbabe 0:a2b784a0e760 341 CMPS.start();
photonicbabe 0:a2b784a0e760 342 // to indicate an i2c read, shift the 7 bit address up 1 bit and set bit 0 to a 1
photonicbabe 0:a2b784a0e760 343 CMPS.write(i2c_address << 1);
photonicbabe 0:a2b784a0e760 344 int writeResult = CMPS.write(BEARING_Register);
photonicbabe 0:a2b784a0e760 345 if(writeResult != 1)
photonicbabe 0:a2b784a0e760 346 {
photonicbabe 0:a2b784a0e760 347 pc.printf("%d\n", writeResult);
photonicbabe 0:a2b784a0e760 348 return bearing;
photonicbabe 0:a2b784a0e760 349 }
photonicbabe 0:a2b784a0e760 350 else
photonicbabe 0:a2b784a0e760 351 {
photonicbabe 0:a2b784a0e760 352 CMPS.stop();
photonicbabe 0:a2b784a0e760 353 CMPS.read(i2c_address <<1, bits, 2);
photonicbabe 0:a2b784a0e760 354 _byteHigh = bits[0];
photonicbabe 0:a2b784a0e760 355 _byteLow = bits[1];
photonicbabe 0:a2b784a0e760 356 bearing = ((_byteHigh<<=8) + _byteLow) / 10;
photonicbabe 0:a2b784a0e760 357 return bearing;
photonicbabe 0:a2b784a0e760 358 }
photonicbabe 0:a2b784a0e760 359 }
photonicbabe 0:a2b784a0e760 360
photonicbabe 0:a2b784a0e760 361 void Get_Count()
photonicbabe 0:a2b784a0e760 362 {
photonicbabe 0:a2b784a0e760 363 Depan_Kanan_Count = Depan_Kanan_Count + d_pulse_DepanKanan;
photonicbabe 0:a2b784a0e760 364 Depan_Kiri_Count = Depan_Kiri_Count + d_pulse_DepanKiri;
photonicbabe 0:a2b784a0e760 365 Belakang_Kanan_Count = Belakang_Kanan_Count + d_pulse_BelakangKanan;
photonicbabe 0:a2b784a0e760 366 Belakang_Kiri_Count = Belakang_Kiri_Count + d_pulse_BelakangKiri;
photonicbabe 0:a2b784a0e760 367
photonicbabe 0:a2b784a0e760 368 temp_y_DepanKiri = ((double)d_pulse_DepanKiri/(double)Resolusi)*pi*diameter*cos(45.0f*rtd);
photonicbabe 0:a2b784a0e760 369 temp_y_DepanKanan = ((double)d_pulse_DepanKanan/(double)Resolusi)*pi*diameter*cos(135.0f*rtd);
photonicbabe 0:a2b784a0e760 370 temp_y_BelakangKanan = ((double)d_pulse_BelakangKanan/(double)Resolusi)*pi*diameter*cos(225.0f*rtd);
photonicbabe 0:a2b784a0e760 371 temp_y_BelakangKiri = ((double)d_pulse_BelakangKiri/(double)Resolusi)*pi*diameter*cos(315.0f*rtd);
photonicbabe 0:a2b784a0e760 372
photonicbabe 0:a2b784a0e760 373 temp_x_DepanKiri = ((double)d_pulse_DepanKiri/(double)Resolusi)*pi*diameter*sin(45.0f*rtd);
photonicbabe 0:a2b784a0e760 374 temp_x_DepanKanan = ((double)d_pulse_DepanKanan/(double)Resolusi)*pi*diameter*sin(135.0f*rtd);
photonicbabe 0:a2b784a0e760 375 temp_x_BelakangKanan = ((double)d_pulse_BelakangKanan/(double)Resolusi)*pi*diameter*sin(225.0f*rtd);
photonicbabe 0:a2b784a0e760 376 temp_x_BelakangKiri = ((double)d_pulse_BelakangKiri/(double)Resolusi)*pi*diameter*sin(315.0f*rtd);
photonicbabe 0:a2b784a0e760 377
photonicbabe 0:a2b784a0e760 378 temp_x = (temp_x_DepanKiri + temp_x_DepanKanan + temp_x_BelakangKiri + temp_x_BelakangKanan)/2;
photonicbabe 0:a2b784a0e760 379 temp_y = (temp_y_DepanKiri + temp_y_DepanKanan + temp_y_BelakangKiri + temp_y_BelakangKanan)/2;
photonicbabe 0:a2b784a0e760 380
photonicbabe 0:a2b784a0e760 381 terhadap_x = -1*(double)temp_y*sin(orientasi*rtd) + (double)temp_x*cos(orientasi*rtd);
photonicbabe 0:a2b784a0e760 382 terhadap_y = (double)temp_y*cos(orientasi*rtd) + (double)temp_x*sin(orientasi*rtd);
photonicbabe 0:a2b784a0e760 383 }
photonicbabe 0:a2b784a0e760 384
photonicbabe 0:a2b784a0e760 385 void Cari_Koordinat()
photonicbabe 0:a2b784a0e760 386 {
photonicbabe 0:a2b784a0e760 387 orientasi = -1.0f * ((double)Get_Yaw()- sudut_awal);
photonicbabe 0:a2b784a0e760 388 orientasi_2 = orientasi;
photonicbabe 0:a2b784a0e760 389
photonicbabe 0:a2b784a0e760 390 koordinat_y = koordinat_y + terhadap_y;
photonicbabe 0:a2b784a0e760 391 koordinat_x = koordinat_x + terhadap_x;
photonicbabe 0:a2b784a0e760 392 koordinat_x_bulat = koordinat_x;
photonicbabe 0:a2b784a0e760 393 koordinat_y_bulat = koordinat_y;
photonicbabe 0:a2b784a0e760 394 }
photonicbabe 0:a2b784a0e760 395
photonicbabe 0:a2b784a0e760 396 float cari_abs(float a)
photonicbabe 0:a2b784a0e760 397 {
photonicbabe 0:a2b784a0e760 398 if(a<0)
photonicbabe 0:a2b784a0e760 399 {
photonicbabe 0:a2b784a0e760 400 if(fabs(a)<20)
photonicbabe 0:a2b784a0e760 401 {
photonicbabe 0:a2b784a0e760 402 return 0.0f;
photonicbabe 0:a2b784a0e760 403 }
photonicbabe 0:a2b784a0e760 404 else if(fabs(a)>=20)
photonicbabe 0:a2b784a0e760 405 {
photonicbabe 0:a2b784a0e760 406 return -1.0f;
photonicbabe 0:a2b784a0e760 407 }
photonicbabe 0:a2b784a0e760 408 }
photonicbabe 0:a2b784a0e760 409 else if(a>0)
photonicbabe 0:a2b784a0e760 410 {
photonicbabe 0:a2b784a0e760 411 if(fabs(a)<20)
photonicbabe 0:a2b784a0e760 412 {
photonicbabe 0:a2b784a0e760 413 return 0.0f;
photonicbabe 0:a2b784a0e760 414 }
photonicbabe 0:a2b784a0e760 415 else if(fabs(a)>=20)
photonicbabe 0:a2b784a0e760 416 {
photonicbabe 0:a2b784a0e760 417 return 1.0f;
photonicbabe 0:a2b784a0e760 418 }
photonicbabe 0:a2b784a0e760 419 }
photonicbabe 0:a2b784a0e760 420 else
photonicbabe 0:a2b784a0e760 421 {
photonicbabe 0:a2b784a0e760 422 return 0.0f;
photonicbabe 0:a2b784a0e760 423 }
photonicbabe 0:a2b784a0e760 424 }
photonicbabe 0:a2b784a0e760 425
photonicbabe 0:a2b784a0e760 426 void cari_inverse(float setpoint_sudut, float target_x, float target_y)
photonicbabe 0:a2b784a0e760 427 {
photonicbabe 0:a2b784a0e760 428 //mengganti basis vektor berdasarkan sudut(rotasi axis)
photonicbabe 0:a2b784a0e760 429 setpoint_sudut = setpoint_sudut*rtd;
photonicbabe 0:a2b784a0e760 430 target_x = target_x - koordinat_x;
photonicbabe 0:a2b784a0e760 431 target_y = target_y - koordinat_y;
photonicbabe 0:a2b784a0e760 432 target_x_basis = target_x*cos(setpoint_sudut*-1.0f) - target_y*sin(setpoint_sudut*-1.0f);
photonicbabe 0:a2b784a0e760 433 target_y_basis = target_x*sin(setpoint_sudut*-1.0f) + target_y*cos(setpoint_sudut*-1.0f);
photonicbabe 0:a2b784a0e760 434 target_x = target_x_basis;
photonicbabe 0:a2b784a0e760 435 target_y = target_y_basis;
photonicbabe 0:a2b784a0e760 436 //meengganti satuan target
photonicbabe 0:a2b784a0e760 437 float target_vx;
photonicbabe 0:a2b784a0e760 438 float target_vy;
photonicbabe 0:a2b784a0e760 439 target_x = (double)target_x/100.0f;
photonicbabe 0:a2b784a0e760 440 target_y = (double)target_y/100.0f;
photonicbabe 0:a2b784a0e760 441 //mencari target kecepatan translasi dari robot
photonicbabe 0:a2b784a0e760 442 //maks kecepatan translasi per aksis 0.6 m/s
photonicbabe 0:a2b784a0e760 443 if(fabs(target_x)>fabs(target_y))
photonicbabe 0:a2b784a0e760 444 {
photonicbabe 0:a2b784a0e760 445 if((target_x<0)&&(target_y<0))
photonicbabe 0:a2b784a0e760 446 {
photonicbabe 0:a2b784a0e760 447 target_vx = -0.6;
photonicbabe 0:a2b784a0e760 448 target_vy = ((double)target_y/(double)target_x)*target_vx;
photonicbabe 0:a2b784a0e760 449 }
photonicbabe 0:a2b784a0e760 450 else if((target_x<0)&&(target_y>0))
photonicbabe 0:a2b784a0e760 451 {
photonicbabe 0:a2b784a0e760 452 target_vx = -0.6;
photonicbabe 0:a2b784a0e760 453 target_vy = ((double)target_y/(double)target_x)*target_vx;
photonicbabe 0:a2b784a0e760 454 }
photonicbabe 0:a2b784a0e760 455 else if((target_x>0)&&(target_y>0))
photonicbabe 0:a2b784a0e760 456 {
photonicbabe 0:a2b784a0e760 457 target_vx = 0.6;
photonicbabe 0:a2b784a0e760 458 target_vy = ((double)target_y/(double)target_x)*target_vx;
photonicbabe 0:a2b784a0e760 459 }
photonicbabe 0:a2b784a0e760 460 else if((target_x>0)&&(target_y<0))
photonicbabe 0:a2b784a0e760 461 {
photonicbabe 0:a2b784a0e760 462 target_vx = 0.6;
photonicbabe 0:a2b784a0e760 463 target_vy = ((double)target_y/(double)target_x)*target_vx;
photonicbabe 0:a2b784a0e760 464 }
photonicbabe 0:a2b784a0e760 465 else if((target_y==0)&&(target_x>0))
photonicbabe 0:a2b784a0e760 466 {
photonicbabe 0:a2b784a0e760 467 target_vx = 0.6;
photonicbabe 0:a2b784a0e760 468 target_vy = 0;
photonicbabe 0:a2b784a0e760 469 }
photonicbabe 0:a2b784a0e760 470 else if((target_y==0)&&(target_x<0))
photonicbabe 0:a2b784a0e760 471 {
photonicbabe 0:a2b784a0e760 472 target_vx = -0.6;
photonicbabe 0:a2b784a0e760 473 target_vy = 0;
photonicbabe 0:a2b784a0e760 474 }
photonicbabe 0:a2b784a0e760 475 }
photonicbabe 0:a2b784a0e760 476 else if(fabs(target_x)<fabs(target_y))
photonicbabe 0:a2b784a0e760 477 {
photonicbabe 0:a2b784a0e760 478 if((target_x<0)&&(target_y<0))
photonicbabe 0:a2b784a0e760 479 {
photonicbabe 0:a2b784a0e760 480 target_vy = -0.6;
photonicbabe 0:a2b784a0e760 481 target_vx = ((double)target_x/(double)target_y)*target_vy;
photonicbabe 0:a2b784a0e760 482 }
photonicbabe 0:a2b784a0e760 483 else if((target_x<0)&&(target_y>0))
photonicbabe 0:a2b784a0e760 484 {
photonicbabe 0:a2b784a0e760 485 target_vy = 0.6;
photonicbabe 0:a2b784a0e760 486 target_vx = ((double)target_x/(double)target_y)*target_vy;
photonicbabe 0:a2b784a0e760 487 }
photonicbabe 0:a2b784a0e760 488 else if((target_x>0)&&(target_y>0))
photonicbabe 0:a2b784a0e760 489 {
photonicbabe 0:a2b784a0e760 490 target_vy = 0.6;
photonicbabe 0:a2b784a0e760 491 target_vx = ((double)target_x/(double)target_y)*target_vy;
photonicbabe 0:a2b784a0e760 492 }
photonicbabe 0:a2b784a0e760 493 else if((target_x>0)&&(target_y<0))
photonicbabe 0:a2b784a0e760 494 {
photonicbabe 0:a2b784a0e760 495 target_vy = -0.6;
photonicbabe 0:a2b784a0e760 496 target_vx = ((double)target_x/(double)target_y)*target_vy;
photonicbabe 0:a2b784a0e760 497 }
photonicbabe 0:a2b784a0e760 498 else if((target_x==0)&&(target_y>0))
photonicbabe 0:a2b784a0e760 499 {
photonicbabe 0:a2b784a0e760 500 target_vy = 0.6;
photonicbabe 0:a2b784a0e760 501 target_vx = 0;
photonicbabe 0:a2b784a0e760 502 }
photonicbabe 0:a2b784a0e760 503 else if((target_x==0)&&(target_y<0))
photonicbabe 0:a2b784a0e760 504 {
photonicbabe 0:a2b784a0e760 505 target_vy = -0.6;
photonicbabe 0:a2b784a0e760 506 target_vx = 0;
photonicbabe 0:a2b784a0e760 507 }
photonicbabe 0:a2b784a0e760 508 }
photonicbabe 0:a2b784a0e760 509 else if(fabs(target_x)==fabs(target_y))
photonicbabe 0:a2b784a0e760 510 {
photonicbabe 0:a2b784a0e760 511 if((target_x<0)&&(target_y<0))
photonicbabe 0:a2b784a0e760 512 {
photonicbabe 0:a2b784a0e760 513 target_vy = -0.6;
photonicbabe 0:a2b784a0e760 514 target_vx = target_vy;
photonicbabe 0:a2b784a0e760 515 }
photonicbabe 0:a2b784a0e760 516 else if((target_x<0)&&(target_y>0))
photonicbabe 0:a2b784a0e760 517 {
photonicbabe 0:a2b784a0e760 518 target_vy = 0.6;
photonicbabe 0:a2b784a0e760 519 target_vx = -1*target_vy;
photonicbabe 0:a2b784a0e760 520 }
photonicbabe 0:a2b784a0e760 521 else if((target_x>0)&&(target_y<0))
photonicbabe 0:a2b784a0e760 522 {
photonicbabe 0:a2b784a0e760 523 target_vy = -0.6;
photonicbabe 0:a2b784a0e760 524 target_vx = -1*target_vy;
photonicbabe 0:a2b784a0e760 525 }
photonicbabe 0:a2b784a0e760 526 else if((target_x>0)&&(target_y>0))
photonicbabe 0:a2b784a0e760 527 {
photonicbabe 0:a2b784a0e760 528 target_vy = 0.6;
photonicbabe 0:a2b784a0e760 529 target_vx = target_vy;
photonicbabe 0:a2b784a0e760 530 }
photonicbabe 0:a2b784a0e760 531 }
photonicbabe 0:a2b784a0e760 532 //mencari kecepatan sudut tiap roda berdasarkan target kecepatan translasi
photonicbabe 0:a2b784a0e760 533 target_depan_kiri = (1/radius)*(sin(45.0f*rtd)*target_vx + cos(45.0f*rtd)*target_vy);
photonicbabe 0:a2b784a0e760 534 target_depan_kanan = (1/radius)*(sin(135.0f*rtd)*target_vx + cos(135.0f*rtd)*target_vy);
photonicbabe 0:a2b784a0e760 535 target_belakang_kanan = (1/radius)*(sin(225.0f*rtd)*target_vx + cos(225.0f*rtd)*target_vy);
photonicbabe 0:a2b784a0e760 536 target_belakang_kiri = (1/radius)*(sin(315.0f*rtd)*target_vx + cos(315.0f*rtd)*target_vy);
photonicbabe 0:a2b784a0e760 537 target_depan_kiri = (target_depan_kiri*624.0f)/(2.0f*3.14);
photonicbabe 0:a2b784a0e760 538 target_depan_kanan = (target_depan_kanan*624.0f)/(2.0f*3.14);
photonicbabe 0:a2b784a0e760 539 target_belakang_kiri = (target_belakang_kiri*624.0f)/(2.0f*3.14);
photonicbabe 0:a2b784a0e760 540 target_belakang_kanan = (target_belakang_kanan*624.0f)/(2.0f*3.14);
photonicbabe 0:a2b784a0e760 541 //mengambil arah gerak motor
photonicbabe 0:a2b784a0e760 542 arah_depan_kiri = cari_abs(target_depan_kiri);
photonicbabe 0:a2b784a0e760 543 arah_depan_kanan = cari_abs(target_depan_kanan);
photonicbabe 0:a2b784a0e760 544 arah_belakang_kiri = cari_abs(target_belakang_kiri);
photonicbabe 0:a2b784a0e760 545 arah_belakang_kanan = cari_abs(target_belakang_kanan);
photonicbabe 0:a2b784a0e760 546 }
photonicbabe 0:a2b784a0e760 547
photonicbabe 0:a2b784a0e760 548 void set_inverse()
photonicbabe 0:a2b784a0e760 549 {
photonicbabe 0:a2b784a0e760 550 //memasukkan nilai target kecepatan sudut untuk setpoint PID
photonicbabe 0:a2b784a0e760 551 if(arah_depan_kanan!=0)
photonicbabe 0:a2b784a0e760 552 {
photonicbabe 0:a2b784a0e760 553 if(arah_depan_kiri==0)
photonicbabe 0:a2b784a0e760 554 {
photonicbabe 0:a2b784a0e760 555 DepanKananPID.setSetPoint(fabs(target_depan_kanan));
photonicbabe 0:a2b784a0e760 556 BelakangKiriPID.setSetPoint(fabs(target_belakang_kiri));
photonicbabe 0:a2b784a0e760 557 BelakangKananPID.setSetPoint(0);
photonicbabe 0:a2b784a0e760 558 DepanKiriPID.setSetPoint(0);
photonicbabe 0:a2b784a0e760 559 }
photonicbabe 0:a2b784a0e760 560 else if(arah_depan_kiri!=0)
photonicbabe 0:a2b784a0e760 561 {
photonicbabe 0:a2b784a0e760 562 DepanKananPID.setSetPoint(fabs(target_depan_kanan));
photonicbabe 0:a2b784a0e760 563 BelakangKiriPID.setSetPoint(fabs(target_belakang_kiri));
photonicbabe 0:a2b784a0e760 564 BelakangKananPID.setSetPoint(fabs(target_belakang_kanan));
photonicbabe 0:a2b784a0e760 565 DepanKiriPID.setSetPoint(fabs(target_depan_kiri));
photonicbabe 0:a2b784a0e760 566 }
photonicbabe 0:a2b784a0e760 567 }
photonicbabe 0:a2b784a0e760 568 else if(arah_depan_kiri!=0)
photonicbabe 0:a2b784a0e760 569 {
photonicbabe 0:a2b784a0e760 570 if(arah_depan_kanan==0)
photonicbabe 0:a2b784a0e760 571 {
photonicbabe 0:a2b784a0e760 572 DepanKananPID.setSetPoint(0);
photonicbabe 0:a2b784a0e760 573 BelakangKiriPID.setSetPoint(0);
photonicbabe 0:a2b784a0e760 574 BelakangKananPID.setSetPoint(fabs(target_belakang_kanan));
photonicbabe 0:a2b784a0e760 575 DepanKiriPID.setSetPoint(fabs(target_depan_kiri));
photonicbabe 0:a2b784a0e760 576 }
photonicbabe 0:a2b784a0e760 577 else if(arah_depan_kanan!=0)
photonicbabe 0:a2b784a0e760 578 {
photonicbabe 0:a2b784a0e760 579 DepanKananPID.setSetPoint(fabs(target_depan_kanan));
photonicbabe 0:a2b784a0e760 580 BelakangKiriPID.setSetPoint(fabs(target_belakang_kiri));
photonicbabe 0:a2b784a0e760 581 BelakangKananPID.setSetPoint(fabs(target_belakang_kanan));
photonicbabe 0:a2b784a0e760 582 DepanKiriPID.setSetPoint(fabs(target_depan_kiri));
photonicbabe 0:a2b784a0e760 583 }
photonicbabe 0:a2b784a0e760 584 }
photonicbabe 0:a2b784a0e760 585 }
photonicbabe 0:a2b784a0e760 586
photonicbabe 0:a2b784a0e760 587 void jalan_maju()
photonicbabe 0:a2b784a0e760 588 {
photonicbabe 0:a2b784a0e760 589 t_jalan.reset();
photonicbabe 0:a2b784a0e760 590 t_jalan.start();
photonicbabe 0:a2b784a0e760 591 proses_kecepatan();
photonicbabe 0:a2b784a0e760 592 Get_Count();
photonicbabe 0:a2b784a0e760 593 Cari_Koordinat();
photonicbabe 0:a2b784a0e760 594 pc.printf("Jarak : %d\n", jarak);
photonicbabe 0:a2b784a0e760 595 //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
photonicbabe 0:a2b784a0e760 596 BelakangKanan.speed(BelakangKananPID.compute()*-1);
photonicbabe 0:a2b784a0e760 597 BelakangKiri.speed(BelakangKiriPID.compute());
photonicbabe 0:a2b784a0e760 598 DepanKanan.speed(DepanKananPID.compute()*-1);
photonicbabe 0:a2b784a0e760 599 DepanKiri.speed(DepanKiriPID.compute());
photonicbabe 0:a2b784a0e760 600 while(t_jalan.read_ms()<=20)
photonicbabe 0:a2b784a0e760 601 {
photonicbabe 0:a2b784a0e760 602 }
photonicbabe 0:a2b784a0e760 603 t_jalan.reset();
photonicbabe 0:a2b784a0e760 604 }
photonicbabe 0:a2b784a0e760 605
photonicbabe 0:a2b784a0e760 606 void jalan_mundur()
photonicbabe 0:a2b784a0e760 607 {
photonicbabe 0:a2b784a0e760 608 t_jalan.reset();
photonicbabe 0:a2b784a0e760 609 t_jalan.start();
photonicbabe 0:a2b784a0e760 610 proses_kecepatan();
photonicbabe 0:a2b784a0e760 611 Get_Count();
photonicbabe 0:a2b784a0e760 612 Cari_Koordinat();
photonicbabe 0:a2b784a0e760 613 pc.printf("Koordinat X : %d\n", koordinat_x_bulat);
photonicbabe 0:a2b784a0e760 614 pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
photonicbabe 0:a2b784a0e760 615 BelakangKanan.speed(BelakangKananPID.compute());
photonicbabe 0:a2b784a0e760 616 BelakangKiri.speed(BelakangKiriPID.compute()*-1);
photonicbabe 0:a2b784a0e760 617 DepanKanan.speed(DepanKananPID.compute());
photonicbabe 0:a2b784a0e760 618 DepanKiri.speed(DepanKiriPID.compute()*-1);
photonicbabe 0:a2b784a0e760 619 while(t_jalan.read_ms()<=20)
photonicbabe 0:a2b784a0e760 620 {
photonicbabe 0:a2b784a0e760 621 }
photonicbabe 0:a2b784a0e760 622 t_jalan.reset();
photonicbabe 0:a2b784a0e760 623 }
photonicbabe 0:a2b784a0e760 624
photonicbabe 0:a2b784a0e760 625 void jalan_kanan()
photonicbabe 0:a2b784a0e760 626 {
photonicbabe 0:a2b784a0e760 627 t_jalan.reset();
photonicbabe 0:a2b784a0e760 628 t_jalan.start();
photonicbabe 0:a2b784a0e760 629 proses_kecepatan();
photonicbabe 0:a2b784a0e760 630 Get_Count();
photonicbabe 0:a2b784a0e760 631 Cari_Koordinat();
photonicbabe 0:a2b784a0e760 632 pc.printf("Koordinat X : %d\n", koordinat_x_bulat);
photonicbabe 0:a2b784a0e760 633 pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
photonicbabe 0:a2b784a0e760 634 BelakangKanan.speed(BelakangKananPID.compute()*-1);
photonicbabe 0:a2b784a0e760 635 BelakangKiri.speed(BelakangKiriPID.compute()*-1);
photonicbabe 0:a2b784a0e760 636 DepanKanan.speed(DepanKananPID.compute());
photonicbabe 0:a2b784a0e760 637 DepanKiri.speed(DepanKiriPID.compute());
photonicbabe 0:a2b784a0e760 638 while(t_jalan.read_ms()<=20)
photonicbabe 0:a2b784a0e760 639 {
photonicbabe 0:a2b784a0e760 640 }
photonicbabe 0:a2b784a0e760 641 t_jalan.reset();
photonicbabe 0:a2b784a0e760 642 }
photonicbabe 0:a2b784a0e760 643
photonicbabe 0:a2b784a0e760 644 void jalan_kiri()
photonicbabe 0:a2b784a0e760 645 {
photonicbabe 0:a2b784a0e760 646 t_jalan.reset();
photonicbabe 0:a2b784a0e760 647 t_jalan.start();
photonicbabe 0:a2b784a0e760 648 proses_kecepatan();
photonicbabe 0:a2b784a0e760 649 Get_Count();
photonicbabe 0:a2b784a0e760 650 Cari_Koordinat();
photonicbabe 0:a2b784a0e760 651 pc.printf("Koordinat X : %d\n", koordinat_x_bulat);
photonicbabe 0:a2b784a0e760 652 pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
photonicbabe 0:a2b784a0e760 653 BelakangKanan.speed(BelakangKananPID.compute());
photonicbabe 0:a2b784a0e760 654 BelakangKiri.speed(BelakangKiriPID.compute());
photonicbabe 0:a2b784a0e760 655 DepanKanan.speed(DepanKananPID.compute()*-1);
photonicbabe 0:a2b784a0e760 656 DepanKiri.speed(DepanKiriPID.compute()*-1);
photonicbabe 0:a2b784a0e760 657 while(t_jalan.read_ms()<=20)
photonicbabe 0:a2b784a0e760 658 {
photonicbabe 0:a2b784a0e760 659 }
photonicbabe 0:a2b784a0e760 660 t_jalan.reset();
photonicbabe 0:a2b784a0e760 661 }
photonicbabe 0:a2b784a0e760 662
photonicbabe 0:a2b784a0e760 663 void jalan_rotasi_kanan()
photonicbabe 0:a2b784a0e760 664 {
photonicbabe 0:a2b784a0e760 665 t_jalan.reset();
photonicbabe 0:a2b784a0e760 666 t_jalan.start();
photonicbabe 0:a2b784a0e760 667 proses_kecepatan();
photonicbabe 0:a2b784a0e760 668 Get_Count();
photonicbabe 0:a2b784a0e760 669 Cari_Koordinat();
photonicbabe 0:a2b784a0e760 670 //pc.printf("Koordinat X : %d\n", koordinat_x_bulat);
photonicbabe 0:a2b784a0e760 671 //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
photonicbabe 0:a2b784a0e760 672 pc.printf("Orientasi : %d\n", orientasi_2);
photonicbabe 0:a2b784a0e760 673 BelakangKanan.speed(BelakangKananPID.compute());
photonicbabe 0:a2b784a0e760 674 BelakangKiri.speed(BelakangKiriPID.compute());
photonicbabe 0:a2b784a0e760 675 DepanKanan.speed(DepanKananPID.compute());
photonicbabe 0:a2b784a0e760 676 DepanKiri.speed(DepanKiriPID.compute());
photonicbabe 0:a2b784a0e760 677 while(t_jalan.read_ms()<=20)
photonicbabe 0:a2b784a0e760 678 {
photonicbabe 0:a2b784a0e760 679 }
photonicbabe 0:a2b784a0e760 680 t_jalan.reset();
photonicbabe 0:a2b784a0e760 681 }
photonicbabe 0:a2b784a0e760 682
photonicbabe 0:a2b784a0e760 683 void jalan_rotasi_kiri()
photonicbabe 0:a2b784a0e760 684 {
photonicbabe 0:a2b784a0e760 685 t_jalan.reset();
photonicbabe 0:a2b784a0e760 686 t_jalan.start();
photonicbabe 0:a2b784a0e760 687 proses_kecepatan();
photonicbabe 0:a2b784a0e760 688 Get_Count();
photonicbabe 0:a2b784a0e760 689 Cari_Koordinat();
photonicbabe 0:a2b784a0e760 690 //pc.printf("Koordinat X : %d\n", koordinat_x_bulat);
photonicbabe 0:a2b784a0e760 691 //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
photonicbabe 0:a2b784a0e760 692 pc.printf("Orientasi : %d\n", orientasi_2);
photonicbabe 0:a2b784a0e760 693 BelakangKanan.speed(BelakangKananPID.compute()*-1);
photonicbabe 0:a2b784a0e760 694 BelakangKiri.speed(BelakangKiriPID.compute()*-1);
photonicbabe 0:a2b784a0e760 695 DepanKanan.speed(DepanKananPID.compute()*-1);
photonicbabe 0:a2b784a0e760 696 DepanKiri.speed(DepanKiriPID.compute()*-1);
photonicbabe 0:a2b784a0e760 697 while(t_jalan.read_ms()<=20)
photonicbabe 0:a2b784a0e760 698 {
photonicbabe 0:a2b784a0e760 699 }
photonicbabe 0:a2b784a0e760 700 t_jalan.reset();
photonicbabe 0:a2b784a0e760 701 }
photonicbabe 0:a2b784a0e760 702
photonicbabe 0:a2b784a0e760 703 void berhenti()
photonicbabe 0:a2b784a0e760 704 {
photonicbabe 0:a2b784a0e760 705 t_jalan.reset();
photonicbabe 0:a2b784a0e760 706 t_jalan.start();
photonicbabe 0:a2b784a0e760 707 //pc.printf("Koordinat X : %d\n", koordinat_x_bulat);
photonicbabe 0:a2b784a0e760 708 //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
photonicbabe 0:a2b784a0e760 709 pc.printf("Orientasi : %d\n", orientasi_2);
photonicbabe 0:a2b784a0e760 710 DepanKanan.brake(0);
photonicbabe 0:a2b784a0e760 711 BelakangKanan.brake(0);
photonicbabe 0:a2b784a0e760 712 BelakangKiri.brake(0);
photonicbabe 0:a2b784a0e760 713 DepanKiri.brake(0);
photonicbabe 0:a2b784a0e760 714 while(t_jalan.read_ms()<=20)
photonicbabe 0:a2b784a0e760 715 {
photonicbabe 0:a2b784a0e760 716 }
photonicbabe 0:a2b784a0e760 717 t_jalan.reset();
photonicbabe 0:a2b784a0e760 718 }
photonicbabe 0:a2b784a0e760 719
photonicbabe 0:a2b784a0e760 720 void ke_setpoint_x()
photonicbabe 0:a2b784a0e760 721 {
photonicbabe 0:a2b784a0e760 722 if(koordinat_x_bulat<setpoint_x)
photonicbabe 0:a2b784a0e760 723 {
photonicbabe 0:a2b784a0e760 724 kosongkan();
photonicbabe 0:a2b784a0e760 725 while(koordinat_x_bulat<setpoint_x)
photonicbabe 0:a2b784a0e760 726 {
photonicbabe 0:a2b784a0e760 727 jalan_kanan();
photonicbabe 0:a2b784a0e760 728 }
photonicbabe 0:a2b784a0e760 729 DepanKanan.brake();
photonicbabe 0:a2b784a0e760 730 BelakangKanan.brake();
photonicbabe 0:a2b784a0e760 731 BelakangKiri.brake();
photonicbabe 0:a2b784a0e760 732 DepanKiri.brake();
photonicbabe 0:a2b784a0e760 733 }
photonicbabe 0:a2b784a0e760 734 else if(koordinat_x_bulat>setpoint_x)
photonicbabe 0:a2b784a0e760 735 {
photonicbabe 0:a2b784a0e760 736 kosongkan();
photonicbabe 0:a2b784a0e760 737 while(koordinat_x_bulat>setpoint_x)
photonicbabe 0:a2b784a0e760 738 {
photonicbabe 0:a2b784a0e760 739 jalan_kiri();
photonicbabe 0:a2b784a0e760 740 }
photonicbabe 0:a2b784a0e760 741 DepanKanan.brake();
photonicbabe 0:a2b784a0e760 742 BelakangKanan.brake();
photonicbabe 0:a2b784a0e760 743 BelakangKiri.brake();
photonicbabe 0:a2b784a0e760 744 DepanKiri.brake();
photonicbabe 0:a2b784a0e760 745 }
photonicbabe 0:a2b784a0e760 746 else if(koordinat_x_bulat==setpoint_x)
photonicbabe 0:a2b784a0e760 747 {
photonicbabe 0:a2b784a0e760 748 berhenti();
photonicbabe 0:a2b784a0e760 749 }
photonicbabe 0:a2b784a0e760 750 }
photonicbabe 0:a2b784a0e760 751
photonicbabe 0:a2b784a0e760 752 void ke_setpoint_y()
photonicbabe 0:a2b784a0e760 753 {
photonicbabe 0:a2b784a0e760 754 if(koordinat_y_bulat<setpoint_y)
photonicbabe 0:a2b784a0e760 755 {
photonicbabe 0:a2b784a0e760 756 kosongkan();
photonicbabe 0:a2b784a0e760 757 while(koordinat_y_bulat<setpoint_y)
photonicbabe 0:a2b784a0e760 758 {
photonicbabe 0:a2b784a0e760 759 jalan_maju();
photonicbabe 0:a2b784a0e760 760 }
photonicbabe 0:a2b784a0e760 761 DepanKanan.brake();
photonicbabe 0:a2b784a0e760 762 BelakangKanan.brake();
photonicbabe 0:a2b784a0e760 763 BelakangKiri.brake();
photonicbabe 0:a2b784a0e760 764 DepanKiri.brake();
photonicbabe 0:a2b784a0e760 765 }
photonicbabe 0:a2b784a0e760 766 else if(koordinat_y_bulat>setpoint_y)
photonicbabe 0:a2b784a0e760 767 {
photonicbabe 0:a2b784a0e760 768 kosongkan();
photonicbabe 0:a2b784a0e760 769 while(koordinat_y>setpoint_y)
photonicbabe 0:a2b784a0e760 770 {
photonicbabe 0:a2b784a0e760 771 jalan_mundur();
photonicbabe 0:a2b784a0e760 772 }
photonicbabe 0:a2b784a0e760 773 DepanKanan.brake();
photonicbabe 0:a2b784a0e760 774 BelakangKanan.brake();
photonicbabe 0:a2b784a0e760 775 BelakangKiri.brake();
photonicbabe 0:a2b784a0e760 776 DepanKiri.brake();
photonicbabe 0:a2b784a0e760 777 }
photonicbabe 0:a2b784a0e760 778 else if(koordinat_y_bulat==setpoint_y)
photonicbabe 0:a2b784a0e760 779 {
photonicbabe 0:a2b784a0e760 780 berhenti();
photonicbabe 0:a2b784a0e760 781 }
photonicbabe 0:a2b784a0e760 782 }
photonicbabe 0:a2b784a0e760 783
photonicbabe 0:a2b784a0e760 784 void mode_cepat()
photonicbabe 0:a2b784a0e760 785 {
photonicbabe 0:a2b784a0e760 786 target_depan_kanan = 1800.0f;
photonicbabe 0:a2b784a0e760 787 target_depan_kiri = 1800.0f;
photonicbabe 0:a2b784a0e760 788 target_belakang_kanan = 1800.0f;
photonicbabe 0:a2b784a0e760 789 target_belakang_kiri = 1800.0f;
photonicbabe 0:a2b784a0e760 790 }
photonicbabe 0:a2b784a0e760 791
photonicbabe 0:a2b784a0e760 792 void mode_lambat()
photonicbabe 0:a2b784a0e760 793 {
photonicbabe 0:a2b784a0e760 794 target_depan_kanan = 1000.0f;
photonicbabe 0:a2b784a0e760 795 target_depan_kiri = 1000.0f;
photonicbabe 0:a2b784a0e760 796 target_belakang_kanan = 1000.0f;
photonicbabe 0:a2b784a0e760 797 target_belakang_kiri = 1000.0f;
photonicbabe 0:a2b784a0e760 798 }
photonicbabe 0:a2b784a0e760 799
photonicbabe 0:a2b784a0e760 800
photonicbabe 0:a2b784a0e760 801 void ke_target_orientasi()
photonicbabe 0:a2b784a0e760 802 {
photonicbabe 0:a2b784a0e760 803 if(orientasi_2>target_orientasi)
photonicbabe 0:a2b784a0e760 804 {
photonicbabe 0:a2b784a0e760 805 kosongkan();
photonicbabe 0:a2b784a0e760 806 while(orientasi_2<target_orientasi)
photonicbabe 0:a2b784a0e760 807 {
photonicbabe 0:a2b784a0e760 808 jalan_rotasi_kiri();
photonicbabe 0:a2b784a0e760 809 }
photonicbabe 0:a2b784a0e760 810 DepanKanan.brake();
photonicbabe 0:a2b784a0e760 811 BelakangKanan.brake();
photonicbabe 0:a2b784a0e760 812 BelakangKiri.brake();
photonicbabe 0:a2b784a0e760 813 DepanKiri.brake();
photonicbabe 0:a2b784a0e760 814 }
photonicbabe 0:a2b784a0e760 815 else if(orientasi_2<target_orientasi)
photonicbabe 0:a2b784a0e760 816 {
photonicbabe 0:a2b784a0e760 817 kosongkan();
photonicbabe 0:a2b784a0e760 818 while(orientasi_2>target_orientasi)
photonicbabe 0:a2b784a0e760 819 {
photonicbabe 0:a2b784a0e760 820 jalan_rotasi_kanan();
photonicbabe 0:a2b784a0e760 821 }
photonicbabe 0:a2b784a0e760 822 DepanKanan.brake();
photonicbabe 0:a2b784a0e760 823 BelakangKanan.brake();
photonicbabe 0:a2b784a0e760 824 BelakangKiri.brake();
photonicbabe 0:a2b784a0e760 825 DepanKiri.brake();
photonicbabe 0:a2b784a0e760 826 }
photonicbabe 0:a2b784a0e760 827 else if(orientasi_2==target_orientasi)
photonicbabe 0:a2b784a0e760 828 {
photonicbabe 0:a2b784a0e760 829 berhenti();
photonicbabe 0:a2b784a0e760 830 }
photonicbabe 0:a2b784a0e760 831 }
photonicbabe 0:a2b784a0e760 832
photonicbabe 0:a2b784a0e760 833 void ke_target_orientasi2()
photonicbabe 0:a2b784a0e760 834 {
photonicbabe 0:a2b784a0e760 835 while(orientasi_2<target_orientasi)
photonicbabe 0:a2b784a0e760 836 {
photonicbabe 0:a2b784a0e760 837 jalan_rotasi_kiri();
photonicbabe 0:a2b784a0e760 838 if(orientasi_2 == target_orientasi)
photonicbabe 0:a2b784a0e760 839 {
photonicbabe 0:a2b784a0e760 840 berhenti();
photonicbabe 0:a2b784a0e760 841 }
photonicbabe 0:a2b784a0e760 842 }
photonicbabe 0:a2b784a0e760 843 while(orientasi_2>target_orientasi)
photonicbabe 0:a2b784a0e760 844 {
photonicbabe 0:a2b784a0e760 845 jalan_rotasi_kanan();
photonicbabe 0:a2b784a0e760 846 if(orientasi_2 == target_orientasi)
photonicbabe 0:a2b784a0e760 847 {
photonicbabe 0:a2b784a0e760 848 berhenti();
photonicbabe 0:a2b784a0e760 849 }
photonicbabe 0:a2b784a0e760 850 }
photonicbabe 0:a2b784a0e760 851 }
photonicbabe 0:a2b784a0e760 852
photonicbabe 0:a2b784a0e760 853 void jalan_inverse()
photonicbabe 0:a2b784a0e760 854 {
photonicbabe 0:a2b784a0e760 855 if((arah_depan_kiri==0)&&(arah_belakang_kanan==0)==1)
photonicbabe 0:a2b784a0e760 856 {
photonicbabe 0:a2b784a0e760 857 BelakangKiri.speed(BelakangKiriPID.compute()*arah_belakang_kiri);
photonicbabe 0:a2b784a0e760 858 DepanKanan.speed(DepanKananPID.compute()*arah_depan_kanan);
photonicbabe 0:a2b784a0e760 859 }
photonicbabe 0:a2b784a0e760 860 else if((arah_depan_kanan==0)&&(arah_belakang_kiri==0)==1)
photonicbabe 0:a2b784a0e760 861 {
photonicbabe 0:a2b784a0e760 862 BelakangKanan.speed(BelakangKiriPID.compute()*arah_belakang_kanan);
photonicbabe 0:a2b784a0e760 863 DepanKiri.speed(DepanKananPID.compute()*arah_depan_kiri);
photonicbabe 0:a2b784a0e760 864 }
photonicbabe 0:a2b784a0e760 865 else if(arah_depan_kiri!=0)
photonicbabe 0:a2b784a0e760 866 {
photonicbabe 0:a2b784a0e760 867 if(arah_depan_kanan!=0)
photonicbabe 0:a2b784a0e760 868 {
photonicbabe 0:a2b784a0e760 869 BelakangKiri.speed(BelakangKiriPID.compute()*arah_belakang_kiri);
photonicbabe 0:a2b784a0e760 870 DepanKanan.speed(DepanKananPID.compute()*arah_depan_kanan);
photonicbabe 0:a2b784a0e760 871 BelakangKanan.speed(BelakangKiriPID.compute()*arah_belakang_kanan);
photonicbabe 0:a2b784a0e760 872 DepanKiri.speed(DepanKananPID.compute()*arah_depan_kiri);
photonicbabe 0:a2b784a0e760 873 }
photonicbabe 0:a2b784a0e760 874 }
photonicbabe 0:a2b784a0e760 875 }
photonicbabe 0:a2b784a0e760 876
photonicbabe 0:a2b784a0e760 877 void cari_koin1()
photonicbabe 0:a2b784a0e760 878 {
photonicbabe 0:a2b784a0e760 879 target_orientasi = 180;
photonicbabe 0:a2b784a0e760 880 ke_target_orientasi();
photonicbabe 0:a2b784a0e760 881 while(jarak>=23)
photonicbabe 0:a2b784a0e760 882 {
photonicbabe 0:a2b784a0e760 883 //jarak = ultra.read();
photonicbabe 0:a2b784a0e760 884 jalan_maju();
photonicbabe 0:a2b784a0e760 885 }
photonicbabe 0:a2b784a0e760 886 //while(jarak<=20)
photonicbabe 0:a2b784a0e760 887 //{
photonicbabe 0:a2b784a0e760 888 // jalan_maju();
photonicbabe 0:a2b784a0e760 889 //}
photonicbabe 0:a2b784a0e760 890 //while(eksistensi_koin!=0)
photonicbabe 0:a2b784a0e760 891 //{
photonicbabe 0:a2b784a0e760 892 // jalan_kanan();
photonicbabe 0:a2b784a0e760 893 //}
photonicbabe 0:a2b784a0e760 894 }
photonicbabe 0:a2b784a0e760 895
photonicbabe 0:a2b784a0e760 896 float sensor(){
photonicbabe 0:a2b784a0e760 897 float jarak2;
photonicbabe 0:a2b784a0e760 898 int durasi;
photonicbabe 0:a2b784a0e760 899 trigPin1=1;
photonicbabe 0:a2b784a0e760 900 t.reset();
photonicbabe 0:a2b784a0e760 901 wait_us(10);
photonicbabe 0:a2b784a0e760 902 trigPin1=0;
photonicbabe 0:a2b784a0e760 903 while (echoPin1 == 0){
photonicbabe 0:a2b784a0e760 904 }t.start();
photonicbabe 0:a2b784a0e760 905 while (echoPin1 == 1)
photonicbabe 0:a2b784a0e760 906 {
photonicbabe 0:a2b784a0e760 907 }
photonicbabe 0:a2b784a0e760 908 t.stop();
photonicbabe 0:a2b784a0e760 909 durasi = t.read_us();
photonicbabe 0:a2b784a0e760 910 jarak2 = (double)(durasi) / 29.0f /2.0f ; // 29 kecepatan suara di udara (2.9 detik untuk 1 kilometer), dibagi 2 karena bolak-balik
photonicbabe 0:a2b784a0e760 911 return jarak2;
photonicbabe 0:a2b784a0e760 912 }
photonicbabe 0:a2b784a0e760 913
photonicbabe 0:a2b784a0e760 914 void cari_koin2()
photonicbabe 0:a2b784a0e760 915 {
photonicbabe 0:a2b784a0e760 916 while(orientasi != 180)
photonicbabe 0:a2b784a0e760 917 {
photonicbabe 0:a2b784a0e760 918 ke_target_orientasi();
photonicbabe 0:a2b784a0e760 919 }
photonicbabe 0:a2b784a0e760 920 while(jarak>=23)
photonicbabe 0:a2b784a0e760 921 {
photonicbabe 0:a2b784a0e760 922 //jarak = ultra.read();
photonicbabe 0:a2b784a0e760 923 jalan_maju();
photonicbabe 0:a2b784a0e760 924 }
photonicbabe 0:a2b784a0e760 925 //while(eksistensi_koin!=0)
photonicbabe 0:a2b784a0e760 926 //{
photonicbabe 0:a2b784a0e760 927 // jalan_kanan();
photonicbabe 0:a2b784a0e760 928 //}
photonicbabe 0:a2b784a0e760 929 }
photonicbabe 0:a2b784a0e760 930
photonicbabe 0:a2b784a0e760 931 int main()
photonicbabe 0:a2b784a0e760 932 {
photonicbabe 0:a2b784a0e760 933 pc.baud(38400);
photonicbabe 0:a2b784a0e760 934 bluetooth.baud(9600);
photonicbabe 0:a2b784a0e760 935 bluetooth.attach(&fungsi_bluetooth, Serial::RxIrq);
photonicbabe 0:a2b784a0e760 936
photonicbabe 0:a2b784a0e760 937 BEARING_Register = 0x02;
photonicbabe 0:a2b784a0e760 938 DepanKanan.period(0.01f);
photonicbabe 0:a2b784a0e760 939 BelakangKanan.period(0.01f);
photonicbabe 0:a2b784a0e760 940 BelakangKiri.period(0.01f); //Set motor PWM periods to 20KHz.
photonicbabe 0:a2b784a0e760 941 DepanKiri.period(0.01f);
photonicbabe 0:a2b784a0e760 942
photonicbabe 0:a2b784a0e760 943 DepanKananPID.setInputLimits(0, 3000);//Input units: counts per second.
photonicbabe 0:a2b784a0e760 944 DepanKananPID.setOutputLimits(0.0, 0.9);//Output units: PwmOut duty cycle as %.
photonicbabe 0:a2b784a0e760 945 DepanKananPID.setMode(AUTO_MODE);
photonicbabe 0:a2b784a0e760 946
photonicbabe 0:a2b784a0e760 947 BelakangKananPID.setInputLimits(0, 3000);
photonicbabe 0:a2b784a0e760 948 BelakangKananPID.setOutputLimits(0.0, 0.9);
photonicbabe 0:a2b784a0e760 949 BelakangKananPID.setMode(AUTO_MODE);
photonicbabe 0:a2b784a0e760 950
photonicbabe 0:a2b784a0e760 951 BelakangKiriPID.setInputLimits(0, 3000);
photonicbabe 0:a2b784a0e760 952 BelakangKiriPID.setOutputLimits(0.0, 0.9);
photonicbabe 0:a2b784a0e760 953 BelakangKiriPID.setMode(AUTO_MODE);
photonicbabe 0:a2b784a0e760 954
photonicbabe 0:a2b784a0e760 955 DepanKiriPID.setInputLimits(0, 3000);
photonicbabe 0:a2b784a0e760 956 DepanKiriPID.setOutputLimits(0.0, 0.9);
photonicbabe 0:a2b784a0e760 957 DepanKiriPID.setMode(AUTO_MODE);
photonicbabe 0:a2b784a0e760 958
photonicbabe 0:a2b784a0e760 959 DepanKananPID.setSetPoint(1400);
photonicbabe 0:a2b784a0e760 960 BelakangKananPID.setSetPoint(1400);
photonicbabe 0:a2b784a0e760 961 BelakangKiriPID.setSetPoint(1400);
photonicbabe 0:a2b784a0e760 962 DepanKiriPID.setSetPoint(1400);
photonicbabe 0:a2b784a0e760 963 sudut_awal = Get_Yaw();
photonicbabe 0:a2b784a0e760 964 while(1)
photonicbabe 0:a2b784a0e760 965 {
photonicbabe 0:a2b784a0e760 966
photonicbabe 0:a2b784a0e760 967 if(Jalan == Jalan_Maju)
photonicbabe 0:a2b784a0e760 968 {
photonicbabe 0:a2b784a0e760 969 DepanKananPID.setSetPoint(target_depan_kanan);
photonicbabe 0:a2b784a0e760 970 BelakangKananPID.setSetPoint(target_belakang_kanan);
photonicbabe 0:a2b784a0e760 971 BelakangKiriPID.setSetPoint(target_belakang_kiri);
photonicbabe 0:a2b784a0e760 972 DepanKiriPID.setSetPoint(target_depan_kiri);
photonicbabe 0:a2b784a0e760 973 kosongkan();
photonicbabe 0:a2b784a0e760 974 while(Jalan== Jalan_Maju)
photonicbabe 0:a2b784a0e760 975 {
photonicbabe 0:a2b784a0e760 976 t_jalan.reset();
photonicbabe 0:a2b784a0e760 977 t_jalan.start();
photonicbabe 0:a2b784a0e760 978 proses_kecepatan();
photonicbabe 0:a2b784a0e760 979 Get_Count();
photonicbabe 0:a2b784a0e760 980 Cari_Koordinat();
photonicbabe 0:a2b784a0e760 981 pc.printf("Koordinat X : %d\n", koordinat_x_bulat);
photonicbabe 0:a2b784a0e760 982 //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
photonicbabe 0:a2b784a0e760 983 pc.printf("Orientasi : %f\n", orientasi);
photonicbabe 0:a2b784a0e760 984 BelakangKanan.speed(BelakangKananPID.compute()*-1);
photonicbabe 0:a2b784a0e760 985 BelakangKiri.speed(BelakangKiriPID.compute());
photonicbabe 0:a2b784a0e760 986 DepanKanan.speed(DepanKananPID.compute()*-1);
photonicbabe 0:a2b784a0e760 987 DepanKiri.speed(DepanKiriPID.compute());
photonicbabe 0:a2b784a0e760 988 while(t_jalan.read_ms()<=20)
photonicbabe 0:a2b784a0e760 989 {
photonicbabe 0:a2b784a0e760 990 }
photonicbabe 0:a2b784a0e760 991 t_jalan.reset();
photonicbabe 0:a2b784a0e760 992 }
photonicbabe 0:a2b784a0e760 993 DepanKanan.brake();
photonicbabe 0:a2b784a0e760 994 BelakangKanan.brake();
photonicbabe 0:a2b784a0e760 995 BelakangKiri.brake();
photonicbabe 0:a2b784a0e760 996 DepanKiri.brake();
photonicbabe 0:a2b784a0e760 997 }
photonicbabe 0:a2b784a0e760 998
photonicbabe 0:a2b784a0e760 999 else if(Jalan == Jalan_Mundur)
photonicbabe 0:a2b784a0e760 1000 {
photonicbabe 0:a2b784a0e760 1001 DepanKananPID.setSetPoint(target_depan_kanan);
photonicbabe 0:a2b784a0e760 1002 BelakangKananPID.setSetPoint(target_belakang_kanan);
photonicbabe 0:a2b784a0e760 1003 BelakangKiriPID.setSetPoint(target_belakang_kiri);
photonicbabe 0:a2b784a0e760 1004 DepanKiriPID.setSetPoint(target_depan_kiri);
photonicbabe 0:a2b784a0e760 1005 kosongkan();
photonicbabe 0:a2b784a0e760 1006 while(Jalan== Jalan_Mundur)
photonicbabe 0:a2b784a0e760 1007 {
photonicbabe 0:a2b784a0e760 1008 t_jalan.reset();
photonicbabe 0:a2b784a0e760 1009 t_jalan.start();
photonicbabe 0:a2b784a0e760 1010 proses_kecepatan();
photonicbabe 0:a2b784a0e760 1011 Get_Count();
photonicbabe 0:a2b784a0e760 1012 Cari_Koordinat();
photonicbabe 0:a2b784a0e760 1013 //pc.printf("Koordinat X : %d\n", koordinat_x_bulat);
photonicbabe 0:a2b784a0e760 1014 //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
photonicbabe 0:a2b784a0e760 1015 pc.printf("Orientasi : %f\n", orientasi);
photonicbabe 0:a2b784a0e760 1016 BelakangKanan.speed(BelakangKananPID.compute());
photonicbabe 0:a2b784a0e760 1017 BelakangKiri.speed(BelakangKiriPID.compute()*-1);
photonicbabe 0:a2b784a0e760 1018 DepanKanan.speed(DepanKananPID.compute());
photonicbabe 0:a2b784a0e760 1019 DepanKiri.speed(DepanKiriPID.compute()*-1);
photonicbabe 0:a2b784a0e760 1020 while(t_jalan.read_ms()<=20)
photonicbabe 0:a2b784a0e760 1021 {
photonicbabe 0:a2b784a0e760 1022 }
photonicbabe 0:a2b784a0e760 1023 t_jalan.reset();
photonicbabe 0:a2b784a0e760 1024 }
photonicbabe 0:a2b784a0e760 1025 DepanKanan.brake();
photonicbabe 0:a2b784a0e760 1026 BelakangKanan.brake();
photonicbabe 0:a2b784a0e760 1027 BelakangKiri.brake();
photonicbabe 0:a2b784a0e760 1028 DepanKiri.brake();
photonicbabe 0:a2b784a0e760 1029 }
photonicbabe 0:a2b784a0e760 1030
photonicbabe 0:a2b784a0e760 1031 else if(Jalan == Jalan_Kiri)
photonicbabe 0:a2b784a0e760 1032 {
photonicbabe 0:a2b784a0e760 1033 DepanKananPID.setSetPoint(target_depan_kanan);
photonicbabe 0:a2b784a0e760 1034 BelakangKananPID.setSetPoint(target_belakang_kanan);
photonicbabe 0:a2b784a0e760 1035 BelakangKiriPID.setSetPoint(target_belakang_kiri);
photonicbabe 0:a2b784a0e760 1036 DepanKiriPID.setSetPoint(target_depan_kiri);
photonicbabe 0:a2b784a0e760 1037 kosongkan();
photonicbabe 0:a2b784a0e760 1038 while(Jalan== Jalan_Kiri)
photonicbabe 0:a2b784a0e760 1039 {
photonicbabe 0:a2b784a0e760 1040 t_jalan.reset();
photonicbabe 0:a2b784a0e760 1041 t_jalan.start();
photonicbabe 0:a2b784a0e760 1042 proses_kecepatan();
photonicbabe 0:a2b784a0e760 1043 Get_Count();
photonicbabe 0:a2b784a0e760 1044 Cari_Koordinat();
photonicbabe 0:a2b784a0e760 1045 //pc.printf("Koordinat X : %d\n", koordinat_x_bulat);
photonicbabe 0:a2b784a0e760 1046 //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
photonicbabe 0:a2b784a0e760 1047 pc.printf("Orientasi : %f\n", orientasi);
photonicbabe 0:a2b784a0e760 1048 BelakangKanan.speed(BelakangKananPID.compute());
photonicbabe 0:a2b784a0e760 1049 BelakangKiri.speed(BelakangKiriPID.compute());
photonicbabe 0:a2b784a0e760 1050 DepanKanan.speed(DepanKananPID.compute()*-1);
photonicbabe 0:a2b784a0e760 1051 DepanKiri.speed(DepanKiriPID.compute()*-1);
photonicbabe 0:a2b784a0e760 1052 while(t_jalan.read_ms()<=20)
photonicbabe 0:a2b784a0e760 1053 {
photonicbabe 0:a2b784a0e760 1054 }
photonicbabe 0:a2b784a0e760 1055 t_jalan.reset();
photonicbabe 0:a2b784a0e760 1056 }
photonicbabe 0:a2b784a0e760 1057 DepanKanan.brake();
photonicbabe 0:a2b784a0e760 1058 BelakangKanan.brake();
photonicbabe 0:a2b784a0e760 1059 BelakangKiri.brake();
photonicbabe 0:a2b784a0e760 1060 DepanKiri.brake();
photonicbabe 0:a2b784a0e760 1061 }
photonicbabe 0:a2b784a0e760 1062 else if(Jalan == Jalan_Kanan)
photonicbabe 0:a2b784a0e760 1063 {
photonicbabe 0:a2b784a0e760 1064 DepanKananPID.setSetPoint(target_depan_kanan);
photonicbabe 0:a2b784a0e760 1065 BelakangKananPID.setSetPoint(target_belakang_kanan);
photonicbabe 0:a2b784a0e760 1066 BelakangKiriPID.setSetPoint(target_belakang_kiri);
photonicbabe 0:a2b784a0e760 1067 DepanKiriPID.setSetPoint(target_depan_kiri);
photonicbabe 0:a2b784a0e760 1068 kosongkan();
photonicbabe 0:a2b784a0e760 1069 while(Jalan== Jalan_Kanan)
photonicbabe 0:a2b784a0e760 1070 {
photonicbabe 0:a2b784a0e760 1071 t_jalan.reset();
photonicbabe 0:a2b784a0e760 1072 t_jalan.start();
photonicbabe 0:a2b784a0e760 1073 proses_kecepatan();
photonicbabe 0:a2b784a0e760 1074 Get_Count();
photonicbabe 0:a2b784a0e760 1075 Cari_Koordinat();
photonicbabe 0:a2b784a0e760 1076 //pc.printf("Koordinat X : %d\n", koordinat_x_bulat);
photonicbabe 0:a2b784a0e760 1077 //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
photonicbabe 0:a2b784a0e760 1078 pc.printf("Orientasi : %f\n", orientasi);
photonicbabe 0:a2b784a0e760 1079 BelakangKanan.speed(BelakangKananPID.compute()*-1);
photonicbabe 0:a2b784a0e760 1080 BelakangKiri.speed(BelakangKiriPID.compute()*-1);
photonicbabe 0:a2b784a0e760 1081 DepanKanan.speed(DepanKananPID.compute());
photonicbabe 0:a2b784a0e760 1082 DepanKiri.speed(DepanKiriPID.compute());
photonicbabe 0:a2b784a0e760 1083 while(t_jalan.read_ms()<=20)
photonicbabe 0:a2b784a0e760 1084 {
photonicbabe 0:a2b784a0e760 1085 }
photonicbabe 0:a2b784a0e760 1086 t_jalan.reset();
photonicbabe 0:a2b784a0e760 1087 }
photonicbabe 0:a2b784a0e760 1088 DepanKanan.brake(1);
photonicbabe 0:a2b784a0e760 1089 BelakangKanan.brake(1);
photonicbabe 0:a2b784a0e760 1090 BelakangKiri.brake(1);
photonicbabe 0:a2b784a0e760 1091 DepanKiri.brake(1);
photonicbabe 0:a2b784a0e760 1092 }
photonicbabe 0:a2b784a0e760 1093
photonicbabe 0:a2b784a0e760 1094 else if(Jalan == Rotasi_Kanan)
photonicbabe 0:a2b784a0e760 1095 {
photonicbabe 0:a2b784a0e760 1096 DepanKananPID.setSetPoint(target_depan_kanan-800.0f);
photonicbabe 0:a2b784a0e760 1097 BelakangKananPID.setSetPoint(target_belakang_kanan-800.0f);
photonicbabe 0:a2b784a0e760 1098 BelakangKiriPID.setSetPoint(target_belakang_kiri-800.0f);
photonicbabe 0:a2b784a0e760 1099 DepanKiriPID.setSetPoint(target_depan_kiri-800.0f);
photonicbabe 0:a2b784a0e760 1100 kosongkan();
photonicbabe 0:a2b784a0e760 1101 while(Jalan== Rotasi_Kanan)
photonicbabe 0:a2b784a0e760 1102 {
photonicbabe 0:a2b784a0e760 1103 t_jalan.reset();
photonicbabe 0:a2b784a0e760 1104 t_jalan.start();
photonicbabe 0:a2b784a0e760 1105 proses_kecepatan();
photonicbabe 0:a2b784a0e760 1106 Get_Count();
photonicbabe 0:a2b784a0e760 1107 Cari_Koordinat();
photonicbabe 0:a2b784a0e760 1108 //pc.printf("Koordinat X : %d\n", koordinat_x_bulat);
photonicbabe 0:a2b784a0e760 1109 //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
photonicbabe 0:a2b784a0e760 1110 pc.printf("Orientasi : %f\n", orientasi);
photonicbabe 0:a2b784a0e760 1111 BelakangKanan.speed(BelakangKananPID.compute());
photonicbabe 0:a2b784a0e760 1112 BelakangKiri.speed(BelakangKiriPID.compute());
photonicbabe 0:a2b784a0e760 1113 DepanKanan.speed(DepanKananPID.compute());
photonicbabe 0:a2b784a0e760 1114 DepanKiri.speed(DepanKiriPID.compute());
photonicbabe 0:a2b784a0e760 1115 while(t_jalan.read_ms()<=20)
photonicbabe 0:a2b784a0e760 1116 {
photonicbabe 0:a2b784a0e760 1117 }
photonicbabe 0:a2b784a0e760 1118 t_jalan.reset();
photonicbabe 0:a2b784a0e760 1119 }
photonicbabe 0:a2b784a0e760 1120 DepanKanan.brake();
photonicbabe 0:a2b784a0e760 1121 BelakangKanan.brake();
photonicbabe 0:a2b784a0e760 1122 BelakangKiri.brake();
photonicbabe 0:a2b784a0e760 1123 DepanKiri.brake();
photonicbabe 0:a2b784a0e760 1124 }
photonicbabe 0:a2b784a0e760 1125
photonicbabe 0:a2b784a0e760 1126 else if(Jalan == Rotasi_Kiri)
photonicbabe 0:a2b784a0e760 1127 {
photonicbabe 0:a2b784a0e760 1128 DepanKananPID.setSetPoint(target_depan_kanan-800.0f);
photonicbabe 0:a2b784a0e760 1129 BelakangKananPID.setSetPoint(target_belakang_kanan-800.0f);
photonicbabe 0:a2b784a0e760 1130 BelakangKiriPID.setSetPoint(target_belakang_kiri-800.0f);
photonicbabe 0:a2b784a0e760 1131 DepanKiriPID.setSetPoint(target_depan_kiri-800.0f);
photonicbabe 0:a2b784a0e760 1132 kosongkan();
photonicbabe 0:a2b784a0e760 1133 while(Jalan== Rotasi_Kiri)
photonicbabe 0:a2b784a0e760 1134 {
photonicbabe 0:a2b784a0e760 1135 t_jalan.reset();
photonicbabe 0:a2b784a0e760 1136 t_jalan.start();
photonicbabe 0:a2b784a0e760 1137 proses_kecepatan();
photonicbabe 0:a2b784a0e760 1138 Get_Count();
photonicbabe 0:a2b784a0e760 1139 Cari_Koordinat();
photonicbabe 0:a2b784a0e760 1140 //pc.printf("Koordinat X : %d\n", koordinat_x_bulat);
photonicbabe 0:a2b784a0e760 1141 //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
photonicbabe 0:a2b784a0e760 1142 pc.printf("Orientasi : %f\n", orientasi);
photonicbabe 0:a2b784a0e760 1143 BelakangKanan.speed(BelakangKananPID.compute()*-1);
photonicbabe 0:a2b784a0e760 1144 BelakangKiri.speed(BelakangKiriPID.compute()*-1);
photonicbabe 0:a2b784a0e760 1145 DepanKanan.speed(DepanKananPID.compute()*-1);
photonicbabe 0:a2b784a0e760 1146 DepanKiri.speed(DepanKiriPID.compute()*-1);
photonicbabe 0:a2b784a0e760 1147 while(t_jalan.read_ms()<=20)
photonicbabe 0:a2b784a0e760 1148 {
photonicbabe 0:a2b784a0e760 1149 }
photonicbabe 0:a2b784a0e760 1150 t_jalan.reset();
photonicbabe 0:a2b784a0e760 1151 }
photonicbabe 0:a2b784a0e760 1152 DepanKanan.brake();
photonicbabe 0:a2b784a0e760 1153 BelakangKanan.brake();
photonicbabe 0:a2b784a0e760 1154 BelakangKiri.brake();
photonicbabe 0:a2b784a0e760 1155 DepanKiri.brake();
photonicbabe 0:a2b784a0e760 1156 }
photonicbabe 0:a2b784a0e760 1157
photonicbabe 0:a2b784a0e760 1158 else if(Jalan == Belok_Kanan)
photonicbabe 0:a2b784a0e760 1159 {
photonicbabe 0:a2b784a0e760 1160 DepanKananPID.setSetPoint(target_depan_kanan-1000.0f);
photonicbabe 0:a2b784a0e760 1161 BelakangKananPID.setSetPoint(target_belakang_kanan-1000.0f);
photonicbabe 0:a2b784a0e760 1162 BelakangKiriPID.setSetPoint(target_belakang_kiri);
photonicbabe 0:a2b784a0e760 1163 DepanKiriPID.setSetPoint(target_depan_kiri);
photonicbabe 0:a2b784a0e760 1164 kosongkan();
photonicbabe 0:a2b784a0e760 1165 while(Jalan== Belok_Kanan)
photonicbabe 0:a2b784a0e760 1166 {
photonicbabe 0:a2b784a0e760 1167 t_jalan.reset();
photonicbabe 0:a2b784a0e760 1168 t_jalan.start();
photonicbabe 0:a2b784a0e760 1169 proses_kecepatan();
photonicbabe 0:a2b784a0e760 1170 Get_Count();
photonicbabe 0:a2b784a0e760 1171 Cari_Koordinat();
photonicbabe 0:a2b784a0e760 1172 //pc.printf("Koordinat X : %d\n", koordinat_x_bulat);
photonicbabe 0:a2b784a0e760 1173 //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
photonicbabe 0:a2b784a0e760 1174 pc.printf("Orientasi : %f\n", orientasi);
photonicbabe 0:a2b784a0e760 1175 BelakangKanan.speed(BelakangKananPID.compute()*-1);
photonicbabe 0:a2b784a0e760 1176 BelakangKiri.speed(BelakangKiriPID.compute());
photonicbabe 0:a2b784a0e760 1177 DepanKanan.speed(DepanKananPID.compute()*-1);
photonicbabe 0:a2b784a0e760 1178 DepanKiri.speed(DepanKiriPID.compute());
photonicbabe 0:a2b784a0e760 1179 while(t_jalan.read_ms()<=20)
photonicbabe 0:a2b784a0e760 1180 {
photonicbabe 0:a2b784a0e760 1181 }
photonicbabe 0:a2b784a0e760 1182 t_jalan.reset();
photonicbabe 0:a2b784a0e760 1183 }
photonicbabe 0:a2b784a0e760 1184 DepanKanan.brake();
photonicbabe 0:a2b784a0e760 1185 BelakangKanan.brake();
photonicbabe 0:a2b784a0e760 1186 BelakangKiri.brake();
photonicbabe 0:a2b784a0e760 1187 DepanKiri.brake();
photonicbabe 0:a2b784a0e760 1188 }
photonicbabe 0:a2b784a0e760 1189
photonicbabe 0:a2b784a0e760 1190 else if(Jalan == Mundur_Belok_Kanan)
photonicbabe 0:a2b784a0e760 1191 {
photonicbabe 0:a2b784a0e760 1192 DepanKananPID.setSetPoint(target_depan_kanan-1000.0f);
photonicbabe 0:a2b784a0e760 1193 BelakangKananPID.setSetPoint(target_belakang_kanan-1000.0f);
photonicbabe 0:a2b784a0e760 1194 BelakangKiriPID.setSetPoint(target_belakang_kiri);
photonicbabe 0:a2b784a0e760 1195 DepanKiriPID.setSetPoint(target_depan_kiri);
photonicbabe 0:a2b784a0e760 1196 kosongkan();
photonicbabe 0:a2b784a0e760 1197 while(Jalan== Mundur_Belok_Kanan)
photonicbabe 0:a2b784a0e760 1198 {
photonicbabe 0:a2b784a0e760 1199 t_jalan.reset();
photonicbabe 0:a2b784a0e760 1200 t_jalan.start();
photonicbabe 0:a2b784a0e760 1201 proses_kecepatan();
photonicbabe 0:a2b784a0e760 1202 Get_Count();
photonicbabe 0:a2b784a0e760 1203 Cari_Koordinat();
photonicbabe 0:a2b784a0e760 1204 //pc.printf("Koordinat X : %d\n", koordinat_x_bulat);
photonicbabe 0:a2b784a0e760 1205 //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
photonicbabe 0:a2b784a0e760 1206 pc.printf("Orientasi : %f\n", orientasi);
photonicbabe 0:a2b784a0e760 1207 BelakangKanan.speed(BelakangKananPID.compute());
photonicbabe 0:a2b784a0e760 1208 BelakangKiri.speed(BelakangKiriPID.compute()*-1);
photonicbabe 0:a2b784a0e760 1209 DepanKanan.speed(DepanKananPID.compute());
photonicbabe 0:a2b784a0e760 1210 DepanKiri.speed(DepanKiriPID.compute()*-1);
photonicbabe 0:a2b784a0e760 1211 while(t_jalan.read_ms()<=20)
photonicbabe 0:a2b784a0e760 1212 {
photonicbabe 0:a2b784a0e760 1213 }
photonicbabe 0:a2b784a0e760 1214 t_jalan.reset();
photonicbabe 0:a2b784a0e760 1215 }
photonicbabe 0:a2b784a0e760 1216 DepanKanan.brake();
photonicbabe 0:a2b784a0e760 1217 BelakangKanan.brake();
photonicbabe 0:a2b784a0e760 1218 BelakangKiri.brake();
photonicbabe 0:a2b784a0e760 1219 DepanKiri.brake();
photonicbabe 0:a2b784a0e760 1220 }
photonicbabe 0:a2b784a0e760 1221
photonicbabe 0:a2b784a0e760 1222 else if(Jalan == Mundur_Belok_Kiri)
photonicbabe 0:a2b784a0e760 1223 {
photonicbabe 0:a2b784a0e760 1224 DepanKananPID.setSetPoint(target_depan_kanan);
photonicbabe 0:a2b784a0e760 1225 BelakangKananPID.setSetPoint(target_belakang_kanan);
photonicbabe 0:a2b784a0e760 1226 BelakangKiriPID.setSetPoint(target_belakang_kiri-1000.0f);
photonicbabe 0:a2b784a0e760 1227 DepanKiriPID.setSetPoint(target_depan_kiri-1000.0f);
photonicbabe 0:a2b784a0e760 1228 kosongkan();
photonicbabe 0:a2b784a0e760 1229 while(Jalan== Mundur_Belok_Kiri)
photonicbabe 0:a2b784a0e760 1230 {
photonicbabe 0:a2b784a0e760 1231 t_jalan.reset();
photonicbabe 0:a2b784a0e760 1232 t_jalan.start();
photonicbabe 0:a2b784a0e760 1233 proses_kecepatan();
photonicbabe 0:a2b784a0e760 1234 Get_Count();
photonicbabe 0:a2b784a0e760 1235 Cari_Koordinat();
photonicbabe 0:a2b784a0e760 1236 //pc.printf("Koordinat X : %d\n", koordinat_x_bulat);
photonicbabe 0:a2b784a0e760 1237 //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
photonicbabe 0:a2b784a0e760 1238 pc.printf("Orientasi : %f\n", orientasi);
photonicbabe 0:a2b784a0e760 1239 BelakangKanan.speed(BelakangKananPID.compute());
photonicbabe 0:a2b784a0e760 1240 BelakangKiri.speed(BelakangKiriPID.compute()*-1);
photonicbabe 0:a2b784a0e760 1241 DepanKanan.speed(DepanKananPID.compute());
photonicbabe 0:a2b784a0e760 1242 DepanKiri.speed(DepanKiriPID.compute()*-1);
photonicbabe 0:a2b784a0e760 1243 while(t_jalan.read_ms()<=20)
photonicbabe 0:a2b784a0e760 1244 {
photonicbabe 0:a2b784a0e760 1245 }
photonicbabe 0:a2b784a0e760 1246 t_jalan.reset();
photonicbabe 0:a2b784a0e760 1247 }
photonicbabe 0:a2b784a0e760 1248 DepanKanan.brake();
photonicbabe 0:a2b784a0e760 1249 BelakangKanan.brake();
photonicbabe 0:a2b784a0e760 1250 BelakangKiri.brake();
photonicbabe 0:a2b784a0e760 1251 DepanKiri.brake();
photonicbabe 0:a2b784a0e760 1252 }
photonicbabe 0:a2b784a0e760 1253
photonicbabe 0:a2b784a0e760 1254 else if(Jalan == Belok_Kiri)
photonicbabe 0:a2b784a0e760 1255 {
photonicbabe 0:a2b784a0e760 1256 DepanKananPID.setSetPoint(target_depan_kanan);
photonicbabe 0:a2b784a0e760 1257 BelakangKananPID.setSetPoint(target_belakang_kanan);
photonicbabe 0:a2b784a0e760 1258 BelakangKiriPID.setSetPoint(target_belakang_kiri-1000);
photonicbabe 0:a2b784a0e760 1259 DepanKiriPID.setSetPoint(target_depan_kiri-1000);
photonicbabe 0:a2b784a0e760 1260 kosongkan();
photonicbabe 0:a2b784a0e760 1261 while(Jalan== Belok_Kiri)
photonicbabe 0:a2b784a0e760 1262 {
photonicbabe 0:a2b784a0e760 1263 t_jalan.reset();
photonicbabe 0:a2b784a0e760 1264 t_jalan.start();
photonicbabe 0:a2b784a0e760 1265 proses_kecepatan();
photonicbabe 0:a2b784a0e760 1266 Get_Count();
photonicbabe 0:a2b784a0e760 1267 Cari_Koordinat();
photonicbabe 0:a2b784a0e760 1268 //pc.printf("Koordinat X : %d\n", koordinat_x_bulat);
photonicbabe 0:a2b784a0e760 1269 //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
photonicbabe 0:a2b784a0e760 1270 pc.printf("Orientasi : %f\n", orientasi);
photonicbabe 0:a2b784a0e760 1271 BelakangKanan.speed(BelakangKananPID.compute()*-1);
photonicbabe 0:a2b784a0e760 1272 BelakangKiri.speed(BelakangKiriPID.compute());
photonicbabe 0:a2b784a0e760 1273 DepanKanan.speed(DepanKananPID.compute()*-1);
photonicbabe 0:a2b784a0e760 1274 DepanKiri.speed(DepanKiriPID.compute());
photonicbabe 0:a2b784a0e760 1275 while(t_jalan.read_ms()<=20)
photonicbabe 0:a2b784a0e760 1276 {
photonicbabe 0:a2b784a0e760 1277 }
photonicbabe 0:a2b784a0e760 1278 t_jalan.reset();
photonicbabe 0:a2b784a0e760 1279 }
photonicbabe 0:a2b784a0e760 1280 DepanKanan.brake();
photonicbabe 0:a2b784a0e760 1281 BelakangKanan.brake();
photonicbabe 0:a2b784a0e760 1282 BelakangKiri.brake();
photonicbabe 0:a2b784a0e760 1283 DepanKiri.brake();
photonicbabe 0:a2b784a0e760 1284 }
photonicbabe 0:a2b784a0e760 1285
photonicbabe 0:a2b784a0e760 1286 else if(Jalan == Muter_Kanan)
photonicbabe 0:a2b784a0e760 1287 {
photonicbabe 0:a2b784a0e760 1288 //DepanKananPID.setSetPoint(1400);
photonicbabe 0:a2b784a0e760 1289 BelakangKananPID.setSetPoint(1400);
photonicbabe 0:a2b784a0e760 1290 //BelakangKiriPID.setSetPoint(1400);
photonicbabe 0:a2b784a0e760 1291 DepanKiriPID.setSetPoint(1400);
photonicbabe 0:a2b784a0e760 1292 kosongkan();
photonicbabe 0:a2b784a0e760 1293 while(Jalan== Muter_Kanan)
photonicbabe 0:a2b784a0e760 1294 {
photonicbabe 0:a2b784a0e760 1295 t_jalan.reset();
photonicbabe 0:a2b784a0e760 1296 t_jalan.start();
photonicbabe 0:a2b784a0e760 1297 proses_kecepatan();
photonicbabe 0:a2b784a0e760 1298 Get_Count();
photonicbabe 0:a2b784a0e760 1299 Cari_Koordinat();
photonicbabe 0:a2b784a0e760 1300 //pc.printf("Koordinat X : %d\n", koordinat_x_bulat);
photonicbabe 0:a2b784a0e760 1301 //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
photonicbabe 0:a2b784a0e760 1302 pc.printf("Orientasi : %f\n", orientasi);
photonicbabe 0:a2b784a0e760 1303 BelakangKanan.speed(BelakangKananPID.compute()*-1);
photonicbabe 0:a2b784a0e760 1304 //BelakangKiri.speed(BelakangKiriPID.compute()*-1);
photonicbabe 0:a2b784a0e760 1305 //DepanKanan.speed(DepanKananPID.compute());
photonicbabe 0:a2b784a0e760 1306 DepanKiri.speed(DepanKiriPID.compute());
photonicbabe 0:a2b784a0e760 1307 while(t_jalan.read_ms()<=20)
photonicbabe 0:a2b784a0e760 1308 {
photonicbabe 0:a2b784a0e760 1309 }
photonicbabe 0:a2b784a0e760 1310 t_jalan.reset();
photonicbabe 0:a2b784a0e760 1311 }
photonicbabe 0:a2b784a0e760 1312 DepanKanan.brake();
photonicbabe 0:a2b784a0e760 1313 BelakangKanan.brake();
photonicbabe 0:a2b784a0e760 1314 BelakangKiri.brake();
photonicbabe 0:a2b784a0e760 1315 DepanKiri.brake();
photonicbabe 0:a2b784a0e760 1316 }
photonicbabe 0:a2b784a0e760 1317
photonicbabe 0:a2b784a0e760 1318 else if(Jalan == Muter_Kiri)
photonicbabe 0:a2b784a0e760 1319 {
photonicbabe 0:a2b784a0e760 1320 DepanKananPID.setSetPoint(1400);
photonicbabe 0:a2b784a0e760 1321 //BelakangKananPID.setSetPoint(1400);
photonicbabe 0:a2b784a0e760 1322 BelakangKiriPID.setSetPoint(1400);
photonicbabe 0:a2b784a0e760 1323 //DepanKiriPID.setSetPoint(1400);
photonicbabe 0:a2b784a0e760 1324 kosongkan();
photonicbabe 0:a2b784a0e760 1325 while(Jalan== Muter_Kiri)
photonicbabe 0:a2b784a0e760 1326 {
photonicbabe 0:a2b784a0e760 1327 t_jalan.reset();
photonicbabe 0:a2b784a0e760 1328 t_jalan.start();
photonicbabe 0:a2b784a0e760 1329 proses_kecepatan();
photonicbabe 0:a2b784a0e760 1330 Get_Count();
photonicbabe 0:a2b784a0e760 1331 Cari_Koordinat();
photonicbabe 0:a2b784a0e760 1332 //pc.printf("Koordinat X : %d\n", koordinat_x_bulat);
photonicbabe 0:a2b784a0e760 1333 //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
photonicbabe 0:a2b784a0e760 1334 pc.printf("Orientasi : %f\n", orientasi);
photonicbabe 0:a2b784a0e760 1335 //BelakangKanan.speed(BelakangKananPID.compute());
photonicbabe 0:a2b784a0e760 1336 BelakangKiri.speed(BelakangKiriPID.compute()*1);
photonicbabe 0:a2b784a0e760 1337 DepanKanan.speed(DepanKananPID.compute()*-1);
photonicbabe 0:a2b784a0e760 1338 //DepanKiri.speed(DepanKiriPID.compute()*-1);
photonicbabe 0:a2b784a0e760 1339 while(t_jalan.read_ms()<=20)
photonicbabe 0:a2b784a0e760 1340 {
photonicbabe 0:a2b784a0e760 1341 }
photonicbabe 0:a2b784a0e760 1342 t_jalan.reset();
photonicbabe 0:a2b784a0e760 1343 }
photonicbabe 0:a2b784a0e760 1344 DepanKanan.brake();
photonicbabe 0:a2b784a0e760 1345 BelakangKanan.brake();
photonicbabe 0:a2b784a0e760 1346 BelakangKiri.brake();
photonicbabe 0:a2b784a0e760 1347 DepanKiri.brake();
photonicbabe 0:a2b784a0e760 1348 }
photonicbabe 0:a2b784a0e760 1349
photonicbabe 0:a2b784a0e760 1350 else if(Jalan == Ganti_Kecepatan)
photonicbabe 0:a2b784a0e760 1351 {
photonicbabe 0:a2b784a0e760 1352 if(kecepatan_state==0)
photonicbabe 0:a2b784a0e760 1353 {
photonicbabe 0:a2b784a0e760 1354 mode_lambat();
photonicbabe 0:a2b784a0e760 1355 wait_ms(200);
photonicbabe 0:a2b784a0e760 1356 kecepatan_state =1;
photonicbabe 0:a2b784a0e760 1357 }
photonicbabe 0:a2b784a0e760 1358 else if(kecepatan_state==1)
photonicbabe 0:a2b784a0e760 1359 {
photonicbabe 0:a2b784a0e760 1360 mode_cepat();
photonicbabe 0:a2b784a0e760 1361 wait_ms(200);
photonicbabe 0:a2b784a0e760 1362 kecepatan_state = 0;
photonicbabe 0:a2b784a0e760 1363 }
photonicbabe 0:a2b784a0e760 1364 }
photonicbabe 0:a2b784a0e760 1365
photonicbabe 0:a2b784a0e760 1366 else if(Jalan == ke_setpoint)
photonicbabe 0:a2b784a0e760 1367 {
photonicbabe 0:a2b784a0e760 1368 DepanKananPID.setSetPoint(1400);
photonicbabe 0:a2b784a0e760 1369 BelakangKananPID.setSetPoint(1400);
photonicbabe 0:a2b784a0e760 1370 BelakangKiriPID.setSetPoint(1400);
photonicbabe 0:a2b784a0e760 1371 DepanKiriPID.setSetPoint(1400);
photonicbabe 0:a2b784a0e760 1372 //while(Jalan == ke_setpoint)
photonicbabe 0:a2b784a0e760 1373 //{
photonicbabe 0:a2b784a0e760 1374 //ke_setpoint_x();
photonicbabe 0:a2b784a0e760 1375 //ke_setpoint_y();
photonicbabe 0:a2b784a0e760 1376 ke_target_orientasi();
photonicbabe 0:a2b784a0e760 1377 //}
photonicbabe 0:a2b784a0e760 1378 }
photonicbabe 0:a2b784a0e760 1379 else if(Jalan == mode_inverse)
photonicbabe 0:a2b784a0e760 1380 {
photonicbabe 0:a2b784a0e760 1381 kosongkan();
photonicbabe 0:a2b784a0e760 1382 cari_inverse(orientasi, (float)setpoint_x, (float)setpoint_y);
photonicbabe 0:a2b784a0e760 1383 set_inverse();
photonicbabe 0:a2b784a0e760 1384 while(Jalan == mode_inverse)
photonicbabe 0:a2b784a0e760 1385 {
photonicbabe 0:a2b784a0e760 1386 if((koordinat_x_bulat!=setpoint_x)&&(koordinat_y_bulat!=setpoint_y))
photonicbabe 0:a2b784a0e760 1387 {
photonicbabe 0:a2b784a0e760 1388 t_jalan.reset();
photonicbabe 0:a2b784a0e760 1389 t_jalan.start();
photonicbabe 0:a2b784a0e760 1390 proses_kecepatan();
photonicbabe 0:a2b784a0e760 1391 Get_Count();
photonicbabe 0:a2b784a0e760 1392 Cari_Koordinat();
photonicbabe 0:a2b784a0e760 1393 if(arah_depan_kanan!=0)
photonicbabe 0:a2b784a0e760 1394 {
photonicbabe 0:a2b784a0e760 1395 if(arah_depan_kiri==0)
photonicbabe 0:a2b784a0e760 1396 {
photonicbabe 0:a2b784a0e760 1397 BelakangKiri.speed(BelakangKiriPID.compute()*arah_belakang_kiri);
photonicbabe 0:a2b784a0e760 1398 DepanKanan.speed(DepanKananPID.compute()*arah_depan_kanan);
photonicbabe 0:a2b784a0e760 1399 BelakangKanan.brake(0);
photonicbabe 0:a2b784a0e760 1400 DepanKiri.brake(0);
photonicbabe 0:a2b784a0e760 1401 }
photonicbabe 0:a2b784a0e760 1402 else if(arah_depan_kiri!=0)
photonicbabe 0:a2b784a0e760 1403 {
photonicbabe 0:a2b784a0e760 1404 BelakangKiri.speed(BelakangKiriPID.compute()*arah_belakang_kiri);
photonicbabe 0:a2b784a0e760 1405 DepanKanan.speed(DepanKananPID.compute()*arah_depan_kanan);
photonicbabe 0:a2b784a0e760 1406 BelakangKanan.speed(BelakangKananPID.compute()*arah_belakang_kanan);
photonicbabe 0:a2b784a0e760 1407 DepanKiri.speed(DepanKiriPID.compute()*arah_depan_kiri);
photonicbabe 0:a2b784a0e760 1408 }
photonicbabe 0:a2b784a0e760 1409 }
photonicbabe 0:a2b784a0e760 1410 else if(arah_depan_kiri!=0)
photonicbabe 0:a2b784a0e760 1411 {
photonicbabe 0:a2b784a0e760 1412 if(arah_depan_kanan==0)
photonicbabe 0:a2b784a0e760 1413 {
photonicbabe 0:a2b784a0e760 1414 BelakangKanan.speed(BelakangKananPID.compute()*arah_belakang_kanan);
photonicbabe 0:a2b784a0e760 1415 DepanKiri.speed(DepanKiriPID.compute()*arah_depan_kiri);
photonicbabe 0:a2b784a0e760 1416 BelakangKiri.brake(0);
photonicbabe 0:a2b784a0e760 1417 DepanKanan.brake(0);
photonicbabe 0:a2b784a0e760 1418 }
photonicbabe 0:a2b784a0e760 1419 else if(arah_depan_kanan!=0)
photonicbabe 0:a2b784a0e760 1420 {
photonicbabe 0:a2b784a0e760 1421 BelakangKiri.speed(BelakangKiriPID.compute()*arah_belakang_kiri);
photonicbabe 0:a2b784a0e760 1422 DepanKanan.speed(DepanKananPID.compute()*arah_depan_kanan);
photonicbabe 0:a2b784a0e760 1423 BelakangKanan.speed(BelakangKananPID.compute()*arah_belakang_kanan);
photonicbabe 0:a2b784a0e760 1424 DepanKiri.speed(DepanKiriPID.compute()*arah_depan_kiri);
photonicbabe 0:a2b784a0e760 1425 }
photonicbabe 0:a2b784a0e760 1426 }
photonicbabe 0:a2b784a0e760 1427 pc.printf("Koordinat X : %d\n", koordinat_x_bulat);
photonicbabe 0:a2b784a0e760 1428 pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
photonicbabe 0:a2b784a0e760 1429 pc.printf("Orientasi : %d\n", orientasi_2);
photonicbabe 0:a2b784a0e760 1430 //pc.printf("X basis : %f\n", target_x_basis);
photonicbabe 0:a2b784a0e760 1431 //pc.printf("Y basis : %f\n", target_y_basis);
photonicbabe 0:a2b784a0e760 1432 //pc.printf("Depan Kiri : %f\n", target_depan_kiri);
photonicbabe 0:a2b784a0e760 1433 //pc.printf("Depan Kanan : %f\n", target_depan_kanan);
photonicbabe 0:a2b784a0e760 1434 //pc.printf("Belakang Kanan : %f\n", target_belakang_kanan);
photonicbabe 0:a2b784a0e760 1435 //pc.printf("Belakang Kiri : %f\n", target_belakang_kiri);
photonicbabe 0:a2b784a0e760 1436 while(t_jalan.read_ms()<=20)
photonicbabe 0:a2b784a0e760 1437 {
photonicbabe 0:a2b784a0e760 1438 }
photonicbabe 0:a2b784a0e760 1439 t_jalan.reset();
photonicbabe 0:a2b784a0e760 1440 }
photonicbabe 0:a2b784a0e760 1441 }
photonicbabe 0:a2b784a0e760 1442 }
photonicbabe 0:a2b784a0e760 1443 else if(Jalan == Capit_Buka)
photonicbabe 0:a2b784a0e760 1444 {
photonicbabe 0:a2b784a0e760 1445 if(capit_state==0)
photonicbabe 0:a2b784a0e760 1446 {
photonicbabe 0:a2b784a0e760 1447 Servo.pulsewidth_us(1230);
photonicbabe 0:a2b784a0e760 1448 capit_state=1;
photonicbabe 0:a2b784a0e760 1449 wait_ms(200);
photonicbabe 0:a2b784a0e760 1450 }
photonicbabe 0:a2b784a0e760 1451 else if(capit_state==1)
photonicbabe 0:a2b784a0e760 1452 {
photonicbabe 0:a2b784a0e760 1453 Servo.pulsewidth_us(2400);
photonicbabe 0:a2b784a0e760 1454 capit_state=0;
photonicbabe 0:a2b784a0e760 1455 wait_ms(200);
photonicbabe 0:a2b784a0e760 1456 }
photonicbabe 0:a2b784a0e760 1457 /*else if(capit_state==2)
photonicbabe 0:a2b784a0e760 1458 {
photonicbabe 0:a2b784a0e760 1459 Servo.pulsewidth_us(1500);
photonicbabe 0:a2b784a0e760 1460 capit_state=0;
photonicbabe 0:a2b784a0e760 1461 wait_ms(200);
photonicbabe 0:a2b784a0e760 1462 }
photonicbabe 0:a2b784a0e760 1463 */
photonicbabe 0:a2b784a0e760 1464 }
photonicbabe 0:a2b784a0e760 1465 else if(Jalan == Capit_Muter)
photonicbabe 0:a2b784a0e760 1466 {
photonicbabe 0:a2b784a0e760 1467 if(capit2_state==0)
photonicbabe 0:a2b784a0e760 1468 {
photonicbabe 0:a2b784a0e760 1469 for(i=1800; i>910; i--)
photonicbabe 0:a2b784a0e760 1470 {
photonicbabe 0:a2b784a0e760 1471 t_capit.reset();
photonicbabe 0:a2b784a0e760 1472 t_capit.start();
photonicbabe 0:a2b784a0e760 1473 Servo2.pulsewidth_us(i);
photonicbabe 0:a2b784a0e760 1474 while(t_capit.read_ms()<1)
photonicbabe 0:a2b784a0e760 1475 {
photonicbabe 0:a2b784a0e760 1476 }
photonicbabe 0:a2b784a0e760 1477 t_capit.stop();
photonicbabe 0:a2b784a0e760 1478 }
photonicbabe 0:a2b784a0e760 1479 capit2_state=1;
photonicbabe 0:a2b784a0e760 1480 }
photonicbabe 0:a2b784a0e760 1481 else if(capit2_state==1)
photonicbabe 0:a2b784a0e760 1482 {
photonicbabe 0:a2b784a0e760 1483 for(i=910; i<1800; i=i++)
photonicbabe 0:a2b784a0e760 1484 {
photonicbabe 0:a2b784a0e760 1485 t_capit.reset();
photonicbabe 0:a2b784a0e760 1486 t_capit.start();
photonicbabe 0:a2b784a0e760 1487 Servo2.pulsewidth_us(i);
photonicbabe 0:a2b784a0e760 1488 while(t_capit.read_us()<600)
photonicbabe 0:a2b784a0e760 1489 {
photonicbabe 0:a2b784a0e760 1490 }
photonicbabe 0:a2b784a0e760 1491 t_capit.stop();
photonicbabe 0:a2b784a0e760 1492 }
photonicbabe 0:a2b784a0e760 1493 capit2_state=0;
photonicbabe 0:a2b784a0e760 1494 }
photonicbabe 0:a2b784a0e760 1495 }
photonicbabe 0:a2b784a0e760 1496 else if(Jalan == Cari_Koin1)
photonicbabe 0:a2b784a0e760 1497 {
photonicbabe 0:a2b784a0e760 1498 DepanKananPID.setSetPoint(1000);
photonicbabe 0:a2b784a0e760 1499 BelakangKananPID.setSetPoint(1000);
photonicbabe 0:a2b784a0e760 1500 BelakangKiriPID.setSetPoint(1000);
photonicbabe 0:a2b784a0e760 1501 DepanKiriPID.setSetPoint(1000);
photonicbabe 0:a2b784a0e760 1502 eksistensi_koin = ir.read();
photonicbabe 0:a2b784a0e760 1503 jarak = sensor();
photonicbabe 0:a2b784a0e760 1504 while(Jalan == Cari_Koin1)
photonicbabe 0:a2b784a0e760 1505 {
photonicbabe 0:a2b784a0e760 1506 //target_orientasi = 180;
photonicbabe 0:a2b784a0e760 1507 //ke_target_orientasi2();
photonicbabe 0:a2b784a0e760 1508 Servo.pulsewidth_us(2290);
photonicbabe 0:a2b784a0e760 1509 wait_ms(100);
photonicbabe 0:a2b784a0e760 1510 while(jarak>16.0f)
photonicbabe 0:a2b784a0e760 1511 {
photonicbabe 0:a2b784a0e760 1512 //jalan_maju();
photonicbabe 0:a2b784a0e760 1513 jarak = sensor();
photonicbabe 0:a2b784a0e760 1514 jalan_maju();
photonicbabe 0:a2b784a0e760 1515 }
photonicbabe 0:a2b784a0e760 1516 berhenti();
photonicbabe 0:a2b784a0e760 1517 wait_ms(300);
photonicbabe 0:a2b784a0e760 1518 while(eksistensi_koin== tidak_ada)
photonicbabe 0:a2b784a0e760 1519 {
photonicbabe 0:a2b784a0e760 1520 eksistensi_koin = ir.read();
photonicbabe 0:a2b784a0e760 1521 jalan_kiri();
photonicbabe 0:a2b784a0e760 1522 }
photonicbabe 0:a2b784a0e760 1523 berhenti();
photonicbabe 0:a2b784a0e760 1524 wait_ms(300);
photonicbabe 0:a2b784a0e760 1525 Servo.pulsewidth_us(1190);
photonicbabe 0:a2b784a0e760 1526 wait_ms(100);
photonicbabe 0:a2b784a0e760 1527 t2.reset();
photonicbabe 0:a2b784a0e760 1528 t2.start();
photonicbabe 0:a2b784a0e760 1529 while(t2.read_ms()<1500)
photonicbabe 0:a2b784a0e760 1530 {
photonicbabe 0:a2b784a0e760 1531 jalan_mundur();
photonicbabe 0:a2b784a0e760 1532 }
photonicbabe 0:a2b784a0e760 1533 t2.stop();
photonicbabe 0:a2b784a0e760 1534 //while(jarak<90)
photonicbabe 0:a2b784a0e760 1535 //{
photonicbabe 0:a2b784a0e760 1536 // jalan_mundur();
photonicbabe 0:a2b784a0e760 1537 // jarak = sensor();
photonicbabe 0:a2b784a0e760 1538 //}
photonicbabe 0:a2b784a0e760 1539 //Servo2.pulsewidth_us(2200);
photonicbabe 0:a2b784a0e760 1540 //target_orientasi = 0;
photonicbabe 0:a2b784a0e760 1541 //ke_target_orientasi2();
photonicbabe 0:a2b784a0e760 1542 }
photonicbabe 0:a2b784a0e760 1543 }
photonicbabe 0:a2b784a0e760 1544 else if(Jalan == Cari_Koin2)
photonicbabe 0:a2b784a0e760 1545 {
photonicbabe 0:a2b784a0e760 1546 while(Jalan == Cari_Koin2)
photonicbabe 0:a2b784a0e760 1547 {
photonicbabe 0:a2b784a0e760 1548 cari_koin2();
photonicbabe 0:a2b784a0e760 1549 }
photonicbabe 0:a2b784a0e760 1550 }
photonicbabe 0:a2b784a0e760 1551 else if(Jalan == Berhenti)
photonicbabe 0:a2b784a0e760 1552 {
photonicbabe 0:a2b784a0e760 1553 kosongkan();
photonicbabe 0:a2b784a0e760 1554 while(Jalan==Berhenti)
photonicbabe 0:a2b784a0e760 1555 {
photonicbabe 0:a2b784a0e760 1556 t_jalan.reset();
photonicbabe 0:a2b784a0e760 1557 t_jalan.start();
photonicbabe 0:a2b784a0e760 1558 proses_kecepatan();
photonicbabe 0:a2b784a0e760 1559 Get_Count();
photonicbabe 0:a2b784a0e760 1560 Cari_Koordinat();
photonicbabe 0:a2b784a0e760 1561 //pc.printf("Koordinat X : %d\n", koordinat_x_bulat);
photonicbabe 0:a2b784a0e760 1562 //pc.printf("Koordinat Y : %d\n", koordinat_y_bulat);
photonicbabe 0:a2b784a0e760 1563 pc.printf("Orientasi : %d\n", orientasi_2);
photonicbabe 0:a2b784a0e760 1564 DepanKanan.brake(0);
photonicbabe 0:a2b784a0e760 1565 BelakangKanan.brake(0);
photonicbabe 0:a2b784a0e760 1566 BelakangKiri.brake(0);
photonicbabe 0:a2b784a0e760 1567 DepanKiri.brake(0);
photonicbabe 0:a2b784a0e760 1568 while(t_jalan.read_ms()<=20)
photonicbabe 0:a2b784a0e760 1569 {
photonicbabe 0:a2b784a0e760 1570 }
photonicbabe 0:a2b784a0e760 1571 t_jalan.reset();
photonicbabe 0:a2b784a0e760 1572 }
photonicbabe 0:a2b784a0e760 1573 }
photonicbabe 0:a2b784a0e760 1574 }
photonicbabe 0:a2b784a0e760 1575 }