KRAI 2017 / Mbed 2 deprecated DagonFly__RoadToJapan_15Mei_A

dear josh

Dependencies:   DigitDisplay Motor PID Ping mbed millis

Fork of DagonFly__RoadToJapan_15Mei by KRAI 2017

main.cpp@46:85169ae8659b, 2017-04-24 (annotated)

Committer:
Najib_irvani
Date:
Mon Apr 24 14:12:08 2017 +0000
Revision:
46:85169ae8659b
Parent:
45:964ae71a30e3
Child:
47:6cac4f1a3c8e
paling baru

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