KRTMI 2019 / Mbed 2 deprecated HAHAHA

Kodingan KRAI 2017

Dependencies:   mbed DigitDisplay PID Motor Ping millis

main.cpp@0:dd4c20b9a83e, 2019-02-24 (annotated)

Committer:
SalbiFaza
Date:
Sun Feb 24 10:39:24 2019 +0000
Revision:
0:dd4c20b9a83e
bismillah

Who changed what in which revision?

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