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@53:9f8af4534481, 2017-06-07 (annotated)

Committer:
Najib_irvani
Date:
Wed Jun 07 10:23:30 2017 +0000
Revision:
53:9f8af4534481
Parent:
52:876ff6bdff3c 
this is disco time

Who changed what in which revision?

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