KRAI 2017 / Mbed 2 deprecated MainProgram_BaseBaru_fix_omni_5April

Kodingan 5 April setting kecepatan power screw

Dependencies:   DigitDisplay Motor PID Ping mbed millis

Fork of MainProgram_BaseBaru_fix_omni12Feb by KRAI 2017

main.cpp@44:d2ebfa6d1a7b, 2017-04-05 (annotated)

Committer:
Najib_irvani
Date:
Wed Apr 05 12:44:30 2017 +0000
Revision:
44:d2ebfa6d1a7b
Parent:
43:3807a95aa284 
Kodingan 5 April setting kecepatan power screw

Who changed what in which revision?

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