KRAI 2017 / Mbed 2 deprecated DagonFly__RoadToJapan_Disco_V1

this is disco time

Dependencies:   DigitDisplay Motor PID Ping mbed millis

Fork of DagonFly__RoadToJapan_17Mei by KRAI 2017

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

Committer:
Najib_irvani
Date:
Wed Apr 05 12:54:03 2017 +0000
Revision:
44:452c214d9cf5
Parent:
43:3807a95aa284
Child:
45:964ae71a30e3
5 april ganti kecepatan power screw. Yang bener

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:452c214d9cf5 28 /* Tombol silang => Pneumatik aktif */
Sufa 30:d69cc27ac644 29 /* Tombol segitiga => Aktif motor Launcher */
Najib_irvani 44:452c214d9cf5 30 /* Tombol lingkaran => Reloader naik */
Najib_irvani 44:452c214d9cf5 31 /* Tombol kotak => Reloader turun */
Sufa 30:d69cc27ac644 32 /* Tombol L1 => Pivot Kiri */
Sufa 30:d69cc27ac644 33 /* Tombol R1 => Pivot Kanan */
Najib_irvani 44:452c214d9cf5 34 /* Tombol L2 => Kurang PWM Motor Launcher */
Najib_irvani 44:452c214d9cf5 35 /* Tombol R2 => Tambah PWM Motor Launcher */
rahmadirizki18 13:8ab42383a2ca 36 /* */
calmantara186 16:90119f03c5d1 37 /* Bismillahirahmanirrahim */
MarchioKevin 20:54dc93e7b016 38 /* Jagalah Kebersihan Kodingan */
rahmadirizki18 6:68293bed71ea 39 /****************************************************************************/
rahmadirizki18 6:68293bed71ea 40
fanny868 0:9072e932503c 41 #include "mbed.h"
fanny868 0:9072e932503c 42 #include "JoystickPS3.h"
fanny868 0:9072e932503c 43 #include "Motor.h"
rahmadirizki18 5:3aa203218306 44 #include "encoderKRAI.h"
be_bryan 26:256160a1a82d 45 #include "millis.h"
gustavaditya 40:5b937cac959a 46 #include "Ping.h"
franshendri 42:6caf8bd5abbc 47 #include "DigitDisplay.h"
franshendri 42:6caf8bd5abbc 48
calmantara186 16:90119f03c5d1 49 /***********************************************/
calmantara186 16:90119f03c5d1 50 /* Konstanta dan Variabel */
calmantara186 16:90119f03c5d1 51 /***********************************************/
calmantara186 16:90119f03c5d1 52 #define PI 3.14159265
be_bryan 26:256160a1a82d 53 #define D_ENCODER 10 // Diameter Roda Encoder
be_bryan 26:256160a1a82d 54 #define D_ROBOT 80 // Diameter Roda Robot
rahmadirizki18 5:3aa203218306 55
be_bryan 26:256160a1a82d 56 // Variable Atas
gustavaditya 38:3ef6754bd8d8 57 double speed, speed2;
gustavaditya 39:11358f3f61ff 58 const double maxSpeed = 0.95, minSpeed = 0.0;
Najib_irvani 44:452c214d9cf5 59 const double kpA=0.6757, kdA=0.7757, kiA=0.00003757;
be_bryan 26:256160a1a82d 60 double p,i,d;
be_bryan 27:68efb1622985 61 double p2,i2,d2;
be_bryan 26:256160a1a82d 62 double last_error = 0, current_error, sum_error = 0;
be_bryan 27:68efb1622985 63 double last_error2 = 0, current_error2, sum_error2 = 0;
gustavaditya 38:3ef6754bd8d8 64 float rpm, rpm2;
Najib_irvani 44:452c214d9cf5 65 float target_rpm = 17.0, target_rpm2 = 17.0; // selisih 4 bagus, sama bagus
gustavaditya 41:336a19289c2d 66 const float maxRPM = 28, minRPM = 0; // Limit 25 atau 27
gustavaditya 38:3ef6754bd8d8 67
Najib_irvani 44:452c214d9cf5 68 const float pwmPowerUp = 1.0;
Najib_irvani 44:452c214d9cf5 69 const float pwmPowerDown = -1.0;
gustavaditya 40:5b937cac959a 70
gustavaditya 40:5b937cac959a 71 float jarak_ping=0;
be_bryan 26:256160a1a82d 72
be_bryan 26:256160a1a82d 73 // Variable Bawah
Joshua23 25:054d3048dd03 74 float Vt;
gustavaditya 31:d5cbda07fd95 75 float keliling_enc = PI*D_ENCODER;
gustavaditya 31:d5cbda07fd95 76 float keliling_robot = PI*D_ROBOT;
gustavaditya 31:d5cbda07fd95 77 float speedT = 0.2;
Najib_irvani 44:452c214d9cf5 78 float PIVOT = 0.17; // PWM Pivot Kanan, Pivot Kiri
Najib_irvani 44:452c214d9cf5 79 float tuneDpn = 0.80; // Tunning PWM motor Depan
Najib_irvani 44:452c214d9cf5 80 float tuneBlk = 0.80; // Tunning PWM motor belakang
Najib_irvani 44:452c214d9cf5 81 float tuneR = 0.78;
Najib_irvani 43:3807a95aa284 82 float tuneL = 0.72;
Najib_irvani 44:452c214d9cf5 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:452c214d9cf5 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:452c214d9cf5 133 encoderKRAI encLauncherDpn( PC_10, PC_11, 28, encoderKRAI::X4_ENCODING);
Najib_irvani 44:452c214d9cf5 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:452c214d9cf5 137 Motor motorDpn(PB_7, PC_3, PC_0); //(PB_9, PA_12, PC_5);
Najib_irvani 44:452c214d9cf5 138 //Motor motorBlk(PB_6, PC_14, PC_13); //(PB_6, PB_1, PB_12); (PC_7, PC_14, PC_13);
Najib_irvani 44:452c214d9cf5 139 Motor motorBlk(PB_2, PB_15, PB_1);
Najib_irvani 44:452c214d9cf5 140 Motor motorL (PB_9, PA_12, PA_6);
Najib_irvani 44:452c214d9cf5 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:452c214d9cf5 144 Motor launcherDpn(PA_5,PB_12,PA_11); // pwm ,fwd, rev
Najib_irvani 44:452c214d9cf5 145 Motor launcherBlk(PB_3, PC_4, PA_10); // pwm, fwd, rev
Najib_irvani 44:452c214d9cf5 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:452c214d9cf5 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:452c214d9cf5 153 DigitalIn limitTengah(PA_9, PullUp);
Najib_irvani 44:452c214d9cf5 154 DigitalIn limitBawah(PC_7, PullUp);
Najib_irvani 44:452c214d9cf5 155 DigitalIn limitBawah1(PA_7, PullUp);
rahmadirizki18 5:3aa203218306 156
gustavaditya 40:5b937cac959a 157 /*deklarasi PING ultrasonic*/
Najib_irvani 44:452c214d9cf5 158 Ping pingAtas(PC_15);
fanny868 0:9072e932503c 159
franshendri 42:6caf8bd5abbc 160 /*Deklarasi Display*/
Najib_irvani 44:452c214d9cf5 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:452c214d9cf5 186 else if ((joystick.START_click)&&(!joystick.SELECT_click)&&(!joystick.R3_click)) {
Najib_irvani 44:452c214d9cf5 187 // tambah rpm dengan nilai tertentu
Najib_irvani 44:452c214d9cf5 188 caseJoystick = 31;
Najib_irvani 44:452c214d9cf5 189 }
Najib_irvani 44:452c214d9cf5 190 else if ((!joystick.START_click)&&(joystick.SELECT_click)&&(!joystick.R3_click)) {
Najib_irvani 44:452c214d9cf5 191 // kurangi rpm dengan nilai tertentu
Najib_irvani 44:452c214d9cf5 192 caseJoystick = 32;
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 = 33;
Najib_irvani 44:452c214d9cf5 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 kanan
Najib_irvani 43:3807a95aa284 200 caseJoystick = 17;
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 // Kanan + Rotasi kiri
Najib_irvani 43:3807a95aa284 204 caseJoystick = 18;
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 // Kiri + Rotasi kanan
Najib_irvani 43:3807a95aa284 208 caseJoystick = 19;
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 // Kanan + Rotasi kiri
Najib_irvani 43:3807a95aa284 212 caseJoystick = 20;
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 kanan
Najib_irvani 43:3807a95aa284 216 caseJoystick = 21;
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 kiri
Najib_irvani 43:3807a95aa284 220 caseJoystick = 22;
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 kanan
Najib_irvani 43:3807a95aa284 224 caseJoystick = 23;
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 kiri
Najib_irvani 43:3807a95aa284 228 caseJoystick = 24;
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 // Kanan + segitiga
Najib_irvani 43:3807a95aa284 232 caseJoystick = 25;
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 // Kiri + segitiga
Najib_irvani 43:3807a95aa284 236 caseJoystick = 26;
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 // Kanan + lingkaran
Najib_irvani 43:3807a95aa284 240 caseJoystick = 27;
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 // Kiri + lingkaran
Najib_irvani 43:3807a95aa284 244 caseJoystick = 28;
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 // Kanan + kotak
Najib_irvani 43:3807a95aa284 248 caseJoystick = 29;
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 // Kiri + kotak
Najib_irvani 43:3807a95aa284 252 caseJoystick = 30;
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 kanan maju
Najib_irvani 44:452c214d9cf5 256 caseJoystick = 13;
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 kiri maju
Najib_irvani 44:452c214d9cf5 260 caseJoystick = 14;
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 kanan mundur
Najib_irvani 44:452c214d9cf5 264 caseJoystick = 15;
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 kiri mundur
Najib_irvani 44:452c214d9cf5 268 caseJoystick = 16;
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 // Kanan
gustavaditya 31:d5cbda07fd95 272 caseJoystick = 3;
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 // Kiri
gustavaditya 31:d5cbda07fd95 276 caseJoystick = 4;
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 // Atas -- Maju
Najib_irvani 44:452c214d9cf5 280 caseJoystick = 8;
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 // Bawah -- Mundur
Najib_irvani 44:452c214d9cf5 284 caseJoystick = 9;
gustavaditya 31:d5cbda07fd95 285 }
gustavaditya 38:3ef6754bd8d8 286 else if (joystick.segitiga_click){
gustavaditya 31:d5cbda07fd95 287 // Motor Launcher
gustavaditya 31:d5cbda07fd95 288 caseJoystick = 5;
gustavaditya 31:d5cbda07fd95 289 }
gustavaditya 38:3ef6754bd8d8 290 else if (joystick.R2_click){
gustavaditya 31:d5cbda07fd95 291 // Target Pulse PID ++ Motor Depan
gustavaditya 31:d5cbda07fd95 292 caseJoystick = 6;
gustavaditya 31:d5cbda07fd95 293 }
gustavaditya 38:3ef6754bd8d8 294 else if (joystick.L2_click){
Najib_irvani 43:3807a95aa284 295 // Target Pulse PID -- Motor
gustavaditya 31:d5cbda07fd95 296 caseJoystick = 7;
gustavaditya 31:d5cbda07fd95 297 }
gustavaditya 31:d5cbda07fd95 298 else if (joystick.silang_click){
gustavaditya 31:d5cbda07fd95 299 // Pnemuatik ON
gustavaditya 31:d5cbda07fd95 300 caseJoystick = 10;
gustavaditya 31:d5cbda07fd95 301 }
Najib_irvani 43:3807a95aa284 302 else if ((joystick.lingkaran_click)&&(!joystick.kotak_click)) {
gustavaditya 31:d5cbda07fd95 303 // Power Screw Up
gustavaditya 31:d5cbda07fd95 304 caseJoystick = 11;
gustavaditya 31:d5cbda07fd95 305 }
Najib_irvani 43:3807a95aa284 306 else if ((joystick.kotak_click)&&(!joystick.lingkaran_click)) {
gustavaditya 31:d5cbda07fd95 307 // Power Screw Down
gustavaditya 31:d5cbda07fd95 308 caseJoystick = 12;
gustavaditya 31:d5cbda07fd95 309 }
gustavaditya 31:d5cbda07fd95 310
gustavaditya 31:d5cbda07fd95 311 return(caseJoystick);
gustavaditya 31:d5cbda07fd95 312 }
gustavaditya 31:d5cbda07fd95 313
gustavaditya 31:d5cbda07fd95 314 float getTetha(){
gustavaditya 31:d5cbda07fd95 315 // Fungsi untuk mendapatkan nilai tetha
gustavaditya 31:d5cbda07fd95 316 float busur, tetha;
gustavaditya 31:d5cbda07fd95 317 busur = ((encoderBase.getPulses())/(float)(2000.0)*keliling_enc);
gustavaditya 31:d5cbda07fd95 318 tetha = busur/keliling_robot*360;
gustavaditya 31:d5cbda07fd95 319
gustavaditya 31:d5cbda07fd95 320 return -(tetha);
gustavaditya 31:d5cbda07fd95 321 }
gustavaditya 31:d5cbda07fd95 322
be_bryan 26:256160a1a82d 323 float pidBase(float Kp, float Ki, float Kd)
Joshua23 25:054d3048dd03 324 {
Joshua23 25:054d3048dd03 325 int errorP;
Joshua23 25:054d3048dd03 326 errorP = getTetha();
gustavaditya 33:69d266bc3fe9 327 if (errorP<3.5 && errorP>(-3.5))
gustavaditya 33:69d266bc3fe9 328 errorP = 0;
Joshua23 25:054d3048dd03 329 return (float)Kp*errorP;
Joshua23 25:054d3048dd03 330 }
gustavaditya 31:d5cbda07fd95 331
gustavaditya 31:d5cbda07fd95 332 void setCenter(){
gustavaditya 31:d5cbda07fd95 333 // Fungsi untuk menentukan center dari robot
gustavaditya 31:d5cbda07fd95 334 encoderBase.reset();
fanny868 0:9072e932503c 335 }
fanny868 0:9072e932503c 336
gustavaditya 37:67d54563af90 337 void init_rpm (){
gustavaditya 38:3ef6754bd8d8 338 if (target_rpm>maxRPM-2){
gustavaditya 38:3ef6754bd8d8 339 target_rpm = maxRPM-2;
gustavaditya 37:67d54563af90 340 }
franshendri 42:6caf8bd5abbc 341 if (target_rpm<minRPM){
gustavaditya 37:67d54563af90 342 target_rpm = minRPM;
gustavaditya 37:67d54563af90 343 }
gustavaditya 37:67d54563af90 344 if (target_rpm2>maxRPM){
gustavaditya 37:67d54563af90 345 target_rpm2 = maxRPM;
gustavaditya 37:67d54563af90 346 }
franshendri 42:6caf8bd5abbc 347 if (target_rpm2<minRPM+2){
gustavaditya 38:3ef6754bd8d8 348 target_rpm2 = minRPM+2;
gustavaditya 37:67d54563af90 349 }
gustavaditya 37:67d54563af90 350 }
gustavaditya 37:67d54563af90 351
gustavaditya 31:d5cbda07fd95 352 void aktuator()
gustavaditya 31:d5cbda07fd95 353 {
gustavaditya 31:d5cbda07fd95 354 switch (case_joy) {
gustavaditya 31:d5cbda07fd95 355 case (1):
gustavaditya 31:d5cbda07fd95 356 {
gustavaditya 31:d5cbda07fd95 357 // Pivot Kanan
gustavaditya 31:d5cbda07fd95 358 motorDpn.speed(-PIVOT);
gustavaditya 31:d5cbda07fd95 359 motorBlk.speed(-PIVOT);
Najib_irvani 43:3807a95aa284 360 motorR.speed(-rasio*PIVOT);
Najib_irvani 43:3807a95aa284 361 motorL.speed(-rasio*PIVOT);
gustavaditya 31:d5cbda07fd95 362 break;
gustavaditya 31:d5cbda07fd95 363 }
gustavaditya 31:d5cbda07fd95 364 case (2):
gustavaditya 31:d5cbda07fd95 365 {
gustavaditya 31:d5cbda07fd95 366 // Pivot Kiri
gustavaditya 31:d5cbda07fd95 367 motorDpn.speed(PIVOT);
gustavaditya 31:d5cbda07fd95 368 motorBlk.speed(PIVOT);
Najib_irvani 43:3807a95aa284 369 motorR.speed(rasio*PIVOT);
Najib_irvani 43:3807a95aa284 370 motorL.speed(rasio*PIVOT);
Najib_irvani 43:3807a95aa284 371 break;
Najib_irvani 43:3807a95aa284 372 }
Najib_irvani 43:3807a95aa284 373 case (17):
Najib_irvani 43:3807a95aa284 374 {
Najib_irvani 43:3807a95aa284 375 // Kanan + Rotasi Kanan
Najib_irvani 43:3807a95aa284 376 motorDpn.speed(-PIVOT);
Najib_irvani 43:3807a95aa284 377 motorBlk.speed(-PIVOT);
Najib_irvani 43:3807a95aa284 378 motorR.speed(-rasio*PIVOT);
Najib_irvani 43:3807a95aa284 379 motorL.speed(-rasio*PIVOT);
Najib_irvani 43:3807a95aa284 380 break;
Najib_irvani 43:3807a95aa284 381 }
Najib_irvani 43:3807a95aa284 382 case (18):
Najib_irvani 43:3807a95aa284 383 {
Najib_irvani 43:3807a95aa284 384 // Kanan + Rotasi Kiri
Najib_irvani 43:3807a95aa284 385 motorDpn.speed(PIVOT);
Najib_irvani 43:3807a95aa284 386 motorBlk.speed(PIVOT);
Najib_irvani 43:3807a95aa284 387 motorR.speed(rasio*PIVOT);
Najib_irvani 43:3807a95aa284 388 motorL.speed(rasio*PIVOT);
Najib_irvani 43:3807a95aa284 389 break;
Najib_irvani 43:3807a95aa284 390 }
Najib_irvani 43:3807a95aa284 391 case (19):
Najib_irvani 43:3807a95aa284 392 {
Najib_irvani 43:3807a95aa284 393 // Kiri + Rotasi Kanan
Najib_irvani 43:3807a95aa284 394 motorDpn.speed(-PIVOT);
Najib_irvani 43:3807a95aa284 395 motorBlk.speed(-PIVOT);
Najib_irvani 43:3807a95aa284 396 motorR.speed(-rasio*PIVOT);
Najib_irvani 43:3807a95aa284 397 motorL.speed(-rasio*PIVOT);
Najib_irvani 43:3807a95aa284 398 break;
Najib_irvani 43:3807a95aa284 399 }
Najib_irvani 43:3807a95aa284 400 case (20):
Najib_irvani 43:3807a95aa284 401 {
Najib_irvani 43:3807a95aa284 402 // Kiri + Rotasi Kiri
Najib_irvani 43:3807a95aa284 403 motorDpn.speed(PIVOT);
Najib_irvani 43:3807a95aa284 404 motorBlk.speed(PIVOT);
Najib_irvani 43:3807a95aa284 405 motorR.speed(rasio*PIVOT);
Najib_irvani 43:3807a95aa284 406 motorL.speed(rasio*PIVOT);
Najib_irvani 43:3807a95aa284 407 break;
Najib_irvani 43:3807a95aa284 408 }
Najib_irvani 43:3807a95aa284 409 case (21):
Najib_irvani 43:3807a95aa284 410 {
Najib_irvani 43:3807a95aa284 411 // Maju + Rotasi Kanan
Najib_irvani 43:3807a95aa284 412 motorDpn.speed(-PIVOT);
Najib_irvani 43:3807a95aa284 413 motorBlk.speed(-PIVOT);
Najib_irvani 43:3807a95aa284 414 motorR.speed(-rasio*PIVOT);
Najib_irvani 43:3807a95aa284 415 motorL.speed(-rasio*PIVOT);
Najib_irvani 43:3807a95aa284 416 break;
Najib_irvani 43:3807a95aa284 417 }
Najib_irvani 43:3807a95aa284 418 case (22):
Najib_irvani 43:3807a95aa284 419 {
Najib_irvani 43:3807a95aa284 420 // Maju + Rotasi Kiri
Najib_irvani 43:3807a95aa284 421 motorDpn.speed(PIVOT);
Najib_irvani 43:3807a95aa284 422 motorBlk.speed(PIVOT);
Najib_irvani 43:3807a95aa284 423 motorR.speed(rasio*PIVOT);
Najib_irvani 43:3807a95aa284 424 motorL.speed(rasio*PIVOT);
Najib_irvani 43:3807a95aa284 425 break;
Najib_irvani 43:3807a95aa284 426 }
Najib_irvani 43:3807a95aa284 427 case (23):
Najib_irvani 43:3807a95aa284 428 {
Najib_irvani 43:3807a95aa284 429 // Mundur + Rotasi Kanan
Najib_irvani 43:3807a95aa284 430 motorDpn.speed(-PIVOT);
Najib_irvani 43:3807a95aa284 431 motorBlk.speed(-PIVOT);
Najib_irvani 43:3807a95aa284 432 motorR.speed(-rasio*PIVOT);
Najib_irvani 43:3807a95aa284 433 motorL.speed(-rasio*PIVOT);
Najib_irvani 43:3807a95aa284 434 break;
Najib_irvani 43:3807a95aa284 435 }
Najib_irvani 43:3807a95aa284 436 case (24):
Najib_irvani 43:3807a95aa284 437 {
Najib_irvani 43:3807a95aa284 438 // Mundur + Rotasi Kiri
Najib_irvani 43:3807a95aa284 439 motorDpn.speed(PIVOT);
Najib_irvani 43:3807a95aa284 440 motorBlk.speed(PIVOT);
Najib_irvani 43:3807a95aa284 441 motorR.speed(rasio*PIVOT);
Najib_irvani 43:3807a95aa284 442 motorL.speed(rasio*PIVOT);
Najib_irvani 43:3807a95aa284 443 break;
Najib_irvani 43:3807a95aa284 444 }
Najib_irvani 43:3807a95aa284 445 case (25):
Najib_irvani 43:3807a95aa284 446 {
Najib_irvani 43:3807a95aa284 447 // Kanan + segitiga
Najib_irvani 43:3807a95aa284 448 isLauncher = !isLauncher;
Najib_irvani 43:3807a95aa284 449 break;
Najib_irvani 43:3807a95aa284 450 }
Najib_irvani 43:3807a95aa284 451 case (26):
Najib_irvani 43:3807a95aa284 452 {
Najib_irvani 43:3807a95aa284 453 // Kiri + segitiga
Najib_irvani 43:3807a95aa284 454 isLauncher = !isLauncher;
Najib_irvani 43:3807a95aa284 455 break;
Najib_irvani 43:3807a95aa284 456 }
Najib_irvani 43:3807a95aa284 457 case (27):
Najib_irvani 43:3807a95aa284 458 {
Najib_irvani 43:3807a95aa284 459 // Kanan + lingkaran
Najib_irvani 43:3807a95aa284 460 ReloadOn = !ReloadOn;
Najib_irvani 43:3807a95aa284 461 isReload = false;
Najib_irvani 43:3807a95aa284 462 break;
Najib_irvani 43:3807a95aa284 463 }
Najib_irvani 43:3807a95aa284 464 case (28):
Najib_irvani 43:3807a95aa284 465 {
Najib_irvani 43:3807a95aa284 466 // Kiri + lingkaran
Najib_irvani 43:3807a95aa284 467 ReloadOn = !ReloadOn;
Najib_irvani 43:3807a95aa284 468 isReload = false;
Najib_irvani 43:3807a95aa284 469 break;
Najib_irvani 43:3807a95aa284 470 }
Najib_irvani 43:3807a95aa284 471 case (29):
Najib_irvani 43:3807a95aa284 472 {
Najib_irvani 43:3807a95aa284 473 // Kanan + kotak
Najib_irvani 43:3807a95aa284 474 ReloadOn = !ReloadOn;
Najib_irvani 43:3807a95aa284 475 isReload = true;
Najib_irvani 43:3807a95aa284 476 break;
Najib_irvani 43:3807a95aa284 477 }
Najib_irvani 43:3807a95aa284 478 case (30):
Najib_irvani 43:3807a95aa284 479 {
Najib_irvani 43:3807a95aa284 480 // Kiri + kotak
Najib_irvani 43:3807a95aa284 481 ReloadOn = !ReloadOn;
Najib_irvani 43:3807a95aa284 482 isReload = true;
Najib_irvani 43:3807a95aa284 483 break;
Najib_irvani 43:3807a95aa284 484 }
Najib_irvani 43:3807a95aa284 485 case (13) :
Najib_irvani 43:3807a95aa284 486 {
Najib_irvani 43:3807a95aa284 487 // Serong kanan maju
Najib_irvani 43:3807a95aa284 488 motorDpn.speed(-serong);
Najib_irvani 44:452c214d9cf5 489 motorL.speed(-serong);
Najib_irvani 43:3807a95aa284 490 motorBlk.speed(serong);
Najib_irvani 44:452c214d9cf5 491 motorR.speed(serong);
Najib_irvani 43:3807a95aa284 492 break;
Najib_irvani 43:3807a95aa284 493 }
Najib_irvani 43:3807a95aa284 494 case (14) :
Najib_irvani 43:3807a95aa284 495 {
Najib_irvani 43:3807a95aa284 496 // Serong kiri maju
Najib_irvani 43:3807a95aa284 497 motorDpn.speed(serong);
Najib_irvani 44:452c214d9cf5 498 motorR.speed(serong);
Najib_irvani 43:3807a95aa284 499 motorBlk.speed(-serong);
Najib_irvani 44:452c214d9cf5 500 motorL.speed(-serong);
Najib_irvani 43:3807a95aa284 501 break;
Najib_irvani 43:3807a95aa284 502 }
Najib_irvani 43:3807a95aa284 503 case (15) :
Najib_irvani 43:3807a95aa284 504 {
Najib_irvani 43:3807a95aa284 505 // Serong kanan mundur
Najib_irvani 44:452c214d9cf5 506 motorDpn.speed(-serong);
Najib_irvani 44:452c214d9cf5 507 motorR.speed(-serong);
Najib_irvani 43:3807a95aa284 508 motorBlk.speed(serong);
Najib_irvani 44:452c214d9cf5 509 motorL.speed(serong);
Najib_irvani 43:3807a95aa284 510 break;
Najib_irvani 43:3807a95aa284 511 }
Najib_irvani 43:3807a95aa284 512 case (16) :
Najib_irvani 43:3807a95aa284 513 {
Najib_irvani 43:3807a95aa284 514 // Serong kiri mundur
Najib_irvani 43:3807a95aa284 515 motorDpn.speed(serong);
Najib_irvani 44:452c214d9cf5 516 motorL.speed(serong);
Najib_irvani 43:3807a95aa284 517 motorBlk.speed(-serong);
Najib_irvani 44:452c214d9cf5 518 motorR.speed(-serong);
gustavaditya 31:d5cbda07fd95 519 break;
gustavaditya 31:d5cbda07fd95 520 }
gustavaditya 31:d5cbda07fd95 521 case (3) :
gustavaditya 31:d5cbda07fd95 522 {
gustavaditya 31:d5cbda07fd95 523 // Kanan
Najib_irvani 43:3807a95aa284 524 motorDpn.speed(-tuneDpn);
Najib_irvani 43:3807a95aa284 525 motorBlk.speed(tuneBlk);
Najib_irvani 43:3807a95aa284 526 motorR.brake(1);
Najib_irvani 43:3807a95aa284 527 motorL.brake(1);
gustavaditya 31:d5cbda07fd95 528 break;
gustavaditya 31:d5cbda07fd95 529 }
gustavaditya 31:d5cbda07fd95 530 case (4) :
gustavaditya 31:d5cbda07fd95 531 {
gustavaditya 31:d5cbda07fd95 532 // Kiri
Najib_irvani 43:3807a95aa284 533 motorDpn.speed(tuneDpn);
Najib_irvani 43:3807a95aa284 534 motorBlk.speed(-tuneBlk);
Najib_irvani 43:3807a95aa284 535 motorR.brake(1);
Najib_irvani 43:3807a95aa284 536 motorL.brake(1);
Najib_irvani 43:3807a95aa284 537 break;
Najib_irvani 43:3807a95aa284 538 }
Najib_irvani 43:3807a95aa284 539 case (8) :
Najib_irvani 43:3807a95aa284 540 {
Najib_irvani 43:3807a95aa284 541 // Maju
Najib_irvani 43:3807a95aa284 542 motorR.speed(tuneR);
Najib_irvani 43:3807a95aa284 543 motorL.speed(-tuneL);
Najib_irvani 43:3807a95aa284 544 motorDpn.brake(1);
Najib_irvani 43:3807a95aa284 545 motorBlk.brake(1);
Najib_irvani 43:3807a95aa284 546 break;
Najib_irvani 43:3807a95aa284 547 }
Najib_irvani 43:3807a95aa284 548 case (9) :
Najib_irvani 43:3807a95aa284 549 {
Najib_irvani 43:3807a95aa284 550 // Mundur
Najib_irvani 43:3807a95aa284 551 motorR.speed(-tuneR);
Najib_irvani 43:3807a95aa284 552 motorL.speed(tuneL);
Najib_irvani 43:3807a95aa284 553 motorDpn.brake(1);
Najib_irvani 43:3807a95aa284 554 motorBlk.brake(1);
gustavaditya 31:d5cbda07fd95 555 break;
gustavaditya 31:d5cbda07fd95 556 }
gustavaditya 31:d5cbda07fd95 557 case (5) :
gustavaditya 31:d5cbda07fd95 558 {
gustavaditya 31:d5cbda07fd95 559 // Aktifkan motor atas
gustavaditya 31:d5cbda07fd95 560 isLauncher = !isLauncher;
gustavaditya 31:d5cbda07fd95 561 break;
Joshua23 8:0711dea61312 562 }
gustavaditya 31:d5cbda07fd95 563 case (6) :
gustavaditya 31:d5cbda07fd95 564 {
gustavaditya 31:d5cbda07fd95 565 // Target Pulse PID ++ Motor Depan
gustavaditya 39:11358f3f61ff 566 target_rpm2 = target_rpm2+1.0;
gustavaditya 39:11358f3f61ff 567 target_rpm = target_rpm+1.0;
gustavaditya 37:67d54563af90 568 init_rpm();
gustavaditya 31:d5cbda07fd95 569 break;
gustavaditya 31:d5cbda07fd95 570 }
gustavaditya 31:d5cbda07fd95 571 case (7) :
gustavaditya 31:d5cbda07fd95 572 {
gustavaditya 31:d5cbda07fd95 573 // Target Pulse PID -- Motor Depan
gustavaditya 39:11358f3f61ff 574 target_rpm2 = target_rpm2-1.0;
gustavaditya 39:11358f3f61ff 575 target_rpm = target_rpm-1.0;
gustavaditya 37:67d54563af90 576 init_rpm();
gustavaditya 31:d5cbda07fd95 577 break;
gustavaditya 31:d5cbda07fd95 578 }
gustavaditya 31:d5cbda07fd95 579 case (10) :
gustavaditya 31:d5cbda07fd95 580 {
gustavaditya 31:d5cbda07fd95 581 // Pneumatik
Najib_irvani 44:452c214d9cf5 582 if (ready)
Najib_irvani 44:452c214d9cf5 583 {
Najib_irvani 44:452c214d9cf5 584 pneumatik = 0;
Najib_irvani 44:452c214d9cf5 585 previousMillis3 = millis();
Najib_irvani 44:452c214d9cf5 586 flag_Pneu = true;
Najib_irvani 44:452c214d9cf5 587 ready = false;
Najib_irvani 44:452c214d9cf5 588 }
gustavaditya 31:d5cbda07fd95 589 break;
gustavaditya 31:d5cbda07fd95 590 }
gustavaditya 31:d5cbda07fd95 591 case (11) :
gustavaditya 31:d5cbda07fd95 592 {
gustavaditya 31:d5cbda07fd95 593 // Power Screw Up
gustavaditya 40:5b937cac959a 594 ReloadOn = !ReloadOn;
Najib_irvani 43:3807a95aa284 595 isReload = false;
gustavaditya 31:d5cbda07fd95 596 break;
gustavaditya 31:d5cbda07fd95 597 }
Najib_irvani 44:452c214d9cf5 598 case (31) :
Najib_irvani 44:452c214d9cf5 599 {
Najib_irvani 44:452c214d9cf5 600 // start
Najib_irvani 44:452c214d9cf5 601 target_rpm2 = 22;
Najib_irvani 44:452c214d9cf5 602 target_rpm = 22;
Najib_irvani 44:452c214d9cf5 603 init_rpm();
Najib_irvani 44:452c214d9cf5 604 break;
Najib_irvani 44:452c214d9cf5 605 }
Najib_irvani 44:452c214d9cf5 606 case (32) :
Najib_irvani 44:452c214d9cf5 607 {
Najib_irvani 44:452c214d9cf5 608 // select
Najib_irvani 44:452c214d9cf5 609 target_rpm2 = 10;
Najib_irvani 44:452c214d9cf5 610 target_rpm = 10;
Najib_irvani 44:452c214d9cf5 611 init_rpm();
Najib_irvani 44:452c214d9cf5 612 break;
Najib_irvani 44:452c214d9cf5 613 }
Najib_irvani 44:452c214d9cf5 614 case (33) :
Najib_irvani 44:452c214d9cf5 615 {
Najib_irvani 44:452c214d9cf5 616 // R3
Najib_irvani 44:452c214d9cf5 617 target_rpm2 = 17;
Najib_irvani 44:452c214d9cf5 618 target_rpm = 17;
Najib_irvani 44:452c214d9cf5 619 init_rpm();
Najib_irvani 44:452c214d9cf5 620 break;
Najib_irvani 44:452c214d9cf5 621 }
gustavaditya 31:d5cbda07fd95 622 case (12) :
gustavaditya 31:d5cbda07fd95 623 {
gustavaditya 31:d5cbda07fd95 624 // Power Screw Down
Najib_irvani 43:3807a95aa284 625 ReloadOn = !ReloadOn;
Najib_irvani 43:3807a95aa284 626 isReload = true;
gustavaditya 31:d5cbda07fd95 627 break;
gustavaditya 31:d5cbda07fd95 628 }
gustavaditya 31:d5cbda07fd95 629 default :
gustavaditya 31:d5cbda07fd95 630 {
gustavaditya 31:d5cbda07fd95 631 motorDpn.brake(1);
gustavaditya 31:d5cbda07fd95 632 motorBlk.brake(1);
Najib_irvani 43:3807a95aa284 633 motorR.brake(1);
Najib_irvani 43:3807a95aa284 634 motorL.brake(1);
gustavaditya 31:d5cbda07fd95 635 }
gustavaditya 31:d5cbda07fd95 636 } // End Switch
gustavaditya 31:d5cbda07fd95 637 }
gustavaditya 31:d5cbda07fd95 638
gustavaditya 40:5b937cac959a 639 void reloader()
rizqicahyo 36:5963c9a49485 640 {
rizqicahyo 36:5963c9a49485 641 if(ReloadOn){
rizqicahyo 36:5963c9a49485 642 if(isReload){
rizqicahyo 36:5963c9a49485 643 powerScrew.speed(pwmPowerDown);
Najib_irvani 44:452c214d9cf5 644 if((!limitBawah)||(!limitBawah1)){
rizqicahyo 36:5963c9a49485 645 isReload = false;
rizqicahyo 36:5963c9a49485 646 ReloadOn = false;
rizqicahyo 36:5963c9a49485 647 }
rizqicahyo 36:5963c9a49485 648 }
rizqicahyo 36:5963c9a49485 649 else if(!limitTengah){
rizqicahyo 36:5963c9a49485 650 isReload = true;
rizqicahyo 36:5963c9a49485 651 }
Najib_irvani 43:3807a95aa284 652 else if((jarak_ping > 6.5) && !flag_Pneu){
gustavaditya 40:5b937cac959a 653 powerScrew.speed(pwmPowerUp);
Najib_irvani 44:452c214d9cf5 654 ready = false;
gustavaditya 40:5b937cac959a 655 }
Najib_irvani 44:452c214d9cf5 656 else if((jarak_ping < 6.0) && !flag_Pneu) {
Najib_irvani 44:452c214d9cf5 657 powerScrew.speed(-0.85);
Najib_irvani 44:452c214d9cf5 658 ready = false;
rizqicahyo 36:5963c9a49485 659 }
rizqicahyo 36:5963c9a49485 660 else{
gustavaditya 40:5b937cac959a 661 powerScrew.brake(1);
Najib_irvani 44:452c214d9cf5 662 ready = true;
rizqicahyo 36:5963c9a49485 663 }
rizqicahyo 36:5963c9a49485 664 }
rizqicahyo 36:5963c9a49485 665 else{
rizqicahyo 36:5963c9a49485 666 powerScrew.brake(1);
rizqicahyo 36:5963c9a49485 667 }
gustavaditya 40:5b937cac959a 668 }
rizqicahyo 36:5963c9a49485 669
rizqicahyo 36:5963c9a49485 670
gustavaditya 31:d5cbda07fd95 671 void launcher()
gustavaditya 31:d5cbda07fd95 672 {
gustavaditya 31:d5cbda07fd95 673 if (isLauncher)
Sufa 30:d69cc27ac644 674 {
be_bryan 28:2d0746dc2d7d 675 currentMillis = millis();
be_bryan 28:2d0746dc2d7d 676 currentMillis2 = millis();
be_bryan 28:2d0746dc2d7d 677
gustavaditya 31:d5cbda07fd95 678 // PID Launcher Depan
gustavaditya 38:3ef6754bd8d8 679 if (currentMillis-previousMillis>=12.5)
be_bryan 26:256160a1a82d 680 {
gustavaditya 31:d5cbda07fd95 681 rpm = (float)encLauncherBlk.getPulses();
be_bryan 26:256160a1a82d 682 current_error = target_rpm - rpm;
Najib_irvani 44:452c214d9cf5 683 sum_error = sum_error + current_error*12.5;
be_bryan 26:256160a1a82d 684 p = current_error*kpA;
gustavaditya 38:3ef6754bd8d8 685 d = (current_error-last_error)*kdA/12.5;
Najib_irvani 44:452c214d9cf5 686 i = sum_error*kiA;
be_bryan 26:256160a1a82d 687 speed = p + d + i;
gustavaditya 38:3ef6754bd8d8 688 //init_speed();
gustavaditya 38:3ef6754bd8d8 689 if(speed > maxSpeed){
gustavaditya 38:3ef6754bd8d8 690 launcherBlk.speed(maxSpeed);
gustavaditya 38:3ef6754bd8d8 691 }
gustavaditya 39:11358f3f61ff 692 else if ( speed < minSpeed){
gustavaditya 39:11358f3f61ff 693 launcherBlk.speed(minSpeed);
gustavaditya 39:11358f3f61ff 694 }
gustavaditya 38:3ef6754bd8d8 695 else {
gustavaditya 38:3ef6754bd8d8 696 launcherBlk.speed(speed);
gustavaditya 38:3ef6754bd8d8 697 }
be_bryan 26:256160a1a82d 698 last_error = current_error;
gustavaditya 31:d5cbda07fd95 699 encLauncherBlk.reset();
be_bryan 26:256160a1a82d 700 //pc.printf("%.04lf\n",rpm);
be_bryan 26:256160a1a82d 701 previousMillis = currentMillis;
be_bryan 26:256160a1a82d 702 }
gustavaditya 38:3ef6754bd8d8 703 if (currentMillis2-previousMillis2>=12.5)
be_bryan 27:68efb1622985 704 {
gustavaditya 31:d5cbda07fd95 705 rpm2 = (float)encLauncherDpn.getPulses();
be_bryan 27:68efb1622985 706 current_error2 = target_rpm2 - rpm2;
Najib_irvani 44:452c214d9cf5 707 sum_error2 = sum_error2 + current_error2*12.5;
be_bryan 27:68efb1622985 708 p2 = current_error2*kpA;
gustavaditya 38:3ef6754bd8d8 709 d2 = (current_error2-last_error2)*kdA/12.5;
Najib_irvani 44:452c214d9cf5 710 i2 = sum_error2*kiA;
be_bryan 27:68efb1622985 711 speed2 = p2 + d2 + i2;
gustavaditya 38:3ef6754bd8d8 712 //init_speed();
gustavaditya 38:3ef6754bd8d8 713 if (speed2 > maxSpeed){
gustavaditya 38:3ef6754bd8d8 714 launcherDpn.speed(maxSpeed);
gustavaditya 38:3ef6754bd8d8 715 }
gustavaditya 39:11358f3f61ff 716 else if ( speed < minSpeed){
gustavaditya 39:11358f3f61ff 717 launcherDpn.speed(minSpeed);
gustavaditya 39:11358f3f61ff 718 }
gustavaditya 38:3ef6754bd8d8 719 else{
gustavaditya 38:3ef6754bd8d8 720 launcherDpn.speed(speed2);
gustavaditya 38:3ef6754bd8d8 721 }
be_bryan 27:68efb1622985 722 last_error2 = current_error2;
gustavaditya 31:d5cbda07fd95 723 encLauncherDpn.reset();
be_bryan 27:68efb1622985 724 previousMillis2 = currentMillis2;
be_bryan 27:68efb1622985 725 }
Najib_irvani 44:452c214d9cf5 726 pc.printf("%.2f\t%.2f\n",rpm,rpm2);
rahmadirizki18 6:68293bed71ea 727 }
Sufa 29:7b372b0aaa61 728 else
Sufa 29:7b372b0aaa61 729 {
gustavaditya 31:d5cbda07fd95 730 launcherDpn.brake(1);
gustavaditya 31:d5cbda07fd95 731 launcherBlk.brake(1);
Najib_irvani 44:452c214d9cf5 732 sum_error = 0;
Najib_irvani 44:452c214d9cf5 733 sum_error2 = 0;
Najib_irvani 44:452c214d9cf5 734 current_error = 0;
Najib_irvani 44:452c214d9cf5 735 current_error2 = 0;
Najib_irvani 44:452c214d9cf5 736 last_error = 0;
Najib_irvani 44:452c214d9cf5 737 last_error2 = 0;
gustavaditya 31:d5cbda07fd95 738 }
rahmadirizki18 5:3aa203218306 739 }
gustavaditya 31:d5cbda07fd95 740
MarchioKevin 22:4632f58461e0 741 /*********************************************************/
MarchioKevin 22:4632f58461e0 742 /* Main Function */
MarchioKevin 22:4632f58461e0 743 /*********************************************************/
calmantara186 16:90119f03c5d1 744
gustavaditya 31:d5cbda07fd95 745 int main (void)
gustavaditya 31:d5cbda07fd95 746 {
gustavaditya 31:d5cbda07fd95 747 // Set baud rate - 115200
fanny868 0:9072e932503c 748 joystick.setup();
rahmadirizki18 23:023b522977b2 749 pc.baud(115200);
rahmadirizki18 6:68293bed71ea 750 wait_ms(1000);
gustavaditya 40:5b937cac959a 751
gustavaditya 40:5b937cac959a 752 // initializing encoder
gustavaditya 41:336a19289c2d 753 pneumatik =1;
gustavaditya 41:336a19289c2d 754
rahmadirizki18 5:3aa203218306 755 setCenter();
gustavaditya 41:336a19289c2d 756
rahmadirizki18 5:3aa203218306 757 wait_ms(500);
gustavaditya 40:5b937cac959a 758
gustavaditya 40:5b937cac959a 759 //initializing PING
gustavaditya 40:5b937cac959a 760 pingAtas.Send();
gustavaditya 40:5b937cac959a 761
rahmadirizki18 23:023b522977b2 762 pc.printf("ready....");
gustavaditya 35:69a47b4cb3fc 763 startMillis();
fanny868 0:9072e932503c 764 while(1)
gustavaditya 40:5b937cac959a 765 {
gustavaditya 40:5b937cac959a 766 // interupsi pembacaan PING setiap 30 ms
gustavaditya 41:336a19289c2d 767 if(millis() - previousMillis4 >= 5){ //30
gustavaditya 41:336a19289c2d 768 jarak_ping = (float)pingAtas.Read_cm()/2;
gustavaditya 40:5b937cac959a 769
gustavaditya 40:5b937cac959a 770 pingAtas.Send();
gustavaditya 40:5b937cac959a 771 previousMillis4 = millis();
gustavaditya 40:5b937cac959a 772 }
gustavaditya 40:5b937cac959a 773
fanny868 0:9072e932503c 774 // Interrupt Serial
calmantara186 16:90119f03c5d1 775 joystick.idle();
gustavaditya 31:d5cbda07fd95 776 if(joystick.readable())
gustavaditya 31:d5cbda07fd95 777 {
fanny868 0:9072e932503c 778 // Panggil fungsi pembacaan joystik
fanny868 0:9072e932503c 779 joystick.baca_data();
fanny868 0:9072e932503c 780 // Panggil fungsi pengolahan data joystik
fanny868 0:9072e932503c 781 joystick.olah_data();
gustavaditya 31:d5cbda07fd95 782 // Masuk ke case joystick
gustavaditya 31:d5cbda07fd95 783 case_joy = case_joystick();
Najib_irvani 44:452c214d9cf5 784 //pc.printf("%d\n",case_joy);
rahmadirizki18 3:1287fccc11be 785 aktuator();
gustavaditya 31:d5cbda07fd95 786 launcher();
gustavaditya 40:5b937cac959a 787 reloader();
Najib_irvani 43:3807a95aa284 788 if ((millis()-previousMillis3 >= 320)&&(flag_Pneu)){
be_bryan 26:256160a1a82d 789 pneumatik = 1;
gustavaditya 31:d5cbda07fd95 790 flag_Pneu = false;
be_bryan 26:256160a1a82d 791 }
be_bryan 26:256160a1a82d 792 }
gustavaditya 31:d5cbda07fd95 793 else
gustavaditya 31:d5cbda07fd95 794 {
gustavaditya 31:d5cbda07fd95 795 joystick.idle();
MarchioKevin 21:da2f3d04468f 796 }
franshendri 42:6caf8bd5abbc 797
Najib_irvani 44:452c214d9cf5 798 if(millis() - previousMillis5 >= 400)
Najib_irvani 44:452c214d9cf5 799 {
Najib_irvani 43:3807a95aa284 800 display.write(0,((int) rpm2) / 10);
Najib_irvani 43:3807a95aa284 801 display.write(1,((int)rpm2) % 10);
franshendri 42:6caf8bd5abbc 802 display.write(2, (int)target_rpm2 / 10);
franshendri 42:6caf8bd5abbc 803 display.write(3, (int)target_rpm2 % 10);
franshendri 42:6caf8bd5abbc 804 display.setColon(true);
franshendri 42:6caf8bd5abbc 805
franshendri 42:6caf8bd5abbc 806 previousMillis5 = millis();
franshendri 42:6caf8bd5abbc 807 }
fanny868 0:9072e932503c 808 }
be_bryan 28:2d0746dc2d7d 809 }