KRAI 2017 / Mbed 2 deprecated DagonFly__RoadToJapan_15Mei_Ultimate

Fix pisan inimah plis jangan revisi ultimate mantep

Dependencies:   DigitDisplay Motor PID mbed millis

Fork of DagonFly__RoadToJapan_15Mei by KRAI 2017

main.cpp@51:df6391c3fa68, 2017-05-15 (annotated)

Committer:
MarchioKevin
Date:
Mon May 15 07:49:13 2017 +0000
Revision:
51:df6391c3fa68
Parent:
50:8bc9dbca2ffa
Child:
52:a39e26b935a9
Fix pisan ini mah plis jangan revisi lagi mantep

Who changed what in which revision?

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