![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
tugas akhir
Dependencies: Motor mbed millis
Fork of DagonFly__RoadToJapan_15Mei_Ultimate by
main.cpp@38:3ef6754bd8d8, 2017-02-17 (annotated)
- Committer:
- gustavaditya
- Date:
- Fri Feb 17 13:05:02 2017 +0000
- Revision:
- 38:3ef6754bd8d8
- Parent:
- 37:67d54563af90
- Child:
- 39:11358f3f61ff
Update 17 Februari 2017, PID launcher sudah dibetulkan
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
rahmadirizki18 | 5:3aa203218306 | 1 | /****************************************************************************/ |
rahmadirizki18 | 5:3aa203218306 | 2 | /* PROGRAM UNTUK PID CLOSED LOOP */ |
rahmadirizki18 | 5:3aa203218306 | 3 | /* */ |
gustavaditya | 38:3ef6754bd8d8 | 4 | /* Last Update : 18 Februari 2017 */ |
gustavaditya | 31:d5cbda07fd95 | 5 | /* */ |
rahmadirizki18 | 5:3aa203218306 | 6 | /* - Digunakan encoder autonics */ |
rahmadirizki18 | 5:3aa203218306 | 7 | /* - Konfigurasi Motor dan Encoder sbb : */ |
MarchioKevin | 22:4632f58461e0 | 8 | /* ______________________ */ |
MarchioKevin | 22:4632f58461e0 | 9 | /* / \ Rode Depan Belakang: */ |
MarchioKevin | 22:4632f58461e0 | 10 | /* / 2 (Belakang) \ Omniwheel */ |
MarchioKevin | 22:4632f58461e0 | 11 | /* | | */ |
MarchioKevin | 22:4632f58461e0 | 12 | /* | 3 (Encoder) 4 | Roda Kiri Kanan: */ |
MarchioKevin | 22:4632f58461e0 | 13 | /* | | Fixed Wheel */ |
MarchioKevin | 22:4632f58461e0 | 14 | /* \ 1 (Depan) / */ |
MarchioKevin | 22:4632f58461e0 | 15 | /* \______________________/ Putaran berlawanan arah */ |
MarchioKevin | 22:4632f58461e0 | 16 | /* jarum jam positif */ |
rahmadirizki18 | 5:3aa203218306 | 17 | /* SETTINGS (WAJIB!) : */ |
rahmadirizki18 | 5:3aa203218306 | 18 | /* 1. Settings Pin Encoder, Resolusi, dan Tipe encoding di omniPos.h */ |
rahmadirizki18 | 5:3aa203218306 | 19 | /* 2. Deklarasi penggunaan library omniPos pada bagian deklarasi encoder */ |
rahmadirizki18 | 5:3aa203218306 | 20 | /* */ |
rahmadirizki18 | 5:3aa203218306 | 21 | /****************************************************************************/ |
rahmadirizki18 | 6:68293bed71ea | 22 | /* */ |
MarchioKevin | 20:54dc93e7b016 | 23 | /* Joystick */ |
Sufa | 30:d69cc27ac644 | 24 | /* Kanan => */ |
Sufa | 30:d69cc27ac644 | 25 | /* Kiri => */ |
rahmadirizki18 | 6:68293bed71ea | 26 | /* */ |
Sufa | 30:d69cc27ac644 | 27 | /* Tombol silang => Power Screw Aktif */ |
Sufa | 30:d69cc27ac644 | 28 | /* Tombol segitiga => Aktif motor Launcher */ |
Sufa | 30:d69cc27ac644 | 29 | /* Tombol lingkaran => Aktif Pneumatik Launcher */ |
Sufa | 30:d69cc27ac644 | 30 | /* Tombol L1 => Pivot Kiri */ |
Sufa | 30:d69cc27ac644 | 31 | /* Tombol R1 => Pivot Kanan */ |
Sufa | 30:d69cc27ac644 | 32 | /* Tombol L3 => PWM Motor Belakang Dikurangi */ |
Sufa | 30:d69cc27ac644 | 33 | /* Tombol R3 => PWM Motor Belakang Ditambah */ |
Sufa | 30:d69cc27ac644 | 34 | /* Tombol L2 => PWM Motor Depan Dikurangi */ |
Sufa | 30:d69cc27ac644 | 35 | /* Tombol R2 => PWM Motor Depan Ditambah */ |
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 | 6:68293bed71ea | 44 | #include "Servo.h" |
rahmadirizki18 | 5:3aa203218306 | 45 | #include "encoderKRAI.h" |
be_bryan | 26:256160a1a82d | 46 | #include "millis.h" |
rahmadirizki18 | 5:3aa203218306 | 47 | |
calmantara186 | 16:90119f03c5d1 | 48 | /***********************************************/ |
calmantara186 | 16:90119f03c5d1 | 49 | /* Konstanta dan Variabel */ |
calmantara186 | 16:90119f03c5d1 | 50 | /***********************************************/ |
calmantara186 | 16:90119f03c5d1 | 51 | #define PI 3.14159265 |
be_bryan | 26:256160a1a82d | 52 | #define D_ENCODER 10 // Diameter Roda Encoder |
be_bryan | 26:256160a1a82d | 53 | #define D_ROBOT 80 // Diameter Roda Robot |
rahmadirizki18 | 17:e4229d77a5ab | 54 | #define PERPINDAHAN 1 // Perpindahan ke kanan dan kiri |
rahmadirizki18 | 5:3aa203218306 | 55 | |
be_bryan | 26:256160a1a82d | 56 | // Variable Atas |
gustavaditya | 38:3ef6754bd8d8 | 57 | double speed, speed2; |
gustavaditya | 38:3ef6754bd8d8 | 58 | const double maxSpeed = 0.95; |
gustavaditya | 38:3ef6754bd8d8 | 59 | const double kpA=0.6757, kdA=0.6757, kiA=0.00006757; |
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; |
gustavaditya | 38:3ef6754bd8d8 | 65 | float target_rpm = 12.0, target_rpm2 = 16.0; |
gustavaditya | 38:3ef6754bd8d8 | 66 | const float maxRPM = 27, minRPM = 0; // Limit 25 atau 27 |
gustavaditya | 38:3ef6754bd8d8 | 67 | |
gustavaditya | 38:3ef6754bd8d8 | 68 | const float pwmPowerUp = 0.78; |
gustavaditya | 38:3ef6754bd8d8 | 69 | const float pwmPowerDown = -0.9; |
be_bryan | 26:256160a1a82d | 70 | |
be_bryan | 26:256160a1a82d | 71 | // Variable Bawah |
Joshua23 | 25:054d3048dd03 | 72 | float Vt; |
gustavaditya | 31:d5cbda07fd95 | 73 | float keliling_enc = PI*D_ENCODER; |
gustavaditya | 31:d5cbda07fd95 | 74 | float keliling_robot = PI*D_ROBOT; |
gustavaditya | 31:d5cbda07fd95 | 75 | float speedT = 0.2; |
gustavaditya | 31:d5cbda07fd95 | 76 | float vpid = 0; |
gustavaditya | 33:69d266bc3fe9 | 77 | float PIVOT = 0.27; // PWM Pivot Kanan, Pivot Kiri |
gustavaditya | 33:69d266bc3fe9 | 78 | float tuneDpn = 0.35; // Tunning PWM motor Depan |
gustavaditya | 33:69d266bc3fe9 | 79 | float tuneBlk = 0.3; // Tunning PWM motor belakang |
gustavaditya | 31:d5cbda07fd95 | 80 | |
gustavaditya | 31:d5cbda07fd95 | 81 | /* Variabel Encoder Bawah */ |
gustavaditya | 31:d5cbda07fd95 | 82 | float errTetha, Tetha; // Variabel yang didapatkan encoder |
gustavaditya | 31:d5cbda07fd95 | 83 | |
gustavaditya | 31:d5cbda07fd95 | 84 | /* Deklarasi Variable Millis */ |
gustavaditya | 33:69d266bc3fe9 | 85 | unsigned long int previousMillis = 0; // PID launcher |
gustavaditya | 31:d5cbda07fd95 | 86 | unsigned long int currentMillis; |
gustavaditya | 33:69d266bc3fe9 | 87 | unsigned long int previousMillis2 = 0; // PID launcher |
gustavaditya | 31:d5cbda07fd95 | 88 | unsigned long int currentMillis2; |
gustavaditya | 33:69d266bc3fe9 | 89 | unsigned long int previousMillis3 = 0; // Pneumatik |
MarchioKevin | 22:4632f58461e0 | 90 | |
gustavaditya | 31:d5cbda07fd95 | 91 | /* Variabel Stick */ |
gustavaditya | 31:d5cbda07fd95 | 92 | //Logic untuk masuk aktuator |
gustavaditya | 31:d5cbda07fd95 | 93 | int case_joy; |
gustavaditya | 31:d5cbda07fd95 | 94 | bool isLauncher = false; |
gustavaditya | 31:d5cbda07fd95 | 95 | bool isReload = false; |
rizqicahyo | 36:5963c9a49485 | 96 | bool ReloadOn = false; |
gustavaditya | 31:d5cbda07fd95 | 97 | bool flag_Pneu = false; |
gustavaditya | 31:d5cbda07fd95 | 98 | |
gustavaditya | 31:d5cbda07fd95 | 99 | /*****************************************************/ |
gustavaditya | 31:d5cbda07fd95 | 100 | /* Definisi Prosedur, Fungsi dan Setting Pinout */ |
gustavaditya | 31:d5cbda07fd95 | 101 | /*****************************************************/ |
gustavaditya | 31:d5cbda07fd95 | 102 | |
gustavaditya | 31:d5cbda07fd95 | 103 | /* Fungsi dan Procedur Encoder */ |
gustavaditya | 31:d5cbda07fd95 | 104 | void init_speed(); // |
gustavaditya | 31:d5cbda07fd95 | 105 | void aktuator(); // Pergerakan aktuator berdasarkan case joystick |
gustavaditya | 31:d5cbda07fd95 | 106 | int case_joystick(); // Mendapatkan case dari joystick |
gustavaditya | 33:69d266bc3fe9 | 107 | //void speedKalibrasiMotor(); // Pertambahan target RPM motor atas melalui joystick |
gustavaditya | 31:d5cbda07fd95 | 108 | void setCenter(); // Prosedur reset encoder, posisi saat itu diset jadi titik (0,0) |
gustavaditya | 31:d5cbda07fd95 | 109 | float getTetha(); // Fungsi mendapatkan error Tetha |
gustavaditya | 31:d5cbda07fd95 | 110 | |
gustavaditya | 31:d5cbda07fd95 | 111 | /* Inisialisasi Pin TX-RX Joystik dan PC */ |
gustavaditya | 31:d5cbda07fd95 | 112 | joysticknucleo joystick(PA_0,PA_1); |
gustavaditya | 31:d5cbda07fd95 | 113 | Serial pc(USBTX,USBRX); |
calmantara186 | 16:90119f03c5d1 | 114 | |
be_bryan | 26:256160a1a82d | 115 | /* Deklarasi Encoder Base */ |
gustavaditya | 31:d5cbda07fd95 | 116 | encoderKRAI encoderBase(PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), 2xresolusi, mode pembacaan |
rahmadirizki18 | 15:98f0d56b14f0 | 117 | |
be_bryan | 26:256160a1a82d | 118 | /* Deklarasi Encoder Launcher */ |
gustavaditya | 37:67d54563af90 | 119 | encoderKRAI encLauncherBlk( PC_10, PC_11, 28, encoderKRAI::X4_ENCODING); |
gustavaditya | 37:67d54563af90 | 120 | encoderKRAI encLauncherDpn( PD_2, PC_12, 28, encoderKRAI::X4_ENCODING); |
be_bryan | 26:256160a1a82d | 121 | |
calmantara186 | 16:90119f03c5d1 | 122 | /* Deklarasi Motor Base */ |
gustavaditya | 31:d5cbda07fd95 | 123 | Motor motorDpn(PB_9, PA_12, PC_5); // pwm, fwd, rev |
gustavaditya | 31:d5cbda07fd95 | 124 | Motor motorBlk(PB_6, PB_1, PB_12); // pwm, fwd, rev |
fanny868 | 0:9072e932503c | 125 | |
calmantara186 | 16:90119f03c5d1 | 126 | /* Deklarasi Motor Launcher */ |
gustavaditya | 31:d5cbda07fd95 | 127 | Motor launcherDpn(PA_8,PC_2,PC_1); // pwm ,fwd, rev |
gustavaditya | 31:d5cbda07fd95 | 128 | Motor launcherBlk(PA_10, PC_3, PC_0); // pwm, fwd, rev |
gustavaditya | 31:d5cbda07fd95 | 129 | Motor powerScrew(PB_7, PA_14, PA_15); // pwm, fwd, rev |
rahmadirizki18 | 5:3aa203218306 | 130 | |
be_bryan | 26:256160a1a82d | 131 | /* Deklarasi Penumatik Launcher */ |
Sufa | 29:7b372b0aaa61 | 132 | DigitalOut pneumatik(PB_3, PullUp); |
be_bryan | 26:256160a1a82d | 133 | |
be_bryan | 27:68efb1622985 | 134 | /*Dekalrasi Limit Switch */ |
gustavaditya | 34:1cfd9b1c7d27 | 135 | DigitalIn infraAtas(PC_9, PullUp); |
gustavaditya | 31:d5cbda07fd95 | 136 | DigitalIn limitTengah(PB_10, PullUp); |
gustavaditya | 31:d5cbda07fd95 | 137 | DigitalIn limitBawah(PC_8, PullUp); |
rahmadirizki18 | 5:3aa203218306 | 138 | |
fanny868 | 0:9072e932503c | 139 | |
MarchioKevin | 22:4632f58461e0 | 140 | /****************************************************/ |
MarchioKevin | 22:4632f58461e0 | 141 | /* Deklarasi Fungsi dan Procedure */ |
MarchioKevin | 22:4632f58461e0 | 142 | /****************************************************/ |
gustavaditya | 31:d5cbda07fd95 | 143 | int case_joystick() |
gustavaditya | 31:d5cbda07fd95 | 144 | { |
gustavaditya | 31:d5cbda07fd95 | 145 | //---------------------------------------------------// |
gustavaditya | 31:d5cbda07fd95 | 146 | // Gerak Motor Base // |
gustavaditya | 31:d5cbda07fd95 | 147 | // Case 1 : Pivot kanan // |
gustavaditya | 31:d5cbda07fd95 | 148 | // Case 2 : Pivot Kiri // |
gustavaditya | 31:d5cbda07fd95 | 149 | // Case 3 : Kanan // |
gustavaditya | 31:d5cbda07fd95 | 150 | // Case 4 : Kiri // |
gustavaditya | 31:d5cbda07fd95 | 151 | // Case 5 : Break // |
gustavaditya | 31:d5cbda07fd95 | 152 | //---------------------------------------------------// |
gustavaditya | 31:d5cbda07fd95 | 153 | |
gustavaditya | 31:d5cbda07fd95 | 154 | int caseJoystick; |
gustavaditya | 31:d5cbda07fd95 | 155 | if (!joystick.L1 && joystick.R1) { |
gustavaditya | 31:d5cbda07fd95 | 156 | // Pivot Kanan |
gustavaditya | 31:d5cbda07fd95 | 157 | caseJoystick = 1; |
gustavaditya | 31:d5cbda07fd95 | 158 | } |
gustavaditya | 31:d5cbda07fd95 | 159 | else if (!joystick.R1 && joystick.L1) { |
gustavaditya | 31:d5cbda07fd95 | 160 | // Pivot Kiri |
gustavaditya | 31:d5cbda07fd95 | 161 | caseJoystick = 2; |
gustavaditya | 31:d5cbda07fd95 | 162 | } |
gustavaditya | 31:d5cbda07fd95 | 163 | else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) { |
gustavaditya | 31:d5cbda07fd95 | 164 | // Kanan |
gustavaditya | 31:d5cbda07fd95 | 165 | caseJoystick = 3; |
gustavaditya | 33:69d266bc3fe9 | 166 | //pc.printf("kanan"); |
gustavaditya | 31:d5cbda07fd95 | 167 | } |
gustavaditya | 31:d5cbda07fd95 | 168 | else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { |
gustavaditya | 31:d5cbda07fd95 | 169 | // Kiri |
gustavaditya | 31:d5cbda07fd95 | 170 | caseJoystick = 4; |
gustavaditya | 33:69d266bc3fe9 | 171 | //pc.printf("kiri"); |
gustavaditya | 31:d5cbda07fd95 | 172 | } |
gustavaditya | 38:3ef6754bd8d8 | 173 | else if (joystick.segitiga_click){ |
gustavaditya | 31:d5cbda07fd95 | 174 | // Motor Launcher |
gustavaditya | 31:d5cbda07fd95 | 175 | caseJoystick = 5; |
gustavaditya | 31:d5cbda07fd95 | 176 | } |
gustavaditya | 38:3ef6754bd8d8 | 177 | else if (joystick.R2_click){ |
gustavaditya | 31:d5cbda07fd95 | 178 | // Target Pulse PID ++ Motor Depan |
gustavaditya | 31:d5cbda07fd95 | 179 | caseJoystick = 6; |
gustavaditya | 31:d5cbda07fd95 | 180 | } |
gustavaditya | 38:3ef6754bd8d8 | 181 | else if (joystick.L2_click){ |
gustavaditya | 31:d5cbda07fd95 | 182 | // Target Pulse PID -- Motor Depan |
gustavaditya | 31:d5cbda07fd95 | 183 | caseJoystick = 7; |
gustavaditya | 31:d5cbda07fd95 | 184 | } |
gustavaditya | 38:3ef6754bd8d8 | 185 | /*else if (joystick.R3_click){ |
gustavaditya | 31:d5cbda07fd95 | 186 | // Target Pulse PID ++ Motor Belakang |
gustavaditya | 31:d5cbda07fd95 | 187 | caseJoystick = 8; |
gustavaditya | 31:d5cbda07fd95 | 188 | } |
gustavaditya | 38:3ef6754bd8d8 | 189 | else if (joystick.L3_click){ |
gustavaditya | 31:d5cbda07fd95 | 190 | // Target Pulse PID -- Motor Belakang |
gustavaditya | 31:d5cbda07fd95 | 191 | caseJoystick = 9; |
gustavaditya | 38:3ef6754bd8d8 | 192 | }*/ |
gustavaditya | 31:d5cbda07fd95 | 193 | else if (joystick.silang_click){ |
gustavaditya | 31:d5cbda07fd95 | 194 | // Pnemuatik ON |
gustavaditya | 31:d5cbda07fd95 | 195 | caseJoystick = 10; |
gustavaditya | 31:d5cbda07fd95 | 196 | } |
gustavaditya | 31:d5cbda07fd95 | 197 | else if ((joystick.atas)&&(!joystick.bawah)) { |
gustavaditya | 31:d5cbda07fd95 | 198 | // Power Screw Up |
gustavaditya | 31:d5cbda07fd95 | 199 | caseJoystick = 11; |
gustavaditya | 31:d5cbda07fd95 | 200 | } |
gustavaditya | 31:d5cbda07fd95 | 201 | else if ((!joystick.atas)&&(joystick.bawah)) { |
gustavaditya | 31:d5cbda07fd95 | 202 | // Power Screw Down |
gustavaditya | 31:d5cbda07fd95 | 203 | caseJoystick = 12; |
gustavaditya | 31:d5cbda07fd95 | 204 | } |
gustavaditya | 31:d5cbda07fd95 | 205 | else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { |
gustavaditya | 31:d5cbda07fd95 | 206 | // Break |
gustavaditya | 31:d5cbda07fd95 | 207 | caseJoystick = 13; |
gustavaditya | 31:d5cbda07fd95 | 208 | } |
gustavaditya | 31:d5cbda07fd95 | 209 | |
gustavaditya | 31:d5cbda07fd95 | 210 | return(caseJoystick); |
gustavaditya | 31:d5cbda07fd95 | 211 | } |
gustavaditya | 31:d5cbda07fd95 | 212 | |
gustavaditya | 31:d5cbda07fd95 | 213 | float getTetha(){ |
gustavaditya | 31:d5cbda07fd95 | 214 | // Fungsi untuk mendapatkan nilai tetha |
gustavaditya | 31:d5cbda07fd95 | 215 | float busur, tetha; |
gustavaditya | 31:d5cbda07fd95 | 216 | busur = ((encoderBase.getPulses())/(float)(2000.0)*keliling_enc); |
gustavaditya | 31:d5cbda07fd95 | 217 | tetha = busur/keliling_robot*360; |
gustavaditya | 31:d5cbda07fd95 | 218 | |
gustavaditya | 31:d5cbda07fd95 | 219 | return -(tetha); |
gustavaditya | 31:d5cbda07fd95 | 220 | } |
gustavaditya | 31:d5cbda07fd95 | 221 | |
be_bryan | 26:256160a1a82d | 222 | float pidBase(float Kp, float Ki, float Kd) |
Joshua23 | 25:054d3048dd03 | 223 | { |
Joshua23 | 25:054d3048dd03 | 224 | int errorP; |
Joshua23 | 25:054d3048dd03 | 225 | errorP = getTetha(); |
gustavaditya | 33:69d266bc3fe9 | 226 | if (errorP<3.5 && errorP>(-3.5)) |
gustavaditya | 33:69d266bc3fe9 | 227 | errorP = 0; |
Joshua23 | 25:054d3048dd03 | 228 | return (float)Kp*errorP; |
Joshua23 | 25:054d3048dd03 | 229 | } |
gustavaditya | 38:3ef6754bd8d8 | 230 | /* |
gustavaditya | 31:d5cbda07fd95 | 231 | void init_speed (){ |
gustavaditya | 31:d5cbda07fd95 | 232 | if (speed>maxSpeed){ |
gustavaditya | 31:d5cbda07fd95 | 233 | speed = maxSpeed; |
gustavaditya | 31:d5cbda07fd95 | 234 | } |
gustavaditya | 31:d5cbda07fd95 | 235 | if (speed<minSpeed){ |
gustavaditya | 31:d5cbda07fd95 | 236 | speed = minSpeed; |
calmantara186 | 16:90119f03c5d1 | 237 | } |
gustavaditya | 31:d5cbda07fd95 | 238 | if (speed2>maxSpeed){ |
gustavaditya | 31:d5cbda07fd95 | 239 | speed2 = maxSpeed; |
gustavaditya | 31:d5cbda07fd95 | 240 | } |
gustavaditya | 31:d5cbda07fd95 | 241 | if (speed2<minSpeed){ |
gustavaditya | 31:d5cbda07fd95 | 242 | speed2 = minSpeed; |
gustavaditya | 31:d5cbda07fd95 | 243 | } |
gustavaditya | 38:3ef6754bd8d8 | 244 | }*/ |
gustavaditya | 31:d5cbda07fd95 | 245 | |
gustavaditya | 31:d5cbda07fd95 | 246 | void setCenter(){ |
gustavaditya | 31:d5cbda07fd95 | 247 | // Fungsi untuk menentukan center dari robot |
gustavaditya | 31:d5cbda07fd95 | 248 | encoderBase.reset(); |
fanny868 | 0:9072e932503c | 249 | } |
fanny868 | 0:9072e932503c | 250 | |
gustavaditya | 37:67d54563af90 | 251 | void init_rpm (){ |
gustavaditya | 38:3ef6754bd8d8 | 252 | if (target_rpm>maxRPM-2){ |
gustavaditya | 38:3ef6754bd8d8 | 253 | target_rpm = maxRPM-2; |
gustavaditya | 37:67d54563af90 | 254 | } |
gustavaditya | 37:67d54563af90 | 255 | if (target_rpm<minRPM){ |
gustavaditya | 37:67d54563af90 | 256 | target_rpm = minRPM; |
gustavaditya | 37:67d54563af90 | 257 | } |
gustavaditya | 37:67d54563af90 | 258 | if (target_rpm2>maxRPM){ |
gustavaditya | 37:67d54563af90 | 259 | target_rpm2 = maxRPM; |
gustavaditya | 37:67d54563af90 | 260 | } |
gustavaditya | 38:3ef6754bd8d8 | 261 | if (target_rpm2<minRPM+2){ |
gustavaditya | 38:3ef6754bd8d8 | 262 | target_rpm2 = minRPM+2; |
gustavaditya | 37:67d54563af90 | 263 | } |
gustavaditya | 37:67d54563af90 | 264 | } |
gustavaditya | 37:67d54563af90 | 265 | |
gustavaditya | 31:d5cbda07fd95 | 266 | void aktuator() |
gustavaditya | 31:d5cbda07fd95 | 267 | { |
gustavaditya | 31:d5cbda07fd95 | 268 | switch (case_joy) { |
gustavaditya | 31:d5cbda07fd95 | 269 | case (1): |
gustavaditya | 31:d5cbda07fd95 | 270 | { |
gustavaditya | 31:d5cbda07fd95 | 271 | // Pivot Kanan |
gustavaditya | 31:d5cbda07fd95 | 272 | motorDpn.speed(-PIVOT); |
gustavaditya | 31:d5cbda07fd95 | 273 | motorBlk.speed(-PIVOT); |
gustavaditya | 31:d5cbda07fd95 | 274 | setCenter(); |
gustavaditya | 31:d5cbda07fd95 | 275 | break; |
gustavaditya | 31:d5cbda07fd95 | 276 | } |
gustavaditya | 31:d5cbda07fd95 | 277 | case (2): |
gustavaditya | 31:d5cbda07fd95 | 278 | { |
gustavaditya | 31:d5cbda07fd95 | 279 | // Pivot Kiri |
gustavaditya | 31:d5cbda07fd95 | 280 | motorDpn.speed(PIVOT); |
gustavaditya | 31:d5cbda07fd95 | 281 | motorBlk.speed(PIVOT); |
gustavaditya | 31:d5cbda07fd95 | 282 | setCenter(); |
gustavaditya | 31:d5cbda07fd95 | 283 | break; |
gustavaditya | 31:d5cbda07fd95 | 284 | } |
gustavaditya | 31:d5cbda07fd95 | 285 | case (3) : |
gustavaditya | 31:d5cbda07fd95 | 286 | { |
gustavaditya | 31:d5cbda07fd95 | 287 | // Kanan |
gustavaditya | 33:69d266bc3fe9 | 288 | motorDpn.speed(-tuneDpn + pidBase(0.009,0,0)); |
gustavaditya | 33:69d266bc3fe9 | 289 | motorBlk.speed(tuneBlk + pidBase(0.009,0,0)); |
gustavaditya | 33:69d266bc3fe9 | 290 | //speedDpn = tuneDpn + pidBase(0.009,0,0) |
gustavaditya | 33:69d266bc3fe9 | 291 | //speedBlk = tuneBlk + pidBase(0.009,0,0) |
gustavaditya | 33:69d266bc3fe9 | 292 | //motorDpn.speed(-tuneDpn); |
gustavaditya | 33:69d266bc3fe9 | 293 | //motorBlk.speed(tuneBlk); |
gustavaditya | 31:d5cbda07fd95 | 294 | break; |
gustavaditya | 31:d5cbda07fd95 | 295 | } |
gustavaditya | 31:d5cbda07fd95 | 296 | case (4) : |
gustavaditya | 31:d5cbda07fd95 | 297 | { |
gustavaditya | 31:d5cbda07fd95 | 298 | // Kiri |
gustavaditya | 33:69d266bc3fe9 | 299 | motorDpn.speed(tuneDpn + pidBase(0.009,0,0)); |
gustavaditya | 33:69d266bc3fe9 | 300 | motorBlk.speed(-tuneBlk + pidBase(0.009,0,0)); |
gustavaditya | 33:69d266bc3fe9 | 301 | //motorDpn.speed(tuneDpn); |
gustavaditya | 33:69d266bc3fe9 | 302 | //motorBlk.speed(-tuneBlk); |
gustavaditya | 31:d5cbda07fd95 | 303 | break; |
gustavaditya | 31:d5cbda07fd95 | 304 | } |
gustavaditya | 31:d5cbda07fd95 | 305 | case (5) : |
gustavaditya | 31:d5cbda07fd95 | 306 | { |
gustavaditya | 31:d5cbda07fd95 | 307 | // Aktifkan motor atas |
gustavaditya | 31:d5cbda07fd95 | 308 | isLauncher = !isLauncher; |
gustavaditya | 31:d5cbda07fd95 | 309 | break; |
Joshua23 | 8:0711dea61312 | 310 | } |
gustavaditya | 31:d5cbda07fd95 | 311 | case (6) : |
gustavaditya | 31:d5cbda07fd95 | 312 | { |
gustavaditya | 31:d5cbda07fd95 | 313 | // Target Pulse PID ++ Motor Depan |
gustavaditya | 31:d5cbda07fd95 | 314 | target_rpm2 = target_rpm2++; |
gustavaditya | 38:3ef6754bd8d8 | 315 | target_rpm = target_rpm++; |
gustavaditya | 37:67d54563af90 | 316 | init_rpm(); |
gustavaditya | 31:d5cbda07fd95 | 317 | break; |
gustavaditya | 31:d5cbda07fd95 | 318 | } |
gustavaditya | 31:d5cbda07fd95 | 319 | case (7) : |
gustavaditya | 31:d5cbda07fd95 | 320 | { |
gustavaditya | 31:d5cbda07fd95 | 321 | // Target Pulse PID -- Motor Depan |
gustavaditya | 31:d5cbda07fd95 | 322 | target_rpm2 = target_rpm2--; |
gustavaditya | 38:3ef6754bd8d8 | 323 | target_rpm = target_rpm--; |
gustavaditya | 37:67d54563af90 | 324 | init_rpm(); |
gustavaditya | 31:d5cbda07fd95 | 325 | break; |
gustavaditya | 31:d5cbda07fd95 | 326 | } |
gustavaditya | 38:3ef6754bd8d8 | 327 | /*case (8) : |
gustavaditya | 31:d5cbda07fd95 | 328 | { |
gustavaditya | 32:581d4a2373f0 | 329 | // Target Pulse PID ++ Motor Belakang= |
gustavaditya | 38:3ef6754bd8d8 | 330 | //init_rpm(); |
gustavaditya | 31:d5cbda07fd95 | 331 | break; |
gustavaditya | 31:d5cbda07fd95 | 332 | } |
gustavaditya | 31:d5cbda07fd95 | 333 | case (9) : |
be_bryan | 27:68efb1622985 | 334 | { |
gustavaditya | 31:d5cbda07fd95 | 335 | // Target Pulse PID -- Motor Belakang |
gustavaditya | 38:3ef6754bd8d8 | 336 | //init_rpm(); |
gustavaditya | 31:d5cbda07fd95 | 337 | break; |
gustavaditya | 38:3ef6754bd8d8 | 338 | }*/ |
gustavaditya | 31:d5cbda07fd95 | 339 | case (10) : |
gustavaditya | 31:d5cbda07fd95 | 340 | { |
gustavaditya | 31:d5cbda07fd95 | 341 | // Pneumatik |
gustavaditya | 31:d5cbda07fd95 | 342 | pneumatik = 0; |
gustavaditya | 31:d5cbda07fd95 | 343 | previousMillis3 = millis(); |
gustavaditya | 31:d5cbda07fd95 | 344 | flag_Pneu = true; |
gustavaditya | 31:d5cbda07fd95 | 345 | break; |
gustavaditya | 31:d5cbda07fd95 | 346 | } |
gustavaditya | 31:d5cbda07fd95 | 347 | case (11) : |
gustavaditya | 31:d5cbda07fd95 | 348 | { |
gustavaditya | 31:d5cbda07fd95 | 349 | // Power Screw Up |
rizqicahyo | 36:5963c9a49485 | 350 | //powerScrew.speed(pwmPowerUp); |
rizqicahyo | 36:5963c9a49485 | 351 | ReloadOn = !ReloadOn; |
gustavaditya | 31:d5cbda07fd95 | 352 | break; |
gustavaditya | 31:d5cbda07fd95 | 353 | } |
gustavaditya | 31:d5cbda07fd95 | 354 | case (12) : |
gustavaditya | 31:d5cbda07fd95 | 355 | { |
gustavaditya | 31:d5cbda07fd95 | 356 | // Power Screw Down |
rizqicahyo | 36:5963c9a49485 | 357 | //powerScrew.speed(pwmPowerDown); |
gustavaditya | 31:d5cbda07fd95 | 358 | break; |
gustavaditya | 31:d5cbda07fd95 | 359 | } |
gustavaditya | 31:d5cbda07fd95 | 360 | default : |
gustavaditya | 31:d5cbda07fd95 | 361 | { |
gustavaditya | 31:d5cbda07fd95 | 362 | motorDpn.brake(1); |
gustavaditya | 31:d5cbda07fd95 | 363 | motorBlk.brake(1); |
rizqicahyo | 36:5963c9a49485 | 364 | // aktuator(); |
rizqicahyo | 36:5963c9a49485 | 365 | /* if (!infraAtas) |
be_bryan | 27:68efb1622985 | 366 | { |
gustavaditya | 31:d5cbda07fd95 | 367 | powerScrew.brake(1); |
gustavaditya | 31:d5cbda07fd95 | 368 | } |
gustavaditya | 31:d5cbda07fd95 | 369 | if (!limitTengah) |
gustavaditya | 31:d5cbda07fd95 | 370 | { |
gustavaditya | 31:d5cbda07fd95 | 371 | case_joy = 12; |
gustavaditya | 31:d5cbda07fd95 | 372 | aktuator(); |
gustavaditya | 31:d5cbda07fd95 | 373 | } |
gustavaditya | 31:d5cbda07fd95 | 374 | if (!limitBawah) |
Sufa | 30:d69cc27ac644 | 375 | { |
gustavaditya | 31:d5cbda07fd95 | 376 | case_joy = 11; |
gustavaditya | 31:d5cbda07fd95 | 377 | aktuator(); |
rizqicahyo | 36:5963c9a49485 | 378 | }*/ |
gustavaditya | 31:d5cbda07fd95 | 379 | } |
gustavaditya | 31:d5cbda07fd95 | 380 | } // End Switch |
gustavaditya | 31:d5cbda07fd95 | 381 | } |
gustavaditya | 31:d5cbda07fd95 | 382 | |
rizqicahyo | 36:5963c9a49485 | 383 | void reloader() |
rizqicahyo | 36:5963c9a49485 | 384 | { |
rizqicahyo | 36:5963c9a49485 | 385 | if(ReloadOn){ |
rizqicahyo | 36:5963c9a49485 | 386 | if(isReload){ |
rizqicahyo | 36:5963c9a49485 | 387 | powerScrew.speed(pwmPowerDown); |
rizqicahyo | 36:5963c9a49485 | 388 | if(!limitBawah){ |
rizqicahyo | 36:5963c9a49485 | 389 | isReload = false; |
rizqicahyo | 36:5963c9a49485 | 390 | ReloadOn = false; |
rizqicahyo | 36:5963c9a49485 | 391 | } |
rizqicahyo | 36:5963c9a49485 | 392 | } |
rizqicahyo | 36:5963c9a49485 | 393 | else if(!limitTengah){ |
rizqicahyo | 36:5963c9a49485 | 394 | isReload = true; |
rizqicahyo | 36:5963c9a49485 | 395 | } |
rizqicahyo | 36:5963c9a49485 | 396 | else if(!infraAtas){ |
rizqicahyo | 36:5963c9a49485 | 397 | powerScrew.brake(1); |
rizqicahyo | 36:5963c9a49485 | 398 | } |
rizqicahyo | 36:5963c9a49485 | 399 | else{ |
rizqicahyo | 36:5963c9a49485 | 400 | powerScrew.speed(pwmPowerUp); |
rizqicahyo | 36:5963c9a49485 | 401 | } |
rizqicahyo | 36:5963c9a49485 | 402 | } |
rizqicahyo | 36:5963c9a49485 | 403 | else{ |
rizqicahyo | 36:5963c9a49485 | 404 | powerScrew.brake(1); |
rizqicahyo | 36:5963c9a49485 | 405 | } |
rizqicahyo | 36:5963c9a49485 | 406 | } |
rizqicahyo | 36:5963c9a49485 | 407 | |
rizqicahyo | 36:5963c9a49485 | 408 | |
gustavaditya | 31:d5cbda07fd95 | 409 | void launcher() |
gustavaditya | 31:d5cbda07fd95 | 410 | { |
gustavaditya | 31:d5cbda07fd95 | 411 | if (isLauncher) |
Sufa | 30:d69cc27ac644 | 412 | { |
be_bryan | 28:2d0746dc2d7d | 413 | currentMillis = millis(); |
be_bryan | 28:2d0746dc2d7d | 414 | currentMillis2 = millis(); |
be_bryan | 28:2d0746dc2d7d | 415 | |
gustavaditya | 31:d5cbda07fd95 | 416 | // PID Launcher Depan |
gustavaditya | 38:3ef6754bd8d8 | 417 | if (currentMillis-previousMillis>=12.5) |
be_bryan | 26:256160a1a82d | 418 | { |
gustavaditya | 31:d5cbda07fd95 | 419 | rpm = (float)encLauncherBlk.getPulses(); |
be_bryan | 26:256160a1a82d | 420 | current_error = target_rpm - rpm; |
be_bryan | 26:256160a1a82d | 421 | sum_error = sum_error + current_error; |
be_bryan | 26:256160a1a82d | 422 | p = current_error*kpA; |
gustavaditya | 38:3ef6754bd8d8 | 423 | d = (current_error-last_error)*kdA/12.5; |
gustavaditya | 38:3ef6754bd8d8 | 424 | i = sum_error*kiA*12.5; |
be_bryan | 26:256160a1a82d | 425 | speed = p + d + i; |
gustavaditya | 38:3ef6754bd8d8 | 426 | //init_speed(); |
gustavaditya | 38:3ef6754bd8d8 | 427 | if(speed > maxSpeed){ |
gustavaditya | 38:3ef6754bd8d8 | 428 | launcherBlk.speed(maxSpeed); |
gustavaditya | 38:3ef6754bd8d8 | 429 | } |
gustavaditya | 38:3ef6754bd8d8 | 430 | else { |
gustavaditya | 38:3ef6754bd8d8 | 431 | launcherBlk.speed(speed); |
gustavaditya | 38:3ef6754bd8d8 | 432 | } |
be_bryan | 26:256160a1a82d | 433 | last_error = current_error; |
gustavaditya | 31:d5cbda07fd95 | 434 | encLauncherBlk.reset(); |
be_bryan | 26:256160a1a82d | 435 | //pc.printf("%.04lf\n",rpm); |
be_bryan | 26:256160a1a82d | 436 | previousMillis = currentMillis; |
be_bryan | 26:256160a1a82d | 437 | } |
gustavaditya | 38:3ef6754bd8d8 | 438 | if (currentMillis2-previousMillis2>=12.5) |
be_bryan | 27:68efb1622985 | 439 | { |
gustavaditya | 31:d5cbda07fd95 | 440 | rpm2 = (float)encLauncherDpn.getPulses(); |
be_bryan | 27:68efb1622985 | 441 | current_error2 = target_rpm2 - rpm2; |
be_bryan | 27:68efb1622985 | 442 | sum_error2 = sum_error2 + current_error2; |
be_bryan | 27:68efb1622985 | 443 | p2 = current_error2*kpA; |
gustavaditya | 38:3ef6754bd8d8 | 444 | d2 = (current_error2-last_error2)*kdA/12.5; |
gustavaditya | 38:3ef6754bd8d8 | 445 | i2 = sum_error2*kiA*12.5; |
be_bryan | 27:68efb1622985 | 446 | speed2 = p2 + d2 + i2; |
gustavaditya | 38:3ef6754bd8d8 | 447 | //init_speed(); |
gustavaditya | 38:3ef6754bd8d8 | 448 | if (speed2 > maxSpeed){ |
gustavaditya | 38:3ef6754bd8d8 | 449 | launcherDpn.speed(maxSpeed); |
gustavaditya | 38:3ef6754bd8d8 | 450 | } |
gustavaditya | 38:3ef6754bd8d8 | 451 | else{ |
gustavaditya | 38:3ef6754bd8d8 | 452 | launcherDpn.speed(speed2); |
gustavaditya | 38:3ef6754bd8d8 | 453 | } |
be_bryan | 27:68efb1622985 | 454 | last_error2 = current_error2; |
gustavaditya | 31:d5cbda07fd95 | 455 | encLauncherDpn.reset(); |
be_bryan | 27:68efb1622985 | 456 | previousMillis2 = currentMillis2; |
be_bryan | 27:68efb1622985 | 457 | } |
rahmadirizki18 | 6:68293bed71ea | 458 | } |
Sufa | 29:7b372b0aaa61 | 459 | else |
Sufa | 29:7b372b0aaa61 | 460 | { |
gustavaditya | 31:d5cbda07fd95 | 461 | launcherDpn.brake(1); |
gustavaditya | 31:d5cbda07fd95 | 462 | launcherBlk.brake(1); |
gustavaditya | 31:d5cbda07fd95 | 463 | } |
rahmadirizki18 | 5:3aa203218306 | 464 | } |
gustavaditya | 31:d5cbda07fd95 | 465 | |
MarchioKevin | 22:4632f58461e0 | 466 | /*********************************************************/ |
MarchioKevin | 22:4632f58461e0 | 467 | /* Main Function */ |
MarchioKevin | 22:4632f58461e0 | 468 | /*********************************************************/ |
calmantara186 | 16:90119f03c5d1 | 469 | |
gustavaditya | 31:d5cbda07fd95 | 470 | int main (void) |
gustavaditya | 31:d5cbda07fd95 | 471 | { |
gustavaditya | 31:d5cbda07fd95 | 472 | // Set baud rate - 115200 |
fanny868 | 0:9072e932503c | 473 | joystick.setup(); |
rahmadirizki18 | 23:023b522977b2 | 474 | pc.baud(115200); |
rahmadirizki18 | 6:68293bed71ea | 475 | wait_ms(1000); |
rahmadirizki18 | 5:3aa203218306 | 476 | setCenter(); |
rahmadirizki18 | 5:3aa203218306 | 477 | wait_ms(500); |
rahmadirizki18 | 23:023b522977b2 | 478 | pc.printf("ready...."); |
gustavaditya | 35:69a47b4cb3fc | 479 | startMillis(); |
fanny868 | 0:9072e932503c | 480 | while(1) |
fanny868 | 0:9072e932503c | 481 | { |
fanny868 | 0:9072e932503c | 482 | // Interrupt Serial |
calmantara186 | 16:90119f03c5d1 | 483 | joystick.idle(); |
gustavaditya | 31:d5cbda07fd95 | 484 | if(joystick.readable()) |
gustavaditya | 31:d5cbda07fd95 | 485 | { |
fanny868 | 0:9072e932503c | 486 | // Panggil fungsi pembacaan joystik |
fanny868 | 0:9072e932503c | 487 | joystick.baca_data(); |
fanny868 | 0:9072e932503c | 488 | // Panggil fungsi pengolahan data joystik |
fanny868 | 0:9072e932503c | 489 | joystick.olah_data(); |
gustavaditya | 31:d5cbda07fd95 | 490 | // Masuk ke case joystick |
gustavaditya | 31:d5cbda07fd95 | 491 | case_joy = case_joystick(); |
rahmadirizki18 | 3:1287fccc11be | 492 | aktuator(); |
gustavaditya | 31:d5cbda07fd95 | 493 | launcher(); |
rizqicahyo | 36:5963c9a49485 | 494 | reloader(); |
gustavaditya | 38:3ef6754bd8d8 | 495 | if ((millis()-previousMillis3 >= 425)&&(flag_Pneu)){ |
be_bryan | 26:256160a1a82d | 496 | pneumatik = 1; |
gustavaditya | 31:d5cbda07fd95 | 497 | flag_Pneu = false; |
be_bryan | 26:256160a1a82d | 498 | } |
be_bryan | 26:256160a1a82d | 499 | } |
gustavaditya | 31:d5cbda07fd95 | 500 | else |
gustavaditya | 31:d5cbda07fd95 | 501 | { |
gustavaditya | 31:d5cbda07fd95 | 502 | joystick.idle(); |
MarchioKevin | 21:da2f3d04468f | 503 | } |
fanny868 | 0:9072e932503c | 504 | } |
be_bryan | 28:2d0746dc2d7d | 505 | } |