Robot Sulistia, Full Kodingan Lomba KRTMI 2022
Dependencies: MPU9250 Motor PID QEI SRF05 mbed
PIDremote.cpp@0:a2b784a0e760, 24 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?
User | Revision | Line number | New 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 | } |