KRAI 2017
Fix pisan inimah plis jangan revisi ultimate mantep
Dependencies: DigitDisplay Motor PID mbed millis
Fork of
DagonFly__RoadToJapan_15Mei
by 
KRAI 2017
main.cpp@45:964ae71a30e3, 2017-04-05 (annotated)
- Committer:
- gustavaditya
- Date:
- Wed Apr 05 16:04:08 2017 +0000
- Revision:
- 45:964ae71a30e3
- Parent:
- 44:452c214d9cf5
- Child:
- 46:85169ae8659b
Pakai akselerasi
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| Najib_irvani | 43:3807a95aa284 | 1 | |
| rahmadirizki18 | 5:3aa203218306 | 2 | /****************************************************************************/ |
| rahmadirizki18 | 5:3aa203218306 | 3 | /* PROGRAM UNTUK PID CLOSED LOOP */ |
| rahmadirizki18 | 5:3aa203218306 | 4 | /* */ |
| Najib_irvani | 43:3807a95aa284 | 5 | /* Last Update : 11 Maret 2017 */ |
| gustavaditya | 31:d5cbda07fd95 | 6 | /* */ |
| rahmadirizki18 | 5:3aa203218306 | 7 | /* - Digunakan encoder autonics */ |
| rahmadirizki18 | 5:3aa203218306 | 8 | /* - Konfigurasi Motor dan Encoder sbb : */ |
| MarchioKevin | 22:4632f58461e0 | 9 | /* ______________________ */ |
| MarchioKevin | 22:4632f58461e0 | 10 | /* / \ Rode Depan Belakang: */ |
| MarchioKevin | 22:4632f58461e0 | 11 | /* / 2 (Belakang) \ Omniwheel */ |
| MarchioKevin | 22:4632f58461e0 | 12 | /* | | */ |
| Najib_irvani | 43:3807a95aa284 | 13 | /* | 3 (kiri) 4 (kanan) | Roda Kiri Kanan: */ |
| Najib_irvani | 43:3807a95aa284 | 14 | /* | | Omniwheel */ |
| MarchioKevin | 22:4632f58461e0 | 15 | /* \ 1 (Depan) / */ |
| MarchioKevin | 22:4632f58461e0 | 16 | /* \______________________/ Putaran berlawanan arah */ |
| MarchioKevin | 22:4632f58461e0 | 17 | /* jarum jam positif */ |
| rahmadirizki18 | 5:3aa203218306 | 18 | /* SETTINGS (WAJIB!) : */ |
| rahmadirizki18 | 5:3aa203218306 | 19 | /* 1. Settings Pin Encoder, Resolusi, dan Tipe encoding di omniPos.h */ |
| rahmadirizki18 | 5:3aa203218306 | 20 | /* 2. Deklarasi penggunaan library omniPos pada bagian deklarasi encoder */ |
| rahmadirizki18 | 5:3aa203218306 | 21 | /* */ |
| rahmadirizki18 | 5:3aa203218306 | 22 | /****************************************************************************/ |
| rahmadirizki18 | 6:68293bed71ea | 23 | /* */ |
| MarchioKevin | 20:54dc93e7b016 | 24 | /* Joystick */ |
| Sufa | 30:d69cc27ac644 | 25 | /* Kanan => */ |
| Sufa | 30:d69cc27ac644 | 26 | /* Kiri => */ |
| rahmadirizki18 | 6:68293bed71ea | 27 | /* */ |
| Najib_irvani | 44:452c214d9cf5 | 28 | /* Tombol silang => Pneumatik aktif */ |
| Sufa | 30:d69cc27ac644 | 29 | /* Tombol segitiga => Aktif motor Launcher */ |
| Najib_irvani | 44:452c214d9cf5 | 30 | /* Tombol lingkaran => Reloader naik */ |
| Najib_irvani | 44:452c214d9cf5 | 31 | /* Tombol kotak => Reloader turun */ |
| Sufa | 30:d69cc27ac644 | 32 | /* Tombol L1 => Pivot Kiri */ |
| Sufa | 30:d69cc27ac644 | 33 | /* Tombol R1 => Pivot Kanan */ |
| Najib_irvani | 44:452c214d9cf5 | 34 | /* Tombol L2 => Kurang PWM Motor Launcher */ |
| Najib_irvani | 44:452c214d9cf5 | 35 | /* Tombol R2 => Tambah PWM Motor Launcher */ |
| rahmadirizki18 | 13:8ab42383a2ca | 36 | /* */ |
| calmantara186 | 16:90119f03c5d1 | 37 | /* Bismillahirahmanirrahim */ |
| MarchioKevin | 20:54dc93e7b016 | 38 | /* Jagalah Kebersihan Kodingan */ |
| rahmadirizki18 | 6:68293bed71ea | 39 | /****************************************************************************/ |
| rahmadirizki18 | 6:68293bed71ea | 40 | |
| fanny868 | 0:9072e932503c | 41 | #include "mbed.h" |
| fanny868 | 0:9072e932503c | 42 | #include "JoystickPS3.h" |
| fanny868 | 0:9072e932503c | 43 | #include "Motor.h" |
| rahmadirizki18 | 5:3aa203218306 | 44 | #include "encoderKRAI.h" |
| be_bryan | 26:256160a1a82d | 45 | #include "millis.h" |
| gustavaditya | 40:5b937cac959a | 46 | #include "Ping.h" |
| franshendri | 42:6caf8bd5abbc | 47 | #include "DigitDisplay.h" |
| franshendri | 42:6caf8bd5abbc | 48 | |
| calmantara186 | 16:90119f03c5d1 | 49 | /***********************************************/ |
| calmantara186 | 16:90119f03c5d1 | 50 | /* Konstanta dan Variabel */ |
| calmantara186 | 16:90119f03c5d1 | 51 | /***********************************************/ |
| calmantara186 | 16:90119f03c5d1 | 52 | #define PI 3.14159265 |
| be_bryan | 26:256160a1a82d | 53 | #define D_ENCODER 10 // Diameter Roda Encoder |
| be_bryan | 26:256160a1a82d | 54 | #define D_ROBOT 80 // Diameter Roda Robot |
| rahmadirizki18 | 5:3aa203218306 | 55 | |
| be_bryan | 26:256160a1a82d | 56 | // Variable Atas |
| gustavaditya | 38:3ef6754bd8d8 | 57 | double speed, speed2; |
| gustavaditya | 39:11358f3f61ff | 58 | const double maxSpeed = 0.95, minSpeed = 0.0; |
| Najib_irvani | 44:452c214d9cf5 | 59 | const double kpA=0.6757, kdA=0.7757, kiA=0.00003757; |
| be_bryan | 26:256160a1a82d | 60 | double p,i,d; |
| be_bryan | 27:68efb1622985 | 61 | double p2,i2,d2; |
| be_bryan | 26:256160a1a82d | 62 | double last_error = 0, current_error, sum_error = 0; |
| be_bryan | 27:68efb1622985 | 63 | double last_error2 = 0, current_error2, sum_error2 = 0; |
| gustavaditya | 38:3ef6754bd8d8 | 64 | float rpm, rpm2; |
| Najib_irvani | 44:452c214d9cf5 | 65 | float target_rpm = 17.0, target_rpm2 = 17.0; // selisih 4 bagus, sama bagus |
| gustavaditya | 41:336a19289c2d | 66 | const float maxRPM = 28, minRPM = 0; // Limit 25 atau 27 |
| gustavaditya | 38:3ef6754bd8d8 | 67 | |
| Najib_irvani | 44:452c214d9cf5 | 68 | const float pwmPowerUp = 1.0; |
| Najib_irvani | 44:452c214d9cf5 | 69 | const float pwmPowerDown = -1.0; |
| gustavaditya | 40:5b937cac959a | 70 | |
| gustavaditya | 40:5b937cac959a | 71 | float jarak_ping=0; |
| be_bryan | 26:256160a1a82d | 72 | |
| be_bryan | 26:256160a1a82d | 73 | // Variable Bawah |
| Joshua23 | 25:054d3048dd03 | 74 | float Vt; |
| gustavaditya | 31:d5cbda07fd95 | 75 | float keliling_enc = PI*D_ENCODER; |
| gustavaditya | 31:d5cbda07fd95 | 76 | float keliling_robot = PI*D_ROBOT; |
| gustavaditya | 31:d5cbda07fd95 | 77 | float speedT = 0.2; |
| Najib_irvani | 44:452c214d9cf5 | 78 | float PIVOT = 0.17; // PWM Pivot Kanan, Pivot Kiri |
| Najib_irvani | 44:452c214d9cf5 | 79 | float tuneDpn = 0.80; // Tunning PWM motor Depan |
| Najib_irvani | 44:452c214d9cf5 | 80 | float tuneBlk = 0.80; // Tunning PWM motor belakang |
| gustavaditya | 45:964ae71a30e3 | 81 | float tuneAksel = 0.6; |
| gustavaditya | 45:964ae71a30e3 | 82 | int aksel = 0; |
| gustavaditya | 45:964ae71a30e3 | 83 | float tuneAkselBlk = 0.4; |
| Najib_irvani | 44:452c214d9cf5 | 84 | float tuneR = 0.78; |
| Najib_irvani | 43:3807a95aa284 | 85 | float tuneL = 0.72; |
| Najib_irvani | 44:452c214d9cf5 | 86 | float serong = 0.4; |
| Najib_irvani | 43:3807a95aa284 | 87 | float rasio = 1.4545; |
| Najib_irvani | 43:3807a95aa284 | 88 | |
| Najib_irvani | 43:3807a95aa284 | 89 | /* variable tunning */ |
| Najib_irvani | 43:3807a95aa284 | 90 | float tunning_L; |
| Najib_irvani | 43:3807a95aa284 | 91 | float tunning_R; |
| Najib_irvani | 43:3807a95aa284 | 92 | float tunning_Dpn; |
| Najib_irvani | 43:3807a95aa284 | 93 | float tunning_Blk; |
| gustavaditya | 31:d5cbda07fd95 | 94 | |
| gustavaditya | 31:d5cbda07fd95 | 95 | /* Variabel Encoder Bawah */ |
| gustavaditya | 31:d5cbda07fd95 | 96 | float errTetha, Tetha; // Variabel yang didapatkan encoder |
| gustavaditya | 31:d5cbda07fd95 | 97 | |
| gustavaditya | 31:d5cbda07fd95 | 98 | /* Deklarasi Variable Millis */ |
| gustavaditya | 33:69d266bc3fe9 | 99 | unsigned long int previousMillis = 0; // PID launcher |
| gustavaditya | 31:d5cbda07fd95 | 100 | unsigned long int currentMillis; |
| gustavaditya | 33:69d266bc3fe9 | 101 | unsigned long int previousMillis2 = 0; // PID launcher |
| gustavaditya | 31:d5cbda07fd95 | 102 | unsigned long int currentMillis2; |
| gustavaditya | 33:69d266bc3fe9 | 103 | unsigned long int previousMillis3 = 0; // Pneumatik |
| gustavaditya | 40:5b937cac959a | 104 | unsigned long int previousMillis4 = 0; // Ping |
| franshendri | 42:6caf8bd5abbc | 105 | unsigned long int previousMillis5 = 0; // Display |
| MarchioKevin | 22:4632f58461e0 | 106 | |
| gustavaditya | 31:d5cbda07fd95 | 107 | /* Variabel Stick */ |
| gustavaditya | 31:d5cbda07fd95 | 108 | //Logic untuk masuk aktuator |
| gustavaditya | 31:d5cbda07fd95 | 109 | int case_joy; |
| gustavaditya | 31:d5cbda07fd95 | 110 | bool isLauncher = false; |
| gustavaditya | 31:d5cbda07fd95 | 111 | bool isReload = false; |
| rizqicahyo | 36:5963c9a49485 | 112 | bool ReloadOn = false; |
| gustavaditya | 31:d5cbda07fd95 | 113 | bool flag_Pneu = false; |
| Najib_irvani | 44:452c214d9cf5 | 114 | bool ready = false; |
| gustavaditya | 31:d5cbda07fd95 | 115 | |
| gustavaditya | 31:d5cbda07fd95 | 116 | /*****************************************************/ |
| gustavaditya | 31:d5cbda07fd95 | 117 | /* Definisi Prosedur, Fungsi dan Setting Pinout */ |
| gustavaditya | 31:d5cbda07fd95 | 118 | /*****************************************************/ |
| gustavaditya | 31:d5cbda07fd95 | 119 | |
| gustavaditya | 31:d5cbda07fd95 | 120 | /* Fungsi dan Procedur Encoder */ |
| gustavaditya | 31:d5cbda07fd95 | 121 | void init_speed(); // |
| gustavaditya | 31:d5cbda07fd95 | 122 | void aktuator(); // Pergerakan aktuator berdasarkan case joystick |
| gustavaditya | 31:d5cbda07fd95 | 123 | int case_joystick(); // Mendapatkan case dari joystick |
| gustavaditya | 33:69d266bc3fe9 | 124 | //void speedKalibrasiMotor(); // Pertambahan target RPM motor atas melalui joystick |
| gustavaditya | 31:d5cbda07fd95 | 125 | void setCenter(); // Prosedur reset encoder, posisi saat itu diset jadi titik (0,0) |
| gustavaditya | 31:d5cbda07fd95 | 126 | float getTetha(); // Fungsi mendapatkan error Tetha |
| gustavaditya | 31:d5cbda07fd95 | 127 | |
| gustavaditya | 31:d5cbda07fd95 | 128 | /* Inisialisasi Pin TX-RX Joystik dan PC */ |
| gustavaditya | 31:d5cbda07fd95 | 129 | joysticknucleo joystick(PA_0,PA_1); |
| gustavaditya | 31:d5cbda07fd95 | 130 | Serial pc(USBTX,USBRX); |
| calmantara186 | 16:90119f03c5d1 | 131 | |
| be_bryan | 26:256160a1a82d | 132 | /* Deklarasi Encoder Base */ |
| gustavaditya | 31:d5cbda07fd95 | 133 | encoderKRAI encoderBase(PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), 2xresolusi, mode pembacaan |
| rahmadirizki18 | 15:98f0d56b14f0 | 134 | |
| be_bryan | 26:256160a1a82d | 135 | /* Deklarasi Encoder Launcher */ |
| Najib_irvani | 44:452c214d9cf5 | 136 | encoderKRAI encLauncherDpn( PC_10, PC_11, 28, encoderKRAI::X4_ENCODING); |
| Najib_irvani | 44:452c214d9cf5 | 137 | encoderKRAI encLauncherBlk( PC_12, PD_2, 28, encoderKRAI::X4_ENCODING); |
| be_bryan | 26:256160a1a82d | 138 | |
| calmantara186 | 16:90119f03c5d1 | 139 | /* Deklarasi Motor Base */ |
| Najib_irvani | 44:452c214d9cf5 | 140 | Motor motorDpn(PB_7, PC_3, PC_0); //(PB_9, PA_12, PC_5); |
| Najib_irvani | 44:452c214d9cf5 | 141 | //Motor motorBlk(PB_6, PC_14, PC_13); //(PB_6, PB_1, PB_12); (PC_7, PC_14, PC_13); |
| Najib_irvani | 44:452c214d9cf5 | 142 | Motor motorBlk(PB_2, PB_15, PB_1); |
| Najib_irvani | 44:452c214d9cf5 | 143 | Motor motorL (PB_9, PA_12, PA_6); |
| Najib_irvani | 44:452c214d9cf5 | 144 | Motor motorR (PB_8, PC_5, PC_6); //(PC_6, PB_4, PB_5); |
| fanny868 | 0:9072e932503c | 145 | |
| calmantara186 | 16:90119f03c5d1 | 146 | /* Deklarasi Motor Launcher */ |
| Najib_irvani | 44:452c214d9cf5 | 147 | Motor launcherDpn(PA_5,PB_12,PA_11); // pwm ,fwd, rev |
| Najib_irvani | 44:452c214d9cf5 | 148 | Motor launcherBlk(PB_3, PC_4, PA_10); // pwm, fwd, rev |
| Najib_irvani | 44:452c214d9cf5 | 149 | Motor powerScrew(PB_10, PB_14, PB_13); // pwm, fwd, rev |
| rahmadirizki18 | 5:3aa203218306 | 150 | |
| be_bryan | 26:256160a1a82d | 151 | /* Deklarasi Penumatik Launcher */ |
| Najib_irvani | 44:452c214d9cf5 | 152 | DigitalOut pneumatik(PA_4, PullUp); |
| be_bryan | 26:256160a1a82d | 153 | |
| be_bryan | 27:68efb1622985 | 154 | /*Dekalrasi Limit Switch */ |
| gustavaditya | 40:5b937cac959a | 155 | //DigitalIn infraAtas(PC_9, PullUp); |
| Najib_irvani | 44:452c214d9cf5 | 156 | DigitalIn limitTengah(PA_9, PullUp); |
| Najib_irvani | 44:452c214d9cf5 | 157 | DigitalIn limitBawah(PC_7, PullUp); |
| Najib_irvani | 44:452c214d9cf5 | 158 | DigitalIn limitBawah1(PA_7, PullUp); |
| rahmadirizki18 | 5:3aa203218306 | 159 | |
| gustavaditya | 40:5b937cac959a | 160 | /*deklarasi PING ultrasonic*/ |
| Najib_irvani | 44:452c214d9cf5 | 161 | Ping pingAtas(PC_15); |
| fanny868 | 0:9072e932503c | 162 | |
| franshendri | 42:6caf8bd5abbc | 163 | /*Deklarasi Display*/ |
| Najib_irvani | 44:452c214d9cf5 | 164 | DigitDisplay display (PA_8, PC_8); |
| franshendri | 42:6caf8bd5abbc | 165 | |
| MarchioKevin | 22:4632f58461e0 | 166 | /****************************************************/ |
| MarchioKevin | 22:4632f58461e0 | 167 | /* Deklarasi Fungsi dan Procedure */ |
| MarchioKevin | 22:4632f58461e0 | 168 | /****************************************************/ |
| gustavaditya | 31:d5cbda07fd95 | 169 | int case_joystick() |
| gustavaditya | 31:d5cbda07fd95 | 170 | { |
| gustavaditya | 31:d5cbda07fd95 | 171 | //---------------------------------------------------// |
| gustavaditya | 31:d5cbda07fd95 | 172 | // Gerak Motor Base // |
| gustavaditya | 31:d5cbda07fd95 | 173 | // Case 1 : Pivot kanan // |
| gustavaditya | 31:d5cbda07fd95 | 174 | // Case 2 : Pivot Kiri // |
| gustavaditya | 31:d5cbda07fd95 | 175 | // Case 3 : Kanan // |
| gustavaditya | 31:d5cbda07fd95 | 176 | // Case 4 : Kiri // |
| gustavaditya | 31:d5cbda07fd95 | 177 | // Case 5 : Break // |
| gustavaditya | 31:d5cbda07fd95 | 178 | //---------------------------------------------------// |
| gustavaditya | 31:d5cbda07fd95 | 179 | |
| gustavaditya | 31:d5cbda07fd95 | 180 | int caseJoystick; |
| Najib_irvani | 43:3807a95aa284 | 181 | if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { |
| gustavaditya | 31:d5cbda07fd95 | 182 | // Pivot Kanan |
| gustavaditya | 31:d5cbda07fd95 | 183 | caseJoystick = 1; |
| gustavaditya | 31:d5cbda07fd95 | 184 | } |
| Najib_irvani | 43:3807a95aa284 | 185 | else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { |
| gustavaditya | 31:d5cbda07fd95 | 186 | // Pivot Kiri |
| gustavaditya | 31:d5cbda07fd95 | 187 | caseJoystick = 2; |
| Najib_irvani | 43:3807a95aa284 | 188 | } |
| Najib_irvani | 44:452c214d9cf5 | 189 | else if ((joystick.START_click)&&(!joystick.SELECT_click)&&(!joystick.R3_click)) { |
| Najib_irvani | 44:452c214d9cf5 | 190 | // tambah rpm dengan nilai tertentu |
| Najib_irvani | 44:452c214d9cf5 | 191 | caseJoystick = 31; |
| Najib_irvani | 44:452c214d9cf5 | 192 | } |
| Najib_irvani | 44:452c214d9cf5 | 193 | else if ((!joystick.START_click)&&(joystick.SELECT_click)&&(!joystick.R3_click)) { |
| Najib_irvani | 44:452c214d9cf5 | 194 | // kurangi rpm dengan nilai tertentu |
| Najib_irvani | 44:452c214d9cf5 | 195 | caseJoystick = 32; |
| Najib_irvani | 44:452c214d9cf5 | 196 | } |
| Najib_irvani | 44:452c214d9cf5 | 197 | else if ((!joystick.START_click)&&(!joystick.SELECT_click)&&(joystick.R3_click)) { |
| Najib_irvani | 44:452c214d9cf5 | 198 | // kurangi rpm dengan nilai tertentu |
| Najib_irvani | 44:452c214d9cf5 | 199 | caseJoystick = 33; |
| Najib_irvani | 44:452c214d9cf5 | 200 | } |
| Najib_irvani | 43:3807a95aa284 | 201 | else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) { |
| Najib_irvani | 43:3807a95aa284 | 202 | // Kanan + Rotasi kanan |
| Najib_irvani | 43:3807a95aa284 | 203 | caseJoystick = 17; |
| Najib_irvani | 43:3807a95aa284 | 204 | } |
| Najib_irvani | 43:3807a95aa284 | 205 | else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) { |
| Najib_irvani | 43:3807a95aa284 | 206 | // Kanan + Rotasi kiri |
| Najib_irvani | 43:3807a95aa284 | 207 | caseJoystick = 18; |
| Najib_irvani | 43:3807a95aa284 | 208 | } |
| Najib_irvani | 43:3807a95aa284 | 209 | else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { |
| Najib_irvani | 43:3807a95aa284 | 210 | // Kiri + Rotasi kanan |
| Najib_irvani | 43:3807a95aa284 | 211 | caseJoystick = 19; |
| Najib_irvani | 43:3807a95aa284 | 212 | } |
| Najib_irvani | 43:3807a95aa284 | 213 | else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { |
| Najib_irvani | 43:3807a95aa284 | 214 | // Kanan + Rotasi kiri |
| Najib_irvani | 43:3807a95aa284 | 215 | caseJoystick = 20; |
| Najib_irvani | 43:3807a95aa284 | 216 | } |
| Najib_irvani | 43:3807a95aa284 | 217 | else if ((joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { |
| Najib_irvani | 43:3807a95aa284 | 218 | // Maju + Rotasi kanan |
| Najib_irvani | 43:3807a95aa284 | 219 | caseJoystick = 21; |
| Najib_irvani | 43:3807a95aa284 | 220 | } |
| Najib_irvani | 43:3807a95aa284 | 221 | else if ((!joystick.R1)&&(joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { |
| Najib_irvani | 43:3807a95aa284 | 222 | // Maju + Rotasi kiri |
| Najib_irvani | 43:3807a95aa284 | 223 | caseJoystick = 22; |
| Najib_irvani | 43:3807a95aa284 | 224 | } |
| Najib_irvani | 43:3807a95aa284 | 225 | else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { |
| Najib_irvani | 43:3807a95aa284 | 226 | // Mundur + Rotasi kanan |
| Najib_irvani | 43:3807a95aa284 | 227 | caseJoystick = 23; |
| Najib_irvani | 43:3807a95aa284 | 228 | } |
| Najib_irvani | 43:3807a95aa284 | 229 | else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { |
| Najib_irvani | 43:3807a95aa284 | 230 | // Mundur + Rotasi kiri |
| Najib_irvani | 43:3807a95aa284 | 231 | caseJoystick = 24; |
| Najib_irvani | 43:3807a95aa284 | 232 | } |
| Najib_irvani | 43:3807a95aa284 | 233 | else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.segitiga_click)) { |
| Najib_irvani | 43:3807a95aa284 | 234 | // Kanan + segitiga |
| Najib_irvani | 43:3807a95aa284 | 235 | caseJoystick = 25; |
| Najib_irvani | 43:3807a95aa284 | 236 | } |
| Najib_irvani | 43:3807a95aa284 | 237 | else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.segitiga_click)) { |
| Najib_irvani | 43:3807a95aa284 | 238 | // Kiri + segitiga |
| Najib_irvani | 43:3807a95aa284 | 239 | caseJoystick = 26; |
| Najib_irvani | 43:3807a95aa284 | 240 | } |
| Najib_irvani | 43:3807a95aa284 | 241 | else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.lingkaran_click)) { |
| Najib_irvani | 43:3807a95aa284 | 242 | // Kanan + lingkaran |
| Najib_irvani | 43:3807a95aa284 | 243 | caseJoystick = 27; |
| Najib_irvani | 43:3807a95aa284 | 244 | } |
| Najib_irvani | 43:3807a95aa284 | 245 | else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.lingkaran_click)) { |
| Najib_irvani | 43:3807a95aa284 | 246 | // Kiri + lingkaran |
| Najib_irvani | 43:3807a95aa284 | 247 | caseJoystick = 28; |
| Najib_irvani | 43:3807a95aa284 | 248 | } |
| Najib_irvani | 43:3807a95aa284 | 249 | else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.kotak_click)) { |
| Najib_irvani | 43:3807a95aa284 | 250 | // Kanan + kotak |
| Najib_irvani | 43:3807a95aa284 | 251 | caseJoystick = 29; |
| Najib_irvani | 43:3807a95aa284 | 252 | } |
| Najib_irvani | 43:3807a95aa284 | 253 | else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.kotak_click)) { |
| Najib_irvani | 43:3807a95aa284 | 254 | // Kiri + kotak |
| Najib_irvani | 43:3807a95aa284 | 255 | caseJoystick = 30; |
| Najib_irvani | 43:3807a95aa284 | 256 | } |
| Najib_irvani | 43:3807a95aa284 | 257 | else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) { |
| Najib_irvani | 43:3807a95aa284 | 258 | // Serong kanan maju |
| Najib_irvani | 44:452c214d9cf5 | 259 | caseJoystick = 13; |
| Najib_irvani | 43:3807a95aa284 | 260 | } |
| Najib_irvani | 43:3807a95aa284 | 261 | else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { |
| Najib_irvani | 43:3807a95aa284 | 262 | // Serong kiri maju |
| Najib_irvani | 44:452c214d9cf5 | 263 | caseJoystick = 14; |
| Najib_irvani | 43:3807a95aa284 | 264 | } |
| Najib_irvani | 43:3807a95aa284 | 265 | else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) { |
| Najib_irvani | 43:3807a95aa284 | 266 | // Serong kanan mundur |
| Najib_irvani | 44:452c214d9cf5 | 267 | caseJoystick = 15; |
| Najib_irvani | 43:3807a95aa284 | 268 | } |
| Najib_irvani | 43:3807a95aa284 | 269 | else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { |
| Najib_irvani | 43:3807a95aa284 | 270 | // Serong kiri mundur |
| Najib_irvani | 44:452c214d9cf5 | 271 | caseJoystick = 16; |
| gustavaditya | 31:d5cbda07fd95 | 272 | } |
| gustavaditya | 31:d5cbda07fd95 | 273 | else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) { |
| gustavaditya | 31:d5cbda07fd95 | 274 | // Kanan |
| gustavaditya | 31:d5cbda07fd95 | 275 | caseJoystick = 3; |
| gustavaditya | 31:d5cbda07fd95 | 276 | } |
| gustavaditya | 31:d5cbda07fd95 | 277 | else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { |
| gustavaditya | 31:d5cbda07fd95 | 278 | // Kiri |
| gustavaditya | 31:d5cbda07fd95 | 279 | caseJoystick = 4; |
| Najib_irvani | 43:3807a95aa284 | 280 | } |
| Najib_irvani | 43:3807a95aa284 | 281 | else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { |
| Najib_irvani | 43:3807a95aa284 | 282 | // Atas -- Maju |
| Najib_irvani | 44:452c214d9cf5 | 283 | caseJoystick = 8; |
| Najib_irvani | 43:3807a95aa284 | 284 | } |
| Najib_irvani | 43:3807a95aa284 | 285 | else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { |
| Najib_irvani | 43:3807a95aa284 | 286 | // Bawah -- Mundur |
| Najib_irvani | 44:452c214d9cf5 | 287 | caseJoystick = 9; |
| gustavaditya | 31:d5cbda07fd95 | 288 | } |
| gustavaditya | 38:3ef6754bd8d8 | 289 | else if (joystick.segitiga_click){ |
| gustavaditya | 31:d5cbda07fd95 | 290 | // Motor Launcher |
| gustavaditya | 31:d5cbda07fd95 | 291 | caseJoystick = 5; |
| gustavaditya | 31:d5cbda07fd95 | 292 | } |
| gustavaditya | 38:3ef6754bd8d8 | 293 | else if (joystick.R2_click){ |
| gustavaditya | 31:d5cbda07fd95 | 294 | // Target Pulse PID ++ Motor Depan |
| gustavaditya | 31:d5cbda07fd95 | 295 | caseJoystick = 6; |
| gustavaditya | 31:d5cbda07fd95 | 296 | } |
| gustavaditya | 38:3ef6754bd8d8 | 297 | else if (joystick.L2_click){ |
| Najib_irvani | 43:3807a95aa284 | 298 | // Target Pulse PID -- Motor |
| gustavaditya | 31:d5cbda07fd95 | 299 | caseJoystick = 7; |
| gustavaditya | 31:d5cbda07fd95 | 300 | } |
| gustavaditya | 31:d5cbda07fd95 | 301 | else if (joystick.silang_click){ |
| gustavaditya | 31:d5cbda07fd95 | 302 | // Pnemuatik ON |
| gustavaditya | 31:d5cbda07fd95 | 303 | caseJoystick = 10; |
| gustavaditya | 31:d5cbda07fd95 | 304 | } |
| Najib_irvani | 43:3807a95aa284 | 305 | else if ((joystick.lingkaran_click)&&(!joystick.kotak_click)) { |
| gustavaditya | 31:d5cbda07fd95 | 306 | // Power Screw Up |
| gustavaditya | 31:d5cbda07fd95 | 307 | caseJoystick = 11; |
| gustavaditya | 31:d5cbda07fd95 | 308 | } |
| Najib_irvani | 43:3807a95aa284 | 309 | else if ((joystick.kotak_click)&&(!joystick.lingkaran_click)) { |
| gustavaditya | 31:d5cbda07fd95 | 310 | // Power Screw Down |
| gustavaditya | 31:d5cbda07fd95 | 311 | caseJoystick = 12; |
| gustavaditya | 31:d5cbda07fd95 | 312 | } |
| gustavaditya | 45:964ae71a30e3 | 313 | else |
| gustavaditya | 45:964ae71a30e3 | 314 | { |
| gustavaditya | 45:964ae71a30e3 | 315 | tuneAksel = 0.6; |
| gustavaditya | 45:964ae71a30e3 | 316 | aksel = 0; |
| gustavaditya | 45:964ae71a30e3 | 317 | } |
| gustavaditya | 31:d5cbda07fd95 | 318 | |
| gustavaditya | 31:d5cbda07fd95 | 319 | return(caseJoystick); |
| gustavaditya | 31:d5cbda07fd95 | 320 | } |
| gustavaditya | 31:d5cbda07fd95 | 321 | |
| gustavaditya | 31:d5cbda07fd95 | 322 | float getTetha(){ |
| gustavaditya | 31:d5cbda07fd95 | 323 | // Fungsi untuk mendapatkan nilai tetha |
| gustavaditya | 31:d5cbda07fd95 | 324 | float busur, tetha; |
| gustavaditya | 31:d5cbda07fd95 | 325 | busur = ((encoderBase.getPulses())/(float)(2000.0)*keliling_enc); |
| gustavaditya | 31:d5cbda07fd95 | 326 | tetha = busur/keliling_robot*360; |
| gustavaditya | 31:d5cbda07fd95 | 327 | |
| gustavaditya | 31:d5cbda07fd95 | 328 | return -(tetha); |
| gustavaditya | 31:d5cbda07fd95 | 329 | } |
| gustavaditya | 31:d5cbda07fd95 | 330 | |
| be_bryan | 26:256160a1a82d | 331 | float pidBase(float Kp, float Ki, float Kd) |
| Joshua23 | 25:054d3048dd03 | 332 | { |
| Joshua23 | 25:054d3048dd03 | 333 | int errorP; |
| Joshua23 | 25:054d3048dd03 | 334 | errorP = getTetha(); |
| gustavaditya | 33:69d266bc3fe9 | 335 | if (errorP<3.5 && errorP>(-3.5)) |
| gustavaditya | 33:69d266bc3fe9 | 336 | errorP = 0; |
| Joshua23 | 25:054d3048dd03 | 337 | return (float)Kp*errorP; |
| Joshua23 | 25:054d3048dd03 | 338 | } |
| gustavaditya | 31:d5cbda07fd95 | 339 | |
| gustavaditya | 31:d5cbda07fd95 | 340 | void setCenter(){ |
| gustavaditya | 31:d5cbda07fd95 | 341 | // Fungsi untuk menentukan center dari robot |
| gustavaditya | 31:d5cbda07fd95 | 342 | encoderBase.reset(); |
| fanny868 | 0:9072e932503c | 343 | } |
| fanny868 | 0:9072e932503c | 344 | |
| gustavaditya | 37:67d54563af90 | 345 | void init_rpm (){ |
| gustavaditya | 38:3ef6754bd8d8 | 346 | if (target_rpm>maxRPM-2){ |
| gustavaditya | 38:3ef6754bd8d8 | 347 | target_rpm = maxRPM-2; |
| gustavaditya | 37:67d54563af90 | 348 | } |
| franshendri | 42:6caf8bd5abbc | 349 | if (target_rpm<minRPM){ |
| gustavaditya | 37:67d54563af90 | 350 | target_rpm = minRPM; |
| gustavaditya | 37:67d54563af90 | 351 | } |
| gustavaditya | 37:67d54563af90 | 352 | if (target_rpm2>maxRPM){ |
| gustavaditya | 37:67d54563af90 | 353 | target_rpm2 = maxRPM; |
| gustavaditya | 37:67d54563af90 | 354 | } |
| franshendri | 42:6caf8bd5abbc | 355 | if (target_rpm2<minRPM+2){ |
| gustavaditya | 38:3ef6754bd8d8 | 356 | target_rpm2 = minRPM+2; |
| gustavaditya | 37:67d54563af90 | 357 | } |
| gustavaditya | 37:67d54563af90 | 358 | } |
| gustavaditya | 37:67d54563af90 | 359 | |
| gustavaditya | 31:d5cbda07fd95 | 360 | void aktuator() |
| gustavaditya | 31:d5cbda07fd95 | 361 | { |
| gustavaditya | 31:d5cbda07fd95 | 362 | switch (case_joy) { |
| gustavaditya | 31:d5cbda07fd95 | 363 | case (1): |
| gustavaditya | 31:d5cbda07fd95 | 364 | { |
| gustavaditya | 31:d5cbda07fd95 | 365 | // Pivot Kanan |
| gustavaditya | 31:d5cbda07fd95 | 366 | motorDpn.speed(-PIVOT); |
| gustavaditya | 31:d5cbda07fd95 | 367 | motorBlk.speed(-PIVOT); |
| Najib_irvani | 43:3807a95aa284 | 368 | motorR.speed(-rasio*PIVOT); |
| Najib_irvani | 43:3807a95aa284 | 369 | motorL.speed(-rasio*PIVOT); |
| gustavaditya | 31:d5cbda07fd95 | 370 | break; |
| gustavaditya | 31:d5cbda07fd95 | 371 | } |
| gustavaditya | 31:d5cbda07fd95 | 372 | case (2): |
| gustavaditya | 31:d5cbda07fd95 | 373 | { |
| gustavaditya | 31:d5cbda07fd95 | 374 | // Pivot Kiri |
| gustavaditya | 31:d5cbda07fd95 | 375 | motorDpn.speed(PIVOT); |
| gustavaditya | 31:d5cbda07fd95 | 376 | motorBlk.speed(PIVOT); |
| Najib_irvani | 43:3807a95aa284 | 377 | motorR.speed(rasio*PIVOT); |
| Najib_irvani | 43:3807a95aa284 | 378 | motorL.speed(rasio*PIVOT); |
| Najib_irvani | 43:3807a95aa284 | 379 | break; |
| Najib_irvani | 43:3807a95aa284 | 380 | } |
| Najib_irvani | 43:3807a95aa284 | 381 | case (17): |
| Najib_irvani | 43:3807a95aa284 | 382 | { |
| Najib_irvani | 43:3807a95aa284 | 383 | // Kanan + Rotasi Kanan |
| Najib_irvani | 43:3807a95aa284 | 384 | motorDpn.speed(-PIVOT); |
| Najib_irvani | 43:3807a95aa284 | 385 | motorBlk.speed(-PIVOT); |
| Najib_irvani | 43:3807a95aa284 | 386 | motorR.speed(-rasio*PIVOT); |
| Najib_irvani | 43:3807a95aa284 | 387 | motorL.speed(-rasio*PIVOT); |
| Najib_irvani | 43:3807a95aa284 | 388 | break; |
| Najib_irvani | 43:3807a95aa284 | 389 | } |
| Najib_irvani | 43:3807a95aa284 | 390 | case (18): |
| Najib_irvani | 43:3807a95aa284 | 391 | { |
| Najib_irvani | 43:3807a95aa284 | 392 | // Kanan + Rotasi Kiri |
| Najib_irvani | 43:3807a95aa284 | 393 | motorDpn.speed(PIVOT); |
| Najib_irvani | 43:3807a95aa284 | 394 | motorBlk.speed(PIVOT); |
| Najib_irvani | 43:3807a95aa284 | 395 | motorR.speed(rasio*PIVOT); |
| Najib_irvani | 43:3807a95aa284 | 396 | motorL.speed(rasio*PIVOT); |
| Najib_irvani | 43:3807a95aa284 | 397 | break; |
| Najib_irvani | 43:3807a95aa284 | 398 | } |
| Najib_irvani | 43:3807a95aa284 | 399 | case (19): |
| Najib_irvani | 43:3807a95aa284 | 400 | { |
| Najib_irvani | 43:3807a95aa284 | 401 | // Kiri + Rotasi Kanan |
| Najib_irvani | 43:3807a95aa284 | 402 | motorDpn.speed(-PIVOT); |
| Najib_irvani | 43:3807a95aa284 | 403 | motorBlk.speed(-PIVOT); |
| Najib_irvani | 43:3807a95aa284 | 404 | motorR.speed(-rasio*PIVOT); |
| Najib_irvani | 43:3807a95aa284 | 405 | motorL.speed(-rasio*PIVOT); |
| Najib_irvani | 43:3807a95aa284 | 406 | break; |
| Najib_irvani | 43:3807a95aa284 | 407 | } |
| Najib_irvani | 43:3807a95aa284 | 408 | case (20): |
| Najib_irvani | 43:3807a95aa284 | 409 | { |
| Najib_irvani | 43:3807a95aa284 | 410 | // Kiri + Rotasi Kiri |
| Najib_irvani | 43:3807a95aa284 | 411 | motorDpn.speed(PIVOT); |
| Najib_irvani | 43:3807a95aa284 | 412 | motorBlk.speed(PIVOT); |
| Najib_irvani | 43:3807a95aa284 | 413 | motorR.speed(rasio*PIVOT); |
| Najib_irvani | 43:3807a95aa284 | 414 | motorL.speed(rasio*PIVOT); |
| Najib_irvani | 43:3807a95aa284 | 415 | break; |
| Najib_irvani | 43:3807a95aa284 | 416 | } |
| Najib_irvani | 43:3807a95aa284 | 417 | case (21): |
| Najib_irvani | 43:3807a95aa284 | 418 | { |
| Najib_irvani | 43:3807a95aa284 | 419 | // Maju + Rotasi Kanan |
| Najib_irvani | 43:3807a95aa284 | 420 | motorDpn.speed(-PIVOT); |
| Najib_irvani | 43:3807a95aa284 | 421 | motorBlk.speed(-PIVOT); |
| Najib_irvani | 43:3807a95aa284 | 422 | motorR.speed(-rasio*PIVOT); |
| Najib_irvani | 43:3807a95aa284 | 423 | motorL.speed(-rasio*PIVOT); |
| Najib_irvani | 43:3807a95aa284 | 424 | break; |
| Najib_irvani | 43:3807a95aa284 | 425 | } |
| Najib_irvani | 43:3807a95aa284 | 426 | case (22): |
| Najib_irvani | 43:3807a95aa284 | 427 | { |
| Najib_irvani | 43:3807a95aa284 | 428 | // Maju + Rotasi Kiri |
| Najib_irvani | 43:3807a95aa284 | 429 | motorDpn.speed(PIVOT); |
| Najib_irvani | 43:3807a95aa284 | 430 | motorBlk.speed(PIVOT); |
| Najib_irvani | 43:3807a95aa284 | 431 | motorR.speed(rasio*PIVOT); |
| Najib_irvani | 43:3807a95aa284 | 432 | motorL.speed(rasio*PIVOT); |
| Najib_irvani | 43:3807a95aa284 | 433 | break; |
| Najib_irvani | 43:3807a95aa284 | 434 | } |
| Najib_irvani | 43:3807a95aa284 | 435 | case (23): |
| Najib_irvani | 43:3807a95aa284 | 436 | { |
| Najib_irvani | 43:3807a95aa284 | 437 | // Mundur + Rotasi Kanan |
| Najib_irvani | 43:3807a95aa284 | 438 | motorDpn.speed(-PIVOT); |
| Najib_irvani | 43:3807a95aa284 | 439 | motorBlk.speed(-PIVOT); |
| Najib_irvani | 43:3807a95aa284 | 440 | motorR.speed(-rasio*PIVOT); |
| Najib_irvani | 43:3807a95aa284 | 441 | motorL.speed(-rasio*PIVOT); |
| Najib_irvani | 43:3807a95aa284 | 442 | break; |
| Najib_irvani | 43:3807a95aa284 | 443 | } |
| Najib_irvani | 43:3807a95aa284 | 444 | case (24): |
| Najib_irvani | 43:3807a95aa284 | 445 | { |
| Najib_irvani | 43:3807a95aa284 | 446 | // Mundur + Rotasi Kiri |
| Najib_irvani | 43:3807a95aa284 | 447 | motorDpn.speed(PIVOT); |
| Najib_irvani | 43:3807a95aa284 | 448 | motorBlk.speed(PIVOT); |
| Najib_irvani | 43:3807a95aa284 | 449 | motorR.speed(rasio*PIVOT); |
| Najib_irvani | 43:3807a95aa284 | 450 | motorL.speed(rasio*PIVOT); |
| Najib_irvani | 43:3807a95aa284 | 451 | break; |
| Najib_irvani | 43:3807a95aa284 | 452 | } |
| Najib_irvani | 43:3807a95aa284 | 453 | case (25): |
| Najib_irvani | 43:3807a95aa284 | 454 | { |
| Najib_irvani | 43:3807a95aa284 | 455 | // Kanan + segitiga |
| Najib_irvani | 43:3807a95aa284 | 456 | isLauncher = !isLauncher; |
| Najib_irvani | 43:3807a95aa284 | 457 | break; |
| Najib_irvani | 43:3807a95aa284 | 458 | } |
| Najib_irvani | 43:3807a95aa284 | 459 | case (26): |
| Najib_irvani | 43:3807a95aa284 | 460 | { |
| Najib_irvani | 43:3807a95aa284 | 461 | // Kiri + segitiga |
| Najib_irvani | 43:3807a95aa284 | 462 | isLauncher = !isLauncher; |
| Najib_irvani | 43:3807a95aa284 | 463 | break; |
| Najib_irvani | 43:3807a95aa284 | 464 | } |
| Najib_irvani | 43:3807a95aa284 | 465 | case (27): |
| Najib_irvani | 43:3807a95aa284 | 466 | { |
| Najib_irvani | 43:3807a95aa284 | 467 | // Kanan + lingkaran |
| Najib_irvani | 43:3807a95aa284 | 468 | ReloadOn = !ReloadOn; |
| Najib_irvani | 43:3807a95aa284 | 469 | isReload = false; |
| Najib_irvani | 43:3807a95aa284 | 470 | break; |
| Najib_irvani | 43:3807a95aa284 | 471 | } |
| Najib_irvani | 43:3807a95aa284 | 472 | case (28): |
| Najib_irvani | 43:3807a95aa284 | 473 | { |
| Najib_irvani | 43:3807a95aa284 | 474 | // Kiri + lingkaran |
| Najib_irvani | 43:3807a95aa284 | 475 | ReloadOn = !ReloadOn; |
| Najib_irvani | 43:3807a95aa284 | 476 | isReload = false; |
| Najib_irvani | 43:3807a95aa284 | 477 | break; |
| Najib_irvani | 43:3807a95aa284 | 478 | } |
| Najib_irvani | 43:3807a95aa284 | 479 | case (29): |
| Najib_irvani | 43:3807a95aa284 | 480 | { |
| Najib_irvani | 43:3807a95aa284 | 481 | // Kanan + kotak |
| Najib_irvani | 43:3807a95aa284 | 482 | ReloadOn = !ReloadOn; |
| Najib_irvani | 43:3807a95aa284 | 483 | isReload = true; |
| Najib_irvani | 43:3807a95aa284 | 484 | break; |
| Najib_irvani | 43:3807a95aa284 | 485 | } |
| Najib_irvani | 43:3807a95aa284 | 486 | case (30): |
| Najib_irvani | 43:3807a95aa284 | 487 | { |
| Najib_irvani | 43:3807a95aa284 | 488 | // Kiri + kotak |
| Najib_irvani | 43:3807a95aa284 | 489 | ReloadOn = !ReloadOn; |
| Najib_irvani | 43:3807a95aa284 | 490 | isReload = true; |
| Najib_irvani | 43:3807a95aa284 | 491 | break; |
| Najib_irvani | 43:3807a95aa284 | 492 | } |
| Najib_irvani | 43:3807a95aa284 | 493 | case (13) : |
| Najib_irvani | 43:3807a95aa284 | 494 | { |
| Najib_irvani | 43:3807a95aa284 | 495 | // Serong kanan maju |
| Najib_irvani | 43:3807a95aa284 | 496 | motorDpn.speed(-serong); |
| Najib_irvani | 44:452c214d9cf5 | 497 | motorL.speed(-serong); |
| Najib_irvani | 43:3807a95aa284 | 498 | motorBlk.speed(serong); |
| Najib_irvani | 44:452c214d9cf5 | 499 | motorR.speed(serong); |
| Najib_irvani | 43:3807a95aa284 | 500 | break; |
| Najib_irvani | 43:3807a95aa284 | 501 | } |
| Najib_irvani | 43:3807a95aa284 | 502 | case (14) : |
| Najib_irvani | 43:3807a95aa284 | 503 | { |
| Najib_irvani | 43:3807a95aa284 | 504 | // Serong kiri maju |
| Najib_irvani | 43:3807a95aa284 | 505 | motorDpn.speed(serong); |
| Najib_irvani | 44:452c214d9cf5 | 506 | motorR.speed(serong); |
| Najib_irvani | 43:3807a95aa284 | 507 | motorBlk.speed(-serong); |
| Najib_irvani | 44:452c214d9cf5 | 508 | motorL.speed(-serong); |
| Najib_irvani | 43:3807a95aa284 | 509 | break; |
| Najib_irvani | 43:3807a95aa284 | 510 | } |
| Najib_irvani | 43:3807a95aa284 | 511 | case (15) : |
| Najib_irvani | 43:3807a95aa284 | 512 | { |
| Najib_irvani | 43:3807a95aa284 | 513 | // Serong kanan mundur |
| Najib_irvani | 44:452c214d9cf5 | 514 | motorDpn.speed(-serong); |
| Najib_irvani | 44:452c214d9cf5 | 515 | motorR.speed(-serong); |
| Najib_irvani | 43:3807a95aa284 | 516 | motorBlk.speed(serong); |
| Najib_irvani | 44:452c214d9cf5 | 517 | motorL.speed(serong); |
| Najib_irvani | 43:3807a95aa284 | 518 | break; |
| Najib_irvani | 43:3807a95aa284 | 519 | } |
| Najib_irvani | 43:3807a95aa284 | 520 | case (16) : |
| Najib_irvani | 43:3807a95aa284 | 521 | { |
| Najib_irvani | 43:3807a95aa284 | 522 | // Serong kiri mundur |
| Najib_irvani | 43:3807a95aa284 | 523 | motorDpn.speed(serong); |
| Najib_irvani | 44:452c214d9cf5 | 524 | motorL.speed(serong); |
| Najib_irvani | 43:3807a95aa284 | 525 | motorBlk.speed(-serong); |
| Najib_irvani | 44:452c214d9cf5 | 526 | motorR.speed(-serong); |
| gustavaditya | 31:d5cbda07fd95 | 527 | break; |
| gustavaditya | 31:d5cbda07fd95 | 528 | } |
| gustavaditya | 31:d5cbda07fd95 | 529 | case (3) : |
| gustavaditya | 31:d5cbda07fd95 | 530 | { |
| gustavaditya | 31:d5cbda07fd95 | 531 | // Kanan |
| gustavaditya | 45:964ae71a30e3 | 532 | aksel++; |
| gustavaditya | 45:964ae71a30e3 | 533 | if (aksel>300) |
| gustavaditya | 45:964ae71a30e3 | 534 | tuneAksel = 0.9; |
| gustavaditya | 45:964ae71a30e3 | 535 | |
| gustavaditya | 45:964ae71a30e3 | 536 | motorDpn.speed(-tuneAksel); |
| gustavaditya | 45:964ae71a30e3 | 537 | motorBlk.speed(tuneAksel); |
| Najib_irvani | 43:3807a95aa284 | 538 | motorR.brake(1); |
| Najib_irvani | 43:3807a95aa284 | 539 | motorL.brake(1); |
| gustavaditya | 31:d5cbda07fd95 | 540 | break; |
| gustavaditya | 31:d5cbda07fd95 | 541 | } |
| gustavaditya | 31:d5cbda07fd95 | 542 | case (4) : |
| gustavaditya | 31:d5cbda07fd95 | 543 | { |
| gustavaditya | 31:d5cbda07fd95 | 544 | // Kiri |
| gustavaditya | 45:964ae71a30e3 | 545 | aksel++; |
| gustavaditya | 45:964ae71a30e3 | 546 | if (aksel>300) |
| gustavaditya | 45:964ae71a30e3 | 547 | tuneAksel = 0.9; |
| gustavaditya | 45:964ae71a30e3 | 548 | |
| gustavaditya | 45:964ae71a30e3 | 549 | motorDpn.speed(tuneAksel); |
| gustavaditya | 45:964ae71a30e3 | 550 | motorBlk.speed(-tuneAksel); |
| Najib_irvani | 43:3807a95aa284 | 551 | motorR.brake(1); |
| Najib_irvani | 43:3807a95aa284 | 552 | motorL.brake(1); |
| Najib_irvani | 43:3807a95aa284 | 553 | break; |
| Najib_irvani | 43:3807a95aa284 | 554 | } |
| Najib_irvani | 43:3807a95aa284 | 555 | case (8) : |
| Najib_irvani | 43:3807a95aa284 | 556 | { |
| Najib_irvani | 43:3807a95aa284 | 557 | // Maju |
| gustavaditya | 45:964ae71a30e3 | 558 | aksel++; |
| gustavaditya | 45:964ae71a30e3 | 559 | if (aksel>300) |
| gustavaditya | 45:964ae71a30e3 | 560 | tuneAksel = 0.9; |
| gustavaditya | 45:964ae71a30e3 | 561 | |
| gustavaditya | 45:964ae71a30e3 | 562 | motorR.speed(tuneAksel); |
| gustavaditya | 45:964ae71a30e3 | 563 | motorL.speed(-tuneAksel); |
| Najib_irvani | 43:3807a95aa284 | 564 | motorDpn.brake(1); |
| Najib_irvani | 43:3807a95aa284 | 565 | motorBlk.brake(1); |
| Najib_irvani | 43:3807a95aa284 | 566 | break; |
| Najib_irvani | 43:3807a95aa284 | 567 | } |
| Najib_irvani | 43:3807a95aa284 | 568 | case (9) : |
| Najib_irvani | 43:3807a95aa284 | 569 | { |
| Najib_irvani | 43:3807a95aa284 | 570 | // Mundur |
| gustavaditya | 45:964ae71a30e3 | 571 | aksel++; |
| gustavaditya | 45:964ae71a30e3 | 572 | if (aksel>300) |
| gustavaditya | 45:964ae71a30e3 | 573 | tuneAksel = 0.9; |
| gustavaditya | 45:964ae71a30e3 | 574 | |
| gustavaditya | 45:964ae71a30e3 | 575 | motorR.speed(-tuneAksel); |
| gustavaditya | 45:964ae71a30e3 | 576 | motorL.speed(tuneAksel); |
| Najib_irvani | 43:3807a95aa284 | 577 | motorDpn.brake(1); |
| Najib_irvani | 43:3807a95aa284 | 578 | motorBlk.brake(1); |
| gustavaditya | 31:d5cbda07fd95 | 579 | break; |
| gustavaditya | 31:d5cbda07fd95 | 580 | } |
| gustavaditya | 31:d5cbda07fd95 | 581 | case (5) : |
| gustavaditya | 31:d5cbda07fd95 | 582 | { |
| gustavaditya | 31:d5cbda07fd95 | 583 | // Aktifkan motor atas |
| gustavaditya | 31:d5cbda07fd95 | 584 | isLauncher = !isLauncher; |
| gustavaditya | 31:d5cbda07fd95 | 585 | break; |
| Joshua23 | 8:0711dea61312 | 586 | } |
| gustavaditya | 31:d5cbda07fd95 | 587 | case (6) : |
| gustavaditya | 31:d5cbda07fd95 | 588 | { |
| gustavaditya | 31:d5cbda07fd95 | 589 | // Target Pulse PID ++ Motor Depan |
| gustavaditya | 39:11358f3f61ff | 590 | target_rpm2 = target_rpm2+1.0; |
| gustavaditya | 39:11358f3f61ff | 591 | target_rpm = target_rpm+1.0; |
| gustavaditya | 37:67d54563af90 | 592 | init_rpm(); |
| gustavaditya | 31:d5cbda07fd95 | 593 | break; |
| gustavaditya | 31:d5cbda07fd95 | 594 | } |
| gustavaditya | 31:d5cbda07fd95 | 595 | case (7) : |
| gustavaditya | 31:d5cbda07fd95 | 596 | { |
| gustavaditya | 31:d5cbda07fd95 | 597 | // Target Pulse PID -- Motor Depan |
| gustavaditya | 39:11358f3f61ff | 598 | target_rpm2 = target_rpm2-1.0; |
| gustavaditya | 39:11358f3f61ff | 599 | target_rpm = target_rpm-1.0; |
| gustavaditya | 37:67d54563af90 | 600 | init_rpm(); |
| gustavaditya | 31:d5cbda07fd95 | 601 | break; |
| gustavaditya | 31:d5cbda07fd95 | 602 | } |
| gustavaditya | 31:d5cbda07fd95 | 603 | case (10) : |
| gustavaditya | 31:d5cbda07fd95 | 604 | { |
| gustavaditya | 31:d5cbda07fd95 | 605 | // Pneumatik |
| Najib_irvani | 44:452c214d9cf5 | 606 | if (ready) |
| Najib_irvani | 44:452c214d9cf5 | 607 | { |
| Najib_irvani | 44:452c214d9cf5 | 608 | pneumatik = 0; |
| Najib_irvani | 44:452c214d9cf5 | 609 | previousMillis3 = millis(); |
| Najib_irvani | 44:452c214d9cf5 | 610 | flag_Pneu = true; |
| Najib_irvani | 44:452c214d9cf5 | 611 | ready = false; |
| gustavaditya | 45:964ae71a30e3 | 612 | |
| Najib_irvani | 44:452c214d9cf5 | 613 | } |
| gustavaditya | 31:d5cbda07fd95 | 614 | break; |
| gustavaditya | 31:d5cbda07fd95 | 615 | } |
| gustavaditya | 31:d5cbda07fd95 | 616 | case (11) : |
| gustavaditya | 31:d5cbda07fd95 | 617 | { |
| gustavaditya | 31:d5cbda07fd95 | 618 | // Power Screw Up |
| gustavaditya | 40:5b937cac959a | 619 | ReloadOn = !ReloadOn; |
| Najib_irvani | 43:3807a95aa284 | 620 | isReload = false; |
| gustavaditya | 31:d5cbda07fd95 | 621 | break; |
| gustavaditya | 31:d5cbda07fd95 | 622 | } |
| Najib_irvani | 44:452c214d9cf5 | 623 | case (31) : |
| Najib_irvani | 44:452c214d9cf5 | 624 | { |
| Najib_irvani | 44:452c214d9cf5 | 625 | // start |
| Najib_irvani | 44:452c214d9cf5 | 626 | target_rpm2 = 22; |
| Najib_irvani | 44:452c214d9cf5 | 627 | target_rpm = 22; |
| Najib_irvani | 44:452c214d9cf5 | 628 | init_rpm(); |
| Najib_irvani | 44:452c214d9cf5 | 629 | break; |
| Najib_irvani | 44:452c214d9cf5 | 630 | } |
| Najib_irvani | 44:452c214d9cf5 | 631 | case (32) : |
| Najib_irvani | 44:452c214d9cf5 | 632 | { |
| Najib_irvani | 44:452c214d9cf5 | 633 | // select |
| Najib_irvani | 44:452c214d9cf5 | 634 | target_rpm2 = 10; |
| Najib_irvani | 44:452c214d9cf5 | 635 | target_rpm = 10; |
| Najib_irvani | 44:452c214d9cf5 | 636 | init_rpm(); |
| Najib_irvani | 44:452c214d9cf5 | 637 | break; |
| Najib_irvani | 44:452c214d9cf5 | 638 | } |
| Najib_irvani | 44:452c214d9cf5 | 639 | case (33) : |
| Najib_irvani | 44:452c214d9cf5 | 640 | { |
| Najib_irvani | 44:452c214d9cf5 | 641 | // R3 |
| Najib_irvani | 44:452c214d9cf5 | 642 | target_rpm2 = 17; |
| Najib_irvani | 44:452c214d9cf5 | 643 | target_rpm = 17; |
| Najib_irvani | 44:452c214d9cf5 | 644 | init_rpm(); |
| Najib_irvani | 44:452c214d9cf5 | 645 | break; |
| Najib_irvani | 44:452c214d9cf5 | 646 | } |
| gustavaditya | 31:d5cbda07fd95 | 647 | case (12) : |
| gustavaditya | 31:d5cbda07fd95 | 648 | { |
| gustavaditya | 31:d5cbda07fd95 | 649 | // Power Screw Down |
| Najib_irvani | 43:3807a95aa284 | 650 | ReloadOn = !ReloadOn; |
| Najib_irvani | 43:3807a95aa284 | 651 | isReload = true; |
| gustavaditya | 31:d5cbda07fd95 | 652 | break; |
| gustavaditya | 31:d5cbda07fd95 | 653 | } |
| gustavaditya | 31:d5cbda07fd95 | 654 | default : |
| gustavaditya | 31:d5cbda07fd95 | 655 | { |
| gustavaditya | 45:964ae71a30e3 | 656 | tuneAksel = 0.6; |
| gustavaditya | 45:964ae71a30e3 | 657 | aksel = 0; |
| gustavaditya | 31:d5cbda07fd95 | 658 | motorDpn.brake(1); |
| gustavaditya | 31:d5cbda07fd95 | 659 | motorBlk.brake(1); |
| Najib_irvani | 43:3807a95aa284 | 660 | motorR.brake(1); |
| Najib_irvani | 43:3807a95aa284 | 661 | motorL.brake(1); |
| gustavaditya | 31:d5cbda07fd95 | 662 | } |
| gustavaditya | 31:d5cbda07fd95 | 663 | } // End Switch |
| gustavaditya | 31:d5cbda07fd95 | 664 | } |
| gustavaditya | 31:d5cbda07fd95 | 665 | |
| gustavaditya | 40:5b937cac959a | 666 | void reloader() |
| rizqicahyo | 36:5963c9a49485 | 667 | { |
| rizqicahyo | 36:5963c9a49485 | 668 | if(ReloadOn){ |
| rizqicahyo | 36:5963c9a49485 | 669 | if(isReload){ |
| rizqicahyo | 36:5963c9a49485 | 670 | powerScrew.speed(pwmPowerDown); |
| Najib_irvani | 44:452c214d9cf5 | 671 | if((!limitBawah)||(!limitBawah1)){ |
| rizqicahyo | 36:5963c9a49485 | 672 | isReload = false; |
| rizqicahyo | 36:5963c9a49485 | 673 | ReloadOn = false; |
| rizqicahyo | 36:5963c9a49485 | 674 | } |
| rizqicahyo | 36:5963c9a49485 | 675 | } |
| rizqicahyo | 36:5963c9a49485 | 676 | else if(!limitTengah){ |
| rizqicahyo | 36:5963c9a49485 | 677 | isReload = true; |
| rizqicahyo | 36:5963c9a49485 | 678 | } |
| Najib_irvani | 43:3807a95aa284 | 679 | else if((jarak_ping > 6.5) && !flag_Pneu){ |
| gustavaditya | 40:5b937cac959a | 680 | powerScrew.speed(pwmPowerUp); |
| Najib_irvani | 44:452c214d9cf5 | 681 | ready = false; |
| gustavaditya | 40:5b937cac959a | 682 | } |
| Najib_irvani | 44:452c214d9cf5 | 683 | else if((jarak_ping < 6.0) && !flag_Pneu) { |
| Najib_irvani | 44:452c214d9cf5 | 684 | powerScrew.speed(-0.85); |
| Najib_irvani | 44:452c214d9cf5 | 685 | ready = false; |
| rizqicahyo | 36:5963c9a49485 | 686 | } |
| rizqicahyo | 36:5963c9a49485 | 687 | else{ |
| gustavaditya | 40:5b937cac959a | 688 | powerScrew.brake(1); |
| Najib_irvani | 44:452c214d9cf5 | 689 | ready = true; |
| rizqicahyo | 36:5963c9a49485 | 690 | } |
| rizqicahyo | 36:5963c9a49485 | 691 | } |
| rizqicahyo | 36:5963c9a49485 | 692 | else{ |
| rizqicahyo | 36:5963c9a49485 | 693 | powerScrew.brake(1); |
| rizqicahyo | 36:5963c9a49485 | 694 | } |
| gustavaditya | 40:5b937cac959a | 695 | } |
| rizqicahyo | 36:5963c9a49485 | 696 | |
| rizqicahyo | 36:5963c9a49485 | 697 | |
| gustavaditya | 31:d5cbda07fd95 | 698 | void launcher() |
| gustavaditya | 31:d5cbda07fd95 | 699 | { |
| gustavaditya | 31:d5cbda07fd95 | 700 | if (isLauncher) |
| Sufa | 30:d69cc27ac644 | 701 | { |
| be_bryan | 28:2d0746dc2d7d | 702 | currentMillis = millis(); |
| be_bryan | 28:2d0746dc2d7d | 703 | currentMillis2 = millis(); |
| be_bryan | 28:2d0746dc2d7d | 704 | |
| gustavaditya | 31:d5cbda07fd95 | 705 | // PID Launcher Depan |
| gustavaditya | 38:3ef6754bd8d8 | 706 | if (currentMillis-previousMillis>=12.5) |
| be_bryan | 26:256160a1a82d | 707 | { |
| gustavaditya | 31:d5cbda07fd95 | 708 | rpm = (float)encLauncherBlk.getPulses(); |
| be_bryan | 26:256160a1a82d | 709 | current_error = target_rpm - rpm; |
| Najib_irvani | 44:452c214d9cf5 | 710 | sum_error = sum_error + current_error*12.5; |
| be_bryan | 26:256160a1a82d | 711 | p = current_error*kpA; |
| gustavaditya | 38:3ef6754bd8d8 | 712 | d = (current_error-last_error)*kdA/12.5; |
| Najib_irvani | 44:452c214d9cf5 | 713 | i = sum_error*kiA; |
| be_bryan | 26:256160a1a82d | 714 | speed = p + d + i; |
| gustavaditya | 38:3ef6754bd8d8 | 715 | //init_speed(); |
| gustavaditya | 38:3ef6754bd8d8 | 716 | if(speed > maxSpeed){ |
| gustavaditya | 38:3ef6754bd8d8 | 717 | launcherBlk.speed(maxSpeed); |
| gustavaditya | 38:3ef6754bd8d8 | 718 | } |
| gustavaditya | 39:11358f3f61ff | 719 | else if ( speed < minSpeed){ |
| gustavaditya | 39:11358f3f61ff | 720 | launcherBlk.speed(minSpeed); |
| gustavaditya | 39:11358f3f61ff | 721 | } |
| gustavaditya | 38:3ef6754bd8d8 | 722 | else { |
| gustavaditya | 38:3ef6754bd8d8 | 723 | launcherBlk.speed(speed); |
| gustavaditya | 38:3ef6754bd8d8 | 724 | } |
| be_bryan | 26:256160a1a82d | 725 | last_error = current_error; |
| gustavaditya | 31:d5cbda07fd95 | 726 | encLauncherBlk.reset(); |
| be_bryan | 26:256160a1a82d | 727 | //pc.printf("%.04lf\n",rpm); |
| be_bryan | 26:256160a1a82d | 728 | previousMillis = currentMillis; |
| be_bryan | 26:256160a1a82d | 729 | } |
| gustavaditya | 38:3ef6754bd8d8 | 730 | if (currentMillis2-previousMillis2>=12.5) |
| be_bryan | 27:68efb1622985 | 731 | { |
| gustavaditya | 31:d5cbda07fd95 | 732 | rpm2 = (float)encLauncherDpn.getPulses(); |
| be_bryan | 27:68efb1622985 | 733 | current_error2 = target_rpm2 - rpm2; |
| Najib_irvani | 44:452c214d9cf5 | 734 | sum_error2 = sum_error2 + current_error2*12.5; |
| be_bryan | 27:68efb1622985 | 735 | p2 = current_error2*kpA; |
| gustavaditya | 38:3ef6754bd8d8 | 736 | d2 = (current_error2-last_error2)*kdA/12.5; |
| Najib_irvani | 44:452c214d9cf5 | 737 | i2 = sum_error2*kiA; |
| be_bryan | 27:68efb1622985 | 738 | speed2 = p2 + d2 + i2; |
| gustavaditya | 38:3ef6754bd8d8 | 739 | //init_speed(); |
| gustavaditya | 38:3ef6754bd8d8 | 740 | if (speed2 > maxSpeed){ |
| gustavaditya | 38:3ef6754bd8d8 | 741 | launcherDpn.speed(maxSpeed); |
| gustavaditya | 38:3ef6754bd8d8 | 742 | } |
| gustavaditya | 39:11358f3f61ff | 743 | else if ( speed < minSpeed){ |
| gustavaditya | 39:11358f3f61ff | 744 | launcherDpn.speed(minSpeed); |
| gustavaditya | 39:11358f3f61ff | 745 | } |
| gustavaditya | 38:3ef6754bd8d8 | 746 | else{ |
| gustavaditya | 38:3ef6754bd8d8 | 747 | launcherDpn.speed(speed2); |
| gustavaditya | 38:3ef6754bd8d8 | 748 | } |
| be_bryan | 27:68efb1622985 | 749 | last_error2 = current_error2; |
| gustavaditya | 31:d5cbda07fd95 | 750 | encLauncherDpn.reset(); |
| be_bryan | 27:68efb1622985 | 751 | previousMillis2 = currentMillis2; |
| be_bryan | 27:68efb1622985 | 752 | } |
| Najib_irvani | 44:452c214d9cf5 | 753 | pc.printf("%.2f\t%.2f\n",rpm,rpm2); |
| rahmadirizki18 | 6:68293bed71ea | 754 | } |
| Sufa | 29:7b372b0aaa61 | 755 | else |
| Sufa | 29:7b372b0aaa61 | 756 | { |
| gustavaditya | 31:d5cbda07fd95 | 757 | launcherDpn.brake(1); |
| gustavaditya | 31:d5cbda07fd95 | 758 | launcherBlk.brake(1); |
| Najib_irvani | 44:452c214d9cf5 | 759 | sum_error = 0; |
| Najib_irvani | 44:452c214d9cf5 | 760 | sum_error2 = 0; |
| Najib_irvani | 44:452c214d9cf5 | 761 | current_error = 0; |
| Najib_irvani | 44:452c214d9cf5 | 762 | current_error2 = 0; |
| Najib_irvani | 44:452c214d9cf5 | 763 | last_error = 0; |
| Najib_irvani | 44:452c214d9cf5 | 764 | last_error2 = 0; |
| gustavaditya | 31:d5cbda07fd95 | 765 | } |
| rahmadirizki18 | 5:3aa203218306 | 766 | } |
| gustavaditya | 31:d5cbda07fd95 | 767 | |
| MarchioKevin | 22:4632f58461e0 | 768 | /*********************************************************/ |
| MarchioKevin | 22:4632f58461e0 | 769 | /* Main Function */ |
| MarchioKevin | 22:4632f58461e0 | 770 | /*********************************************************/ |
| calmantara186 | 16:90119f03c5d1 | 771 | |
| gustavaditya | 31:d5cbda07fd95 | 772 | int main (void) |
| gustavaditya | 31:d5cbda07fd95 | 773 | { |
| gustavaditya | 31:d5cbda07fd95 | 774 | // Set baud rate - 115200 |
| fanny868 | 0:9072e932503c | 775 | joystick.setup(); |
| rahmadirizki18 | 23:023b522977b2 | 776 | pc.baud(115200); |
| rahmadirizki18 | 6:68293bed71ea | 777 | wait_ms(1000); |
| gustavaditya | 40:5b937cac959a | 778 | |
| gustavaditya | 40:5b937cac959a | 779 | // initializing encoder |
| gustavaditya | 41:336a19289c2d | 780 | pneumatik =1; |
| gustavaditya | 41:336a19289c2d | 781 | |
| rahmadirizki18 | 5:3aa203218306 | 782 | setCenter(); |
| gustavaditya | 41:336a19289c2d | 783 | |
| rahmadirizki18 | 5:3aa203218306 | 784 | wait_ms(500); |
| gustavaditya | 40:5b937cac959a | 785 | |
| gustavaditya | 40:5b937cac959a | 786 | //initializing PING |
| gustavaditya | 40:5b937cac959a | 787 | pingAtas.Send(); |
| gustavaditya | 40:5b937cac959a | 788 | |
| rahmadirizki18 | 23:023b522977b2 | 789 | pc.printf("ready...."); |
| gustavaditya | 35:69a47b4cb3fc | 790 | startMillis(); |
| fanny868 | 0:9072e932503c | 791 | while(1) |
| gustavaditya | 40:5b937cac959a | 792 | { |
| gustavaditya | 40:5b937cac959a | 793 | // interupsi pembacaan PING setiap 30 ms |
| gustavaditya | 41:336a19289c2d | 794 | if(millis() - previousMillis4 >= 5){ //30 |
| gustavaditya | 41:336a19289c2d | 795 | jarak_ping = (float)pingAtas.Read_cm()/2; |
| gustavaditya | 40:5b937cac959a | 796 | |
| gustavaditya | 40:5b937cac959a | 797 | pingAtas.Send(); |
| gustavaditya | 40:5b937cac959a | 798 | previousMillis4 = millis(); |
| gustavaditya | 40:5b937cac959a | 799 | } |
| gustavaditya | 40:5b937cac959a | 800 | |
| fanny868 | 0:9072e932503c | 801 | // Interrupt Serial |
| calmantara186 | 16:90119f03c5d1 | 802 | joystick.idle(); |
| gustavaditya | 31:d5cbda07fd95 | 803 | if(joystick.readable()) |
| gustavaditya | 31:d5cbda07fd95 | 804 | { |
| fanny868 | 0:9072e932503c | 805 | // Panggil fungsi pembacaan joystik |
| fanny868 | 0:9072e932503c | 806 | joystick.baca_data(); |
| fanny868 | 0:9072e932503c | 807 | // Panggil fungsi pengolahan data joystik |
| fanny868 | 0:9072e932503c | 808 | joystick.olah_data(); |
| gustavaditya | 31:d5cbda07fd95 | 809 | // Masuk ke case joystick |
| gustavaditya | 31:d5cbda07fd95 | 810 | case_joy = case_joystick(); |
| Najib_irvani | 44:452c214d9cf5 | 811 | //pc.printf("%d\n",case_joy); |
| rahmadirizki18 | 3:1287fccc11be | 812 | aktuator(); |
| gustavaditya | 31:d5cbda07fd95 | 813 | launcher(); |
| gustavaditya | 40:5b937cac959a | 814 | reloader(); |
| Najib_irvani | 43:3807a95aa284 | 815 | if ((millis()-previousMillis3 >= 320)&&(flag_Pneu)){ |
| be_bryan | 26:256160a1a82d | 816 | pneumatik = 1; |
| gustavaditya | 31:d5cbda07fd95 | 817 | flag_Pneu = false; |
| be_bryan | 26:256160a1a82d | 818 | } |
| be_bryan | 26:256160a1a82d | 819 | } |
| gustavaditya | 31:d5cbda07fd95 | 820 | else |
| gustavaditya | 31:d5cbda07fd95 | 821 | { |
| gustavaditya | 31:d5cbda07fd95 | 822 | joystick.idle(); |
| MarchioKevin | 21:da2f3d04468f | 823 | } |
| franshendri | 42:6caf8bd5abbc | 824 | |
| Najib_irvani | 44:452c214d9cf5 | 825 | if(millis() - previousMillis5 >= 400) |
| Najib_irvani | 44:452c214d9cf5 | 826 | { |
| Najib_irvani | 43:3807a95aa284 | 827 | display.write(0,((int) rpm2) / 10); |
| Najib_irvani | 43:3807a95aa284 | 828 | display.write(1,((int)rpm2) % 10); |
| franshendri | 42:6caf8bd5abbc | 829 | display.write(2, (int)target_rpm2 / 10); |
| franshendri | 42:6caf8bd5abbc | 830 | display.write(3, (int)target_rpm2 % 10); |
| franshendri | 42:6caf8bd5abbc | 831 | display.setColon(true); |
| franshendri | 42:6caf8bd5abbc | 832 | |
| franshendri | 42:6caf8bd5abbc | 833 | previousMillis5 = millis(); |
| franshendri | 42:6caf8bd5abbc | 834 | } |
| fanny868 | 0:9072e932503c | 835 | } |
| be_bryan | 28:2d0746dc2d7d | 836 | } |