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