KRAI 2017 / Mbed 2 deprecated DagonFly__RoadToJapan_15Mei_Ultimate

Fix pisan inimah plis jangan revisi ultimate mantep

Dependencies:   DigitDisplay Motor PID mbed millis

Fork of DagonFly__RoadToJapan_15Mei by KRAI 2017

main.cpp@52:a39e26b935a9, 2017-07-02 (annotated)

Committer:
be_bryan
Date:
Sun Jul 02 01:37:31 2017 +0000
Revision:
52:a39e26b935a9
Parent:
51:df6391c3fa68 
Convert_KeilToMbed

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"
franshendri 42:6caf8bd5abbc 46 #include "DigitDisplay.h"
franshendri 42:6caf8bd5abbc 47
calmantara186 16:90119f03c5d1 48 /***********************************************/
calmantara186 16:90119f03c5d1 49 /* Konstanta dan Variabel */
calmantara186 16:90119f03c5d1 50 /***********************************************/
calmantara186 16:90119f03c5d1 51 #define PI 3.14159265
be_bryan 26:256160a1a82d 52 #define D_ENCODER 10 // Diameter Roda Encoder
be_bryan 52:a39e26b935a9 53 #define D_ROBOT1 54 // Diameter Roda Robot dari kiri ke kanan
be_bryan 52:a39e26b935a9 54 #define D_ROBOT2 79 // Diameter Roda Robot dari depan ke belakang
rahmadirizki18 5:3aa203218306 55
be_bryan 52:a39e26b935a9 56 /**********************************************************/
be_bryan 52:a39e26b935a9 57 /* LAUNCHER */
be_bryan 52:a39e26b935a9 58 bool isLauncher = false;
be_bryan 52:a39e26b935a9 59 double speed;
be_bryan 52:a39e26b935a9 60 const double maxSpeed = 1.0, minSpeed = -1.0, Ts = 20;
be_bryan 52:a39e26b935a9 61 const double kpA=0.0004667, kdA=0, kiA=6.2645e-07;
be_bryan 52:a39e26b935a9 62 double a,b,c;
be_bryan 52:a39e26b935a9 63 double current_error, previous_error_1 = 0, previous_error_2 = 0;
be_bryan 52:a39e26b935a9 64 double previous_speed = 0;
Najib_irvani 46:85169ae8659b 65
be_bryan 52:a39e26b935a9 66 float rpm;
be_bryan 52:a39e26b935a9 67 double target_rpm = 2600;
be_bryan 52:a39e26b935a9 68 const float maxRPM = 4000, minRPM = 0.0;
be_bryan 52:a39e26b935a9 69 /**********************************************************/
Najib_irvani 46:85169ae8659b 70
be_bryan 52:a39e26b935a9 71 /**********************************************************/
be_bryan 52:a39e26b935a9 72 /* BASE */
be_bryan 52:a39e26b935a9 73 float PIVOT = 0.3; // PWM Pivot Kanan, Pivot Kiri
gustavaditya 45:964ae71a30e3 74 float tuneAksel = 0.6;
be_bryan 52:a39e26b935a9 75 float serong = 0.45;
be_bryan 52:a39e26b935a9 76 float rasio= (D_ROBOT2/D_ROBOT1);
be_bryan 52:a39e26b935a9 77 //float jariR = 15, jariL = 64, jariDpn = 27, jariBlk = 27;
be_bryan 52:a39e26b935a9 78 float K_enc = PI*D_ENCODER;
be_bryan 52:a39e26b935a9 79 float K_robot1 = PI*D_ROBOT1;
be_bryan 52:a39e26b935a9 80 float K_robot2 = PI*D_ROBOT2;
be_bryan 52:a39e26b935a9 81 float speedDpn, speedBlk, speedR, speedL;
be_bryan 52:a39e26b935a9 82 //unsigned long int previousMillis=0;
be_bryan 52:a39e26b935a9 83 float jarakXnow, jarakXbefore, jarakYnow, jarakYbefore; //Untuk aksel
be_bryan 52:a39e26b935a9 84 /* PID BASE */
be_bryan 52:a39e26b935a9 85 float setpointX=0.0, setpointY=0.0, setpointT=0.0;
be_bryan 52:a39e26b935a9 86 const float maxPIDSpeedX = 0.6, minPIDSpeedX = -0.6;
be_bryan 52:a39e26b935a9 87 const float maxPIDSpeedY = 0.6, minPIDSpeedY = -0.6;
be_bryan 52:a39e26b935a9 88 const float maxPIDSpeedT = 0.4, minPIDSpeedT = -0.4;
be_bryan 52:a39e26b935a9 89 const float TsBase = 50;
be_bryan 52:a39e26b935a9 90 double current_efrror, previous_error1 = 0, previous_error2 = 0;
be_bryan 52:a39e26b935a9 91 float PIDSpeedX, PIDSpeedY, PIDSpeedT;
be_bryan 52:a39e26b935a9 92 float errorX, previous_errorX=0, derivativeX, integralX=0;
be_bryan 52:a39e26b935a9 93 float errorY, previous_errorY=0, derivativeY, integralY=0;
be_bryan 52:a39e26b935a9 94 float errorT, previous_errorT=0, derivativeT, integralT=0;
be_bryan 52:a39e26b935a9 95 float KpX=0.06, KiX=0, KdX=0.4;
be_bryan 52:a39e26b935a9 96 float KpY=0.06, KiY=0, KdY=0.4;
be_bryan 52:a39e26b935a9 97 float KpT=0.08, KiT=0, KdT=3.33;
be_bryan 52:a39e26b935a9 98 //float KpT=0.08, KiT=0, KdT=0.0;
be_bryan 52:a39e26b935a9 99 // Otomatis sb X
be_bryan 52:a39e26b935a9 100 bool autoX = false, auto_rotate = false;
be_bryan 52:a39e26b935a9 101 bool reset_encoder, inAutoRotate;
be_bryan 52:a39e26b935a9 102 int mode=0;
be_bryan 52:a39e26b935a9 103 /**********************************************************/
Najib_irvani 43:3807a95aa284 104
be_bryan 52:a39e26b935a9 105 /**********************************************************/
be_bryan 52:a39e26b935a9 106 /* RELOADER */
gustavaditya 31:d5cbda07fd95 107 bool isReload = false;
rizqicahyo 36:5963c9a49485 108 bool ReloadOn = false;
gustavaditya 31:d5cbda07fd95 109 bool flag_Pneu = false;
be_bryan 52:a39e26b935a9 110 bool readySlideFromLeft = false;
be_bryan 52:a39e26b935a9 111 bool readySlideFromMiddle = false;
be_bryan 52:a39e26b935a9 112 bool getBack = false;
be_bryan 52:a39e26b935a9 113 bool isDown = false, sliderOn = false;
be_bryan 52:a39e26b935a9 114 bool ready = true;
be_bryan 52:a39e26b935a9 115 bool init_slider = true;
be_bryan 52:a39e26b935a9 116 bool init_lifter = true;
be_bryan 52:a39e26b935a9 117 bool sliderReady = false;
be_bryan 52:a39e26b935a9 118 bool flag_tengah = true;
be_bryan 52:a39e26b935a9 119 bool delay = false;
be_bryan 52:a39e26b935a9 120 float lempar1 = -0.8, lempar2 = -0.85, balik = 0.7;
be_bryan 52:a39e26b935a9 121 float lempar_naik1 = -0.85, lempar2_naik = -0.8;
be_bryan 52:a39e26b935a9 122 float swipper_dorong1, swipper_dorong2;
be_bryan 52:a39e26b935a9 123 bool slideNowLeft = false, slideNowMid = false;
be_bryan 52:a39e26b935a9 124 bool naik = false;
be_bryan 52:a39e26b935a9 125 /**********************************************************/
gustavaditya 31:d5cbda07fd95 126
be_bryan 52:a39e26b935a9 127 /**********************************************************/
be_bryan 52:a39e26b935a9 128 /* MILLIS */
be_bryan 52:a39e26b935a9 129 static volatile uint32_t previousMillis = 0;
be_bryan 52:a39e26b935a9 130 static volatile uint32_t previousMillis3 = 0; // Pneumatik
be_bryan 52:a39e26b935a9 131 static volatile uint32_t previousMillis5 = 0; // Display
be_bryan 52:a39e26b935a9 132 static volatile uint32_t previousMillis6 = 0; // pneu
be_bryan 52:a39e26b935a9 133 static volatile uint32_t previousMillis7 = 0; // delay
be_bryan 52:a39e26b935a9 134 static volatile uint32_t currentMillis;
be_bryan 52:a39e26b935a9 135 static volatile uint32_t previousMillisAutoX=0; // Otomatis X
be_bryan 52:a39e26b935a9 136 /**********************************************************/
gustavaditya 31:d5cbda07fd95 137
be_bryan 52:a39e26b935a9 138 /**********************************************************/
be_bryan 52:a39e26b935a9 139 /* JOYSTICK */
be_bryan 52:a39e26b935a9 140 bool flag_LXmax = true, flag_LXmin = true, flag_LYmax = true, flag_LYmin = true;
be_bryan 52:a39e26b935a9 141 int case_joy;
be_bryan 52:a39e26b935a9 142 //int debug = 0;
be_bryan 52:a39e26b935a9 143 bool awal = true;
be_bryan 52:a39e26b935a9 144 /**********************************************************/
be_bryan 26:256160a1a82d 145
be_bryan 52:a39e26b935a9 146 /**********************************************************/
be_bryan 52:a39e26b935a9 147 /* Motor */
be_bryan 52:a39e26b935a9 148 Motor motorDpn(PB_6, PA_10, PB_13);
be_bryan 52:a39e26b935a9 149 Motor motorBlk(PB_7, PA_8, PB_1);
be_bryan 52:a39e26b935a9 150 Motor motorL(PB_9, PA_11, PA_12);
be_bryan 52:a39e26b935a9 151 Motor motorR(PB_8, PA_9, PA_5);
be_bryan 52:a39e26b935a9 152 Motor Launcher(PA_6, PB_2, PB_12);
be_bryan 52:a39e26b935a9 153 Motor powerScrew(PA_7, PB_14, PB_15);
be_bryan 52:a39e26b935a9 154 Motor swipper(PB_0, PA_4, PA_13);
be_bryan 52:a39e26b935a9 155 /**********************************************************/
fanny868 0:9072e932503c 156
be_bryan 52:a39e26b935a9 157 /**********************************************************/
be_bryan 52:a39e26b935a9 158 /* Encoder */
be_bryan 52:a39e26b935a9 159 encoderKRAI Benc_R(PC_5, PC_4, 540, encoderKRAI::X4_ENCODING);
be_bryan 52:a39e26b935a9 160 encoderKRAI Benc_L(PC_7, PC_6, 540, encoderKRAI::X4_ENCODING);
be_bryan 52:a39e26b935a9 161 encoderKRAI Benc_Dpn(PC_1, PC_0, 540, encoderKRAI::X4_ENCODING);
be_bryan 52:a39e26b935a9 162 encoderKRAI Benc_Blk(PC_3, PC_2, 540, encoderKRAI::X4_ENCODING);
be_bryan 52:a39e26b935a9 163 encoderKRAI Lenc(PC_9, PC_8, 1000, encoderKRAI::X4_ENCODING);
be_bryan 52:a39e26b935a9 164
be_bryan 52:a39e26b935a9 165 /**********************************************************/
rahmadirizki18 5:3aa203218306 166
be_bryan 52:a39e26b935a9 167 /**********************************************************/
be_bryan 52:a39e26b935a9 168 /* Display 7 Segmen */
be_bryan 52:a39e26b935a9 169 DigitDisplay display (PC_12, PB_10);
be_bryan 52:a39e26b935a9 170 /**********************************************************/
be_bryan 26:256160a1a82d 171
be_bryan 52:a39e26b935a9 172 /**********************************************************/
be_bryan 52:a39e26b935a9 173 /* Limit Switch */
be_bryan 52:a39e26b935a9 174 DigitalIn limitKanan(PD_2, PullUp);
be_bryan 52:a39e26b935a9 175 DigitalIn limitTengah(PB_4, PullUp);
be_bryan 52:a39e26b935a9 176 DigitalIn limitKiri(PB_5, PullUp);
be_bryan 52:a39e26b935a9 177 DigitalIn limitBawah(PC_13, PullUp);
be_bryan 52:a39e26b935a9 178 DigitalIn limitAtas(PC_14, PullUp);
be_bryan 52:a39e26b935a9 179 DigitalIn limitFB(PC_15, PullUp);
be_bryan 52:a39e26b935a9 180 /**********************************************************/
rahmadirizki18 5:3aa203218306 181
be_bryan 52:a39e26b935a9 182 /**********************************************************/
be_bryan 52:a39e26b935a9 183 /* Pneumatik */
be_bryan 52:a39e26b935a9 184 DigitalOut Pneumatik(PA_14, PullUp);
be_bryan 52:a39e26b935a9 185 DigitalOut Elevator(PA_15, PullUp);
be_bryan 52:a39e26b935a9 186 /****************************************************/
fanny868 0:9072e932503c 187
be_bryan 52:a39e26b935a9 188 /**********************************************************/
be_bryan 52:a39e26b935a9 189 /* Serial */
be_bryan 52:a39e26b935a9 190 Serial pc(USBTX,USBRX);
be_bryan 52:a39e26b935a9 191 joysticknucleo joystick(PA_0,PA_1);
be_bryan 52:a39e26b935a9 192 /**********************************************************/
franshendri 42:6caf8bd5abbc 193
MarchioKevin 22:4632f58461e0 194 /* Deklarasi Fungsi dan Procedure */
MarchioKevin 22:4632f58461e0 195 /****************************************************/
be_bryan 52:a39e26b935a9 196
be_bryan 52:a39e26b935a9 197 void setCenter()
be_bryan 52:a39e26b935a9 198 {
be_bryan 52:a39e26b935a9 199 Benc_R.reset();
be_bryan 52:a39e26b935a9 200 Benc_L.reset();
be_bryan 52:a39e26b935a9 201 Benc_Dpn.reset();
be_bryan 52:a39e26b935a9 202 Benc_Blk.reset();
be_bryan 52:a39e26b935a9 203 Lenc.reset();
be_bryan 52:a39e26b935a9 204 }
be_bryan 52:a39e26b935a9 205
be_bryan 52:a39e26b935a9 206 float getX()
be_bryan 52:a39e26b935a9 207 {
be_bryan 52:a39e26b935a9 208 float jarakEncDpn, jarakEncBlk;
be_bryan 52:a39e26b935a9 209 jarakEncDpn = (Benc_Dpn.getPulses())/(float)(540.0)*K_enc;
be_bryan 52:a39e26b935a9 210 jarakEncBlk = (Benc_Blk.getPulses())/(float)(540.0)*K_enc;
be_bryan 52:a39e26b935a9 211 return (jarakEncDpn-jarakEncBlk)/2;
be_bryan 52:a39e26b935a9 212 }
be_bryan 52:a39e26b935a9 213
be_bryan 52:a39e26b935a9 214 float getY()
be_bryan 52:a39e26b935a9 215 {
be_bryan 52:a39e26b935a9 216 float jarakEncKir, jarakEncKan;
be_bryan 52:a39e26b935a9 217 jarakEncKir = (Benc_L.getPulses())/(float)(540.0)*K_enc;
be_bryan 52:a39e26b935a9 218 jarakEncKan = (Benc_R.getPulses())/(float)(540.0)*K_enc;
be_bryan 52:a39e26b935a9 219 return (jarakEncKir-jarakEncKan)/2;
be_bryan 52:a39e26b935a9 220 }
be_bryan 52:a39e26b935a9 221
be_bryan 52:a39e26b935a9 222 float getTetha()
be_bryan 52:a39e26b935a9 223 {
be_bryan 52:a39e26b935a9 224 float busurDpn, busurBlk, busurKir, busurKan, sudut;
be_bryan 52:a39e26b935a9 225 busurKir = ((Benc_L.getPulses())/(float)(540.0)*K_enc)/K_robot2*360.0;
be_bryan 52:a39e26b935a9 226 busurKan = ((Benc_R.getPulses())/(float)(540.0)*K_enc)/K_robot2*360.0;
be_bryan 52:a39e26b935a9 227 busurDpn = ((Benc_Dpn.getPulses())/(float)(540.0)*K_enc)/K_robot1*360.0;
be_bryan 52:a39e26b935a9 228 busurBlk = ((Benc_Blk.getPulses())/(float)(540.0)*K_enc)/K_robot1*360.0;
be_bryan 52:a39e26b935a9 229 sudut = -(busurDpn+busurBlk+busurKir+busurKan)/4;
be_bryan 52:a39e26b935a9 230 if (sudut>=360.0 || sudut<=-360.0)
be_bryan 52:a39e26b935a9 231 { sudut = 0.0; }
be_bryan 52:a39e26b935a9 232 return sudut;
be_bryan 52:a39e26b935a9 233 }
be_bryan 52:a39e26b935a9 234
be_bryan 52:a39e26b935a9 235 void PIDcompute_X()
be_bryan 52:a39e26b935a9 236 {
be_bryan 52:a39e26b935a9 237 errorX = setpointX - getX();
be_bryan 52:a39e26b935a9 238 integralX = integralX + errorX*TsBase;
be_bryan 52:a39e26b935a9 239 derivativeX = (errorX - previous_errorX)/TsBase;
be_bryan 52:a39e26b935a9 240 PIDSpeedX = KpX*errorY + KiX*integralY + KdX*derivativeX;
be_bryan 52:a39e26b935a9 241 if(PIDSpeedX > maxPIDSpeedX)
be_bryan 52:a39e26b935a9 242 { PIDSpeedX = maxPIDSpeedX; }
be_bryan 52:a39e26b935a9 243 else if (PIDSpeedX < minPIDSpeedX)
be_bryan 52:a39e26b935a9 244 { PIDSpeedX = minPIDSpeedX; }
be_bryan 52:a39e26b935a9 245 previous_errorX = errorX;
be_bryan 52:a39e26b935a9 246 }
be_bryan 52:a39e26b935a9 247
be_bryan 52:a39e26b935a9 248 void PIDcompute_Y()
be_bryan 52:a39e26b935a9 249 {
be_bryan 52:a39e26b935a9 250 errorY = setpointY - getY();
be_bryan 52:a39e26b935a9 251 integralY = integralY + errorY*TsBase;
be_bryan 52:a39e26b935a9 252 derivativeY = (errorY - previous_errorY)/TsBase;
be_bryan 52:a39e26b935a9 253 PIDSpeedY = KpY*errorY + KiY*integralY + KdY*derivativeY;
be_bryan 52:a39e26b935a9 254 if(PIDSpeedY > maxPIDSpeedY)
be_bryan 52:a39e26b935a9 255 { PIDSpeedY = maxPIDSpeedY; }
be_bryan 52:a39e26b935a9 256 else if (PIDSpeedY < minPIDSpeedY)
be_bryan 52:a39e26b935a9 257 { PIDSpeedY = minPIDSpeedY; }
be_bryan 52:a39e26b935a9 258 previous_errorY = errorY;
be_bryan 52:a39e26b935a9 259 }
be_bryan 52:a39e26b935a9 260
be_bryan 52:a39e26b935a9 261 void PIDcompute_T()
be_bryan 52:a39e26b935a9 262 {
be_bryan 52:a39e26b935a9 263 errorT = setpointT - getTetha();
be_bryan 52:a39e26b935a9 264 integralT = integralT + errorT*TsBase;
be_bryan 52:a39e26b935a9 265 derivativeT = (errorT - previous_errorT)/TsBase;
be_bryan 52:a39e26b935a9 266 PIDSpeedT = KpT*errorT + KiT*integralT + KdT*derivativeT;
be_bryan 52:a39e26b935a9 267 if(PIDSpeedT > maxPIDSpeedT)
be_bryan 52:a39e26b935a9 268 { PIDSpeedT = maxPIDSpeedT; }
be_bryan 52:a39e26b935a9 269 else if (PIDSpeedT < minPIDSpeedT)
be_bryan 52:a39e26b935a9 270 { PIDSpeedT = minPIDSpeedT; }
be_bryan 52:a39e26b935a9 271 previous_errorT = errorT;
be_bryan 52:a39e26b935a9 272 }
be_bryan 52:a39e26b935a9 273
gustavaditya 31:d5cbda07fd95 274 int case_joystick()
gustavaditya 31:d5cbda07fd95 275 {
gustavaditya 31:d5cbda07fd95 276 int caseJoystick;
be_bryan 52:a39e26b935a9 277 if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
gustavaditya 31:d5cbda07fd95 278 // Pivot Kanan
gustavaditya 31:d5cbda07fd95 279 caseJoystick = 1;
be_bryan 52:a39e26b935a9 280 }
be_bryan 52:a39e26b935a9 281 else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
gustavaditya 31:d5cbda07fd95 282 // Pivot Kiri
gustavaditya 31:d5cbda07fd95 283 caseJoystick = 2;
Najib_irvani 43:3807a95aa284 284 }
be_bryan 52:a39e26b935a9 285 else if ((joystick.START_click)&&(!joystick.SELECT_click)&&(!joystick.R3_click)) {
Najib_irvani 44:452c214d9cf5 286 // tambah rpm dengan nilai tertentu
be_bryan 52:a39e26b935a9 287 caseJoystick = 31;
Najib_irvani 44:452c214d9cf5 288 }
be_bryan 52:a39e26b935a9 289 else if ((!joystick.START_click)&&(joystick.SELECT_click)&&(!joystick.R3_click)) {
Najib_irvani 44:452c214d9cf5 290 // kurangi rpm dengan nilai tertentu
be_bryan 52:a39e26b935a9 291 caseJoystick = 32;
Najib_irvani 44:452c214d9cf5 292 }
be_bryan 52:a39e26b935a9 293 else if ((!joystick.START_click)&&(!joystick.SELECT_click)&&(joystick.R3_click)) {
Najib_irvani 44:452c214d9cf5 294 // kurangi rpm dengan nilai tertentu
be_bryan 52:a39e26b935a9 295 caseJoystick = 33;
Najib_irvani 44:452c214d9cf5 296 }
be_bryan 52:a39e26b935a9 297 else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
Najib_irvani 43:3807a95aa284 298 // Kanan + Rotasi kanan
Najib_irvani 43:3807a95aa284 299 caseJoystick = 17;
be_bryan 52:a39e26b935a9 300 }
be_bryan 52:a39e26b935a9 301 else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
Najib_irvani 43:3807a95aa284 302 // Kanan + Rotasi kiri
Najib_irvani 43:3807a95aa284 303 caseJoystick = 18;
Najib_irvani 43:3807a95aa284 304 }
be_bryan 52:a39e26b935a9 305 else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
Najib_irvani 43:3807a95aa284 306 // Kiri + Rotasi kanan
Najib_irvani 43:3807a95aa284 307 caseJoystick = 19;
be_bryan 52:a39e26b935a9 308 }
be_bryan 52:a39e26b935a9 309 else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
Najib_irvani 43:3807a95aa284 310 // Kanan + Rotasi kiri
Najib_irvani 43:3807a95aa284 311 caseJoystick = 20;
Najib_irvani 43:3807a95aa284 312 }
be_bryan 52:a39e26b935a9 313 else if ((joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
Najib_irvani 43:3807a95aa284 314 // Maju + Rotasi kanan
Najib_irvani 43:3807a95aa284 315 caseJoystick = 21;
be_bryan 52:a39e26b935a9 316 }
be_bryan 52:a39e26b935a9 317 else if ((!joystick.R1)&&(joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
Najib_irvani 43:3807a95aa284 318 // Maju + Rotasi kiri
Najib_irvani 43:3807a95aa284 319 caseJoystick = 22;
Najib_irvani 43:3807a95aa284 320 }
be_bryan 52:a39e26b935a9 321 else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
Najib_irvani 43:3807a95aa284 322 // Mundur + Rotasi kanan
Najib_irvani 43:3807a95aa284 323 caseJoystick = 23;
be_bryan 52:a39e26b935a9 324 }
be_bryan 52:a39e26b935a9 325 else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
Najib_irvani 43:3807a95aa284 326 // Mundur + Rotasi kiri
Najib_irvani 43:3807a95aa284 327 caseJoystick = 24;
Najib_irvani 43:3807a95aa284 328 }
be_bryan 52:a39e26b935a9 329 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.segitiga_click)) {
Najib_irvani 43:3807a95aa284 330 // Kanan + segitiga
Najib_irvani 43:3807a95aa284 331 caseJoystick = 25;
Najib_irvani 43:3807a95aa284 332 }
be_bryan 52:a39e26b935a9 333 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.segitiga_click)) {
Najib_irvani 43:3807a95aa284 334 // Kiri + segitiga
Najib_irvani 43:3807a95aa284 335 caseJoystick = 26;
Najib_irvani 43:3807a95aa284 336 }
be_bryan 52:a39e26b935a9 337 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.lingkaran_click)) {
Najib_irvani 43:3807a95aa284 338 // Kanan + lingkaran
Najib_irvani 43:3807a95aa284 339 caseJoystick = 27;
Najib_irvani 43:3807a95aa284 340 }
be_bryan 52:a39e26b935a9 341 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.lingkaran_click)) {
Najib_irvani 43:3807a95aa284 342 // Kiri + lingkaran
Najib_irvani 43:3807a95aa284 343 caseJoystick = 28;
Najib_irvani 43:3807a95aa284 344 }
be_bryan 52:a39e26b935a9 345 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.kotak_click)) {
Najib_irvani 43:3807a95aa284 346 // Kanan + kotak
Najib_irvani 43:3807a95aa284 347 caseJoystick = 29;
Najib_irvani 43:3807a95aa284 348 }
be_bryan 52:a39e26b935a9 349 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.kotak_click)) {
Najib_irvani 43:3807a95aa284 350 // Kiri + kotak
Najib_irvani 43:3807a95aa284 351 caseJoystick = 30;
Najib_irvani 43:3807a95aa284 352 }
be_bryan 52:a39e26b935a9 353 else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
Najib_irvani 43:3807a95aa284 354 // Serong kanan maju
be_bryan 52:a39e26b935a9 355 caseJoystick = 13;
Najib_irvani 43:3807a95aa284 356 }
be_bryan 52:a39e26b935a9 357 else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
Najib_irvani 43:3807a95aa284 358 // Serong kiri maju
be_bryan 52:a39e26b935a9 359 caseJoystick = 14;
Najib_irvani 43:3807a95aa284 360 }
be_bryan 52:a39e26b935a9 361 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
Najib_irvani 43:3807a95aa284 362 // Serong kanan mundur
be_bryan 52:a39e26b935a9 363 caseJoystick = 15;
Najib_irvani 43:3807a95aa284 364 }
be_bryan 52:a39e26b935a9 365 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
Najib_irvani 43:3807a95aa284 366 // Serong kiri mundur
be_bryan 52:a39e26b935a9 367 caseJoystick = 16;
be_bryan 52:a39e26b935a9 368 }
be_bryan 52:a39e26b935a9 369 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
gustavaditya 31:d5cbda07fd95 370 // Kanan
gustavaditya 31:d5cbda07fd95 371 caseJoystick = 3;
be_bryan 52:a39e26b935a9 372 }
be_bryan 52:a39e26b935a9 373 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
gustavaditya 31:d5cbda07fd95 374 // Kiri
gustavaditya 31:d5cbda07fd95 375 caseJoystick = 4;
Najib_irvani 43:3807a95aa284 376 }
be_bryan 52:a39e26b935a9 377 else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
Najib_irvani 43:3807a95aa284 378 // Atas -- Maju
be_bryan 52:a39e26b935a9 379 caseJoystick = 8;
Najib_irvani 43:3807a95aa284 380 }
be_bryan 52:a39e26b935a9 381 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
Najib_irvani 43:3807a95aa284 382 // Bawah -- Mundur
be_bryan 52:a39e26b935a9 383 caseJoystick = 9;
be_bryan 52:a39e26b935a9 384 }
gustavaditya 38:3ef6754bd8d8 385 else if (joystick.segitiga_click){
gustavaditya 31:d5cbda07fd95 386 // Motor Launcher
gustavaditya 31:d5cbda07fd95 387 caseJoystick = 5;
gustavaditya 31:d5cbda07fd95 388 }
gustavaditya 38:3ef6754bd8d8 389 else if (joystick.R2_click){
gustavaditya 31:d5cbda07fd95 390 // Target Pulse PID ++ Motor Depan
gustavaditya 31:d5cbda07fd95 391 caseJoystick = 6;
be_bryan 52:a39e26b935a9 392 }
gustavaditya 38:3ef6754bd8d8 393 else if (joystick.L2_click){
be_bryan 52:a39e26b935a9 394 // Target Pulse PID -- Motor
gustavaditya 31:d5cbda07fd95 395 caseJoystick = 7;
gustavaditya 31:d5cbda07fd95 396 }
gustavaditya 31:d5cbda07fd95 397 else if (joystick.silang_click){
gustavaditya 31:d5cbda07fd95 398 // Pnemuatik ON
gustavaditya 31:d5cbda07fd95 399 caseJoystick = 10;
gustavaditya 31:d5cbda07fd95 400 }
be_bryan 52:a39e26b935a9 401 else if ((joystick.lingkaran_click)&&(!joystick.kotak_click)) {
gustavaditya 31:d5cbda07fd95 402 // Power Screw Up
gustavaditya 31:d5cbda07fd95 403 caseJoystick = 11;
be_bryan 52:a39e26b935a9 404 }
be_bryan 52:a39e26b935a9 405 else if ((joystick.kotak_click)&&(!joystick.lingkaran_click)) {
gustavaditya 31:d5cbda07fd95 406 // Power Screw Down
be_bryan 52:a39e26b935a9 407 caseJoystick = 12;
be_bryan 52:a39e26b935a9 408 }
be_bryan 52:a39e26b935a9 409 else if (joystick.L3_click){
be_bryan 52:a39e26b935a9 410 // elevasi ON/OFF
Najib_irvani 47:6cac4f1a3c8e 411 caseJoystick = 34;
Najib_irvani 47:6cac4f1a3c8e 412 }
gustavaditya 45:964ae71a30e3 413 else
gustavaditya 45:964ae71a30e3 414 {
be_bryan 52:a39e26b935a9 415 //tuneAksel = 0.6;
gustavaditya 45:964ae71a30e3 416 }
gustavaditya 31:d5cbda07fd95 417 return(caseJoystick);
gustavaditya 31:d5cbda07fd95 418 }
gustavaditya 31:d5cbda07fd95 419
be_bryan 52:a39e26b935a9 420 void aturSetpoint()
be_bryan 52:a39e26b935a9 421 {
be_bryan 52:a39e26b935a9 422 switch (mode)
be_bryan 52:a39e26b935a9 423 {
be_bryan 52:a39e26b935a9 424 case (-2):
be_bryan 52:a39e26b935a9 425 setpointT = -45.0;
be_bryan 52:a39e26b935a9 426 break;
be_bryan 52:a39e26b935a9 427 case (-1):
be_bryan 52:a39e26b935a9 428 setpointT = -20.0;
be_bryan 52:a39e26b935a9 429 break;
be_bryan 52:a39e26b935a9 430 case (0):
be_bryan 52:a39e26b935a9 431 setpointT = 0.0;
be_bryan 52:a39e26b935a9 432 break;
be_bryan 52:a39e26b935a9 433 case (1):
be_bryan 52:a39e26b935a9 434 setpointT = 20.0;
be_bryan 52:a39e26b935a9 435 break;
be_bryan 52:a39e26b935a9 436 case (2):
be_bryan 52:a39e26b935a9 437 setpointT = 45.0;
be_bryan 52:a39e26b935a9 438 break;
be_bryan 52:a39e26b935a9 439 }
be_bryan 52:a39e26b935a9 440 }
be_bryan 52:a39e26b935a9 441 void cekMode()
be_bryan 52:a39e26b935a9 442 {
be_bryan 52:a39e26b935a9 443 if (mode<-2){mode = -2;}
be_bryan 52:a39e26b935a9 444 else if (mode>2){mode = 2;}
be_bryan 52:a39e26b935a9 445 }
fanny868 0:9072e932503c 446
be_bryan 52:a39e26b935a9 447 void case_autoRotate()
be_bryan 52:a39e26b935a9 448 {
be_bryan 52:a39e26b935a9 449 if ((joystick.LY>175)&&flag_LYmax) // Bawah, Keluar
be_bryan 52:a39e26b935a9 450 {
be_bryan 52:a39e26b935a9 451 flag_LYmax = false;
be_bryan 52:a39e26b935a9 452 auto_rotate = false;
be_bryan 52:a39e26b935a9 453 setpointT = 0.0;
be_bryan 52:a39e26b935a9 454 inAutoRotate = false;
be_bryan 52:a39e26b935a9 455 mode = 0;
be_bryan 52:a39e26b935a9 456 }
be_bryan 52:a39e26b935a9 457 else if ((15<joystick.LY&&joystick.LY<175)&&!flag_LYmax){ flag_LYmax = true; }
be_bryan 52:a39e26b935a9 458
be_bryan 52:a39e26b935a9 459 if ((joystick.LY<15)&&flag_LYmin) // Atas, Masuk (aktif)
be_bryan 52:a39e26b935a9 460 {
be_bryan 52:a39e26b935a9 461 flag_LYmin = false;
be_bryan 52:a39e26b935a9 462 auto_rotate = true;
be_bryan 52:a39e26b935a9 463 reset_encoder = true;
be_bryan 52:a39e26b935a9 464 inAutoRotate =true;
be_bryan 52:a39e26b935a9 465 setCenter();
be_bryan 52:a39e26b935a9 466 mode = 0;
be_bryan 52:a39e26b935a9 467 }
be_bryan 52:a39e26b935a9 468 else if ((15<joystick.LY && joystick.LY<175)&&!flag_LYmin){ flag_LYmin = true;}
be_bryan 52:a39e26b935a9 469
be_bryan 52:a39e26b935a9 470 if ((joystick.LX>200)&&flag_LXmax)
be_bryan 52:a39e26b935a9 471 {
be_bryan 52:a39e26b935a9 472 flag_LXmax = false;
be_bryan 52:a39e26b935a9 473 mode--;
be_bryan 52:a39e26b935a9 474 cekMode();
be_bryan 52:a39e26b935a9 475 }
be_bryan 52:a39e26b935a9 476 else if ((55<joystick.LX&&joystick.LX<200)&&!flag_LXmax){ flag_LXmax = true; }
be_bryan 52:a39e26b935a9 477
be_bryan 52:a39e26b935a9 478 if ((joystick.LX<55)&&flag_LXmin)
be_bryan 52:a39e26b935a9 479 {
be_bryan 52:a39e26b935a9 480 flag_LXmin = false;
be_bryan 52:a39e26b935a9 481 mode++;
be_bryan 52:a39e26b935a9 482 cekMode();
be_bryan 52:a39e26b935a9 483 }
be_bryan 52:a39e26b935a9 484 else if ((55<joystick.LX&&joystick.LX<200)&&!flag_LXmin){ flag_LXmin = true; }
be_bryan 52:a39e26b935a9 485 }
be_bryan 52:a39e26b935a9 486
be_bryan 52:a39e26b935a9 487 void init_rpm ()
be_bryan 52:a39e26b935a9 488 {
be_bryan 52:a39e26b935a9 489 if (target_rpm>maxRPM){
be_bryan 52:a39e26b935a9 490 target_rpm = maxRPM;
gustavaditya 37:67d54563af90 491 }
franshendri 42:6caf8bd5abbc 492 if (target_rpm<minRPM){
gustavaditya 37:67d54563af90 493 target_rpm = minRPM;
gustavaditya 37:67d54563af90 494 }
be_bryan 52:a39e26b935a9 495 }
be_bryan 52:a39e26b935a9 496
be_bryan 52:a39e26b935a9 497 void sliderMove()
be_bryan 52:a39e26b935a9 498 {
be_bryan 52:a39e26b935a9 499 if (readySlideFromLeft && slideNowLeft && !delay)
be_bryan 52:a39e26b935a9 500 {
be_bryan 52:a39e26b935a9 501 swipper.speed(swipper_dorong2);
be_bryan 52:a39e26b935a9 502 if(!(limitTengah) && flag_tengah)
be_bryan 52:a39e26b935a9 503 {
be_bryan 52:a39e26b935a9 504 swipper.brake(1);
be_bryan 52:a39e26b935a9 505 slideNowLeft = false;
be_bryan 52:a39e26b935a9 506 slideNowMid = false;
be_bryan 52:a39e26b935a9 507 readySlideFromLeft = false;
be_bryan 52:a39e26b935a9 508 readySlideFromMiddle = true;
be_bryan 52:a39e26b935a9 509 getBack = false;
be_bryan 52:a39e26b935a9 510 flag_tengah = false;
be_bryan 52:a39e26b935a9 511 }
be_bryan 52:a39e26b935a9 512 else {flag_tengah = true;}
be_bryan 52:a39e26b935a9 513 }
be_bryan 52:a39e26b935a9 514 else if (readySlideFromMiddle && slideNowMid && !delay)
be_bryan 52:a39e26b935a9 515 {
be_bryan 52:a39e26b935a9 516 swipper.speed(swipper_dorong1);
be_bryan 52:a39e26b935a9 517 if(!(limitKanan))
be_bryan 52:a39e26b935a9 518 {
be_bryan 52:a39e26b935a9 519 readySlideFromMiddle = false;
be_bryan 52:a39e26b935a9 520 slideNowLeft = false;
be_bryan 52:a39e26b935a9 521 slideNowMid = false;
be_bryan 52:a39e26b935a9 522 readySlideFromLeft = false;
be_bryan 52:a39e26b935a9 523 getBack = true;
be_bryan 52:a39e26b935a9 524 }
gustavaditya 37:67d54563af90 525 }
be_bryan 52:a39e26b935a9 526 else if (getBack)
be_bryan 52:a39e26b935a9 527 {
be_bryan 52:a39e26b935a9 528 swipper.speed(balik);
be_bryan 52:a39e26b935a9 529 if(!(limitKiri))
be_bryan 52:a39e26b935a9 530 {
be_bryan 52:a39e26b935a9 531 swipper.brake(1);
be_bryan 52:a39e26b935a9 532 slideNowLeft = false;
be_bryan 52:a39e26b935a9 533 slideNowMid = false;
be_bryan 52:a39e26b935a9 534 readySlideFromLeft = false;
be_bryan 52:a39e26b935a9 535 readySlideFromMiddle = false;
be_bryan 52:a39e26b935a9 536 getBack = false;
be_bryan 52:a39e26b935a9 537 sliderReady = false;
be_bryan 52:a39e26b935a9 538 ReloadOn = true;
be_bryan 52:a39e26b935a9 539 isDown = false;
be_bryan 52:a39e26b935a9 540 }
be_bryan 52:a39e26b935a9 541 }
be_bryan 52:a39e26b935a9 542 else
be_bryan 52:a39e26b935a9 543 {
be_bryan 52:a39e26b935a9 544 swipper.brake(1);
be_bryan 52:a39e26b935a9 545 }
be_bryan 52:a39e26b935a9 546 }
be_bryan 52:a39e26b935a9 547
be_bryan 52:a39e26b935a9 548 void lifterMove()
be_bryan 52:a39e26b935a9 549 {
be_bryan 52:a39e26b935a9 550 if(ReloadOn)
be_bryan 52:a39e26b935a9 551 {
be_bryan 52:a39e26b935a9 552 if(isDown)
be_bryan 52:a39e26b935a9 553 {
be_bryan 52:a39e26b935a9 554 powerScrew.speed(-1.0);
be_bryan 52:a39e26b935a9 555 if(!(limitBawah))
be_bryan 52:a39e26b935a9 556 {
be_bryan 52:a39e26b935a9 557 ReloadOn = false;
be_bryan 52:a39e26b935a9 558 isDown = false;
be_bryan 52:a39e26b935a9 559 }
be_bryan 52:a39e26b935a9 560 }
be_bryan 52:a39e26b935a9 561 else if (!(limitAtas))
be_bryan 52:a39e26b935a9 562 {
be_bryan 52:a39e26b935a9 563 isDown = true;
be_bryan 52:a39e26b935a9 564 readySlideFromMiddle = false;
be_bryan 52:a39e26b935a9 565 getBack = true;
be_bryan 52:a39e26b935a9 566 }
be_bryan 52:a39e26b935a9 567 else if (sliderReady)
be_bryan 52:a39e26b935a9 568 {
be_bryan 52:a39e26b935a9 569 powerScrew.brake(1);
be_bryan 52:a39e26b935a9 570 sliderMove();
be_bryan 52:a39e26b935a9 571 }
be_bryan 52:a39e26b935a9 572 else if(!(limitFB))
be_bryan 52:a39e26b935a9 573 {
be_bryan 52:a39e26b935a9 574 sliderReady = true;
be_bryan 52:a39e26b935a9 575 readySlideFromLeft = true;
be_bryan 52:a39e26b935a9 576 }
be_bryan 52:a39e26b935a9 577 else
be_bryan 52:a39e26b935a9 578 {
be_bryan 52:a39e26b935a9 579 powerScrew.speed(1.0);
be_bryan 52:a39e26b935a9 580 }
be_bryan 52:a39e26b935a9 581 }
be_bryan 52:a39e26b935a9 582 else
be_bryan 52:a39e26b935a9 583 {
be_bryan 52:a39e26b935a9 584 powerScrew.brake(1);
gustavaditya 37:67d54563af90 585 }
gustavaditya 37:67d54563af90 586 }
gustavaditya 37:67d54563af90 587
gustavaditya 31:d5cbda07fd95 588 void aktuator()
gustavaditya 31:d5cbda07fd95 589 {
gustavaditya 31:d5cbda07fd95 590 switch (case_joy) {
be_bryan 52:a39e26b935a9 591 case (1):
be_bryan 52:a39e26b935a9 592 {
gustavaditya 31:d5cbda07fd95 593 // Pivot Kanan
gustavaditya 31:d5cbda07fd95 594 motorDpn.speed(-PIVOT);
gustavaditya 31:d5cbda07fd95 595 motorBlk.speed(-PIVOT);
be_bryan 52:a39e26b935a9 596 motorR.speed(-rasio*PIVOT);
be_bryan 52:a39e26b935a9 597 motorL.speed(-rasio*PIVOT);
gustavaditya 31:d5cbda07fd95 598 break;
gustavaditya 31:d5cbda07fd95 599 }
be_bryan 52:a39e26b935a9 600 case (2):
gustavaditya 31:d5cbda07fd95 601 {
gustavaditya 31:d5cbda07fd95 602 // Pivot Kiri
gustavaditya 31:d5cbda07fd95 603 motorDpn.speed(PIVOT);
gustavaditya 31:d5cbda07fd95 604 motorBlk.speed(PIVOT);
be_bryan 52:a39e26b935a9 605 motorR.speed(rasio*PIVOT);
be_bryan 52:a39e26b935a9 606 motorL.speed(rasio*PIVOT);
Najib_irvani 43:3807a95aa284 607 break;
Najib_irvani 43:3807a95aa284 608 }
be_bryan 52:a39e26b935a9 609 case (17):
be_bryan 52:a39e26b935a9 610 {
Najib_irvani 43:3807a95aa284 611 // Kanan + Rotasi Kanan
Najib_irvani 43:3807a95aa284 612 motorDpn.speed(-PIVOT);
Najib_irvani 43:3807a95aa284 613 motorBlk.speed(-PIVOT);
be_bryan 52:a39e26b935a9 614 motorR.speed(-rasio*PIVOT);
be_bryan 52:a39e26b935a9 615 motorL.speed(-rasio*PIVOT);
Najib_irvani 43:3807a95aa284 616 break;
Najib_irvani 43:3807a95aa284 617 }
be_bryan 52:a39e26b935a9 618 case (18):
Najib_irvani 43:3807a95aa284 619 {
Najib_irvani 43:3807a95aa284 620 // Kanan + Rotasi Kiri
Najib_irvani 43:3807a95aa284 621 motorDpn.speed(PIVOT);
Najib_irvani 43:3807a95aa284 622 motorBlk.speed(PIVOT);
be_bryan 52:a39e26b935a9 623 motorR.speed(rasio*PIVOT);
be_bryan 52:a39e26b935a9 624 motorL.speed(rasio*PIVOT);
Najib_irvani 43:3807a95aa284 625 break;
Najib_irvani 43:3807a95aa284 626 }
be_bryan 52:a39e26b935a9 627 case (19):
be_bryan 52:a39e26b935a9 628 {
Najib_irvani 43:3807a95aa284 629 // Kiri + Rotasi Kanan
Najib_irvani 43:3807a95aa284 630 motorDpn.speed(-PIVOT);
Najib_irvani 43:3807a95aa284 631 motorBlk.speed(-PIVOT);
be_bryan 52:a39e26b935a9 632 motorR.speed(-rasio*PIVOT);
be_bryan 52:a39e26b935a9 633 motorL.speed(-rasio*PIVOT);
Najib_irvani 43:3807a95aa284 634 break;
Najib_irvani 43:3807a95aa284 635 }
be_bryan 52:a39e26b935a9 636 case (20):
Najib_irvani 43:3807a95aa284 637 {
Najib_irvani 43:3807a95aa284 638 // Kiri + Rotasi Kiri
Najib_irvani 43:3807a95aa284 639 motorDpn.speed(PIVOT);
Najib_irvani 43:3807a95aa284 640 motorBlk.speed(PIVOT);
be_bryan 52:a39e26b935a9 641 motorR.speed(rasio*PIVOT);
be_bryan 52:a39e26b935a9 642 motorL.speed(rasio*PIVOT);
Najib_irvani 43:3807a95aa284 643 break;
Najib_irvani 43:3807a95aa284 644 }
be_bryan 52:a39e26b935a9 645 case (21):
be_bryan 52:a39e26b935a9 646 {
Najib_irvani 43:3807a95aa284 647 // Maju + Rotasi Kanan
Najib_irvani 43:3807a95aa284 648 motorDpn.speed(-PIVOT);
Najib_irvani 43:3807a95aa284 649 motorBlk.speed(-PIVOT);
be_bryan 52:a39e26b935a9 650 motorR.speed(-rasio*PIVOT);
be_bryan 52:a39e26b935a9 651 motorL.speed(-rasio*PIVOT);
Najib_irvani 43:3807a95aa284 652 break;
Najib_irvani 43:3807a95aa284 653 }
be_bryan 52:a39e26b935a9 654 case (22):
Najib_irvani 43:3807a95aa284 655 {
Najib_irvani 43:3807a95aa284 656 // Maju + Rotasi Kiri
Najib_irvani 43:3807a95aa284 657 motorDpn.speed(PIVOT);
Najib_irvani 43:3807a95aa284 658 motorBlk.speed(PIVOT);
be_bryan 52:a39e26b935a9 659 motorR.speed(rasio*PIVOT);
be_bryan 52:a39e26b935a9 660 motorL.speed(rasio*PIVOT);
Najib_irvani 43:3807a95aa284 661 break;
Najib_irvani 43:3807a95aa284 662 }
be_bryan 52:a39e26b935a9 663 case (23):
be_bryan 52:a39e26b935a9 664 {
Najib_irvani 43:3807a95aa284 665 // Mundur + Rotasi Kanan
Najib_irvani 43:3807a95aa284 666 motorDpn.speed(-PIVOT);
Najib_irvani 43:3807a95aa284 667 motorBlk.speed(-PIVOT);
be_bryan 52:a39e26b935a9 668 motorR.speed(-rasio*PIVOT);
be_bryan 52:a39e26b935a9 669 motorL.speed(-rasio*PIVOT);
Najib_irvani 43:3807a95aa284 670 break;
Najib_irvani 43:3807a95aa284 671 }
be_bryan 52:a39e26b935a9 672 case (24):
Najib_irvani 43:3807a95aa284 673 {
Najib_irvani 43:3807a95aa284 674 // Mundur + Rotasi Kiri
Najib_irvani 43:3807a95aa284 675 motorDpn.speed(PIVOT);
Najib_irvani 43:3807a95aa284 676 motorBlk.speed(PIVOT);
be_bryan 52:a39e26b935a9 677 motorR.speed(rasio*PIVOT);
be_bryan 52:a39e26b935a9 678 motorL.speed(rasio*PIVOT);
Najib_irvani 43:3807a95aa284 679 break;
Najib_irvani 43:3807a95aa284 680 }
be_bryan 52:a39e26b935a9 681 case (25):
Najib_irvani 43:3807a95aa284 682 {
Najib_irvani 43:3807a95aa284 683 // Kanan + segitiga
Najib_irvani 43:3807a95aa284 684 isLauncher = !isLauncher;
Najib_irvani 43:3807a95aa284 685 break;
Najib_irvani 43:3807a95aa284 686 }
be_bryan 52:a39e26b935a9 687 case (26):
Najib_irvani 43:3807a95aa284 688 {
Najib_irvani 43:3807a95aa284 689 // Kiri + segitiga
Najib_irvani 43:3807a95aa284 690 isLauncher = !isLauncher;
Najib_irvani 43:3807a95aa284 691 break;
Najib_irvani 43:3807a95aa284 692 }
be_bryan 52:a39e26b935a9 693 case (27):
Najib_irvani 43:3807a95aa284 694 {
be_bryan 52:a39e26b935a9 695 // Kanan + lingkaran ----- Lifter Up
be_bryan 52:a39e26b935a9 696 ReloadOn = !ReloadOn;
be_bryan 52:a39e26b935a9 697 isDown = false;
Najib_irvani 43:3807a95aa284 698 break;
Najib_irvani 43:3807a95aa284 699 }
be_bryan 52:a39e26b935a9 700 case (28):
Najib_irvani 43:3807a95aa284 701 {
be_bryan 52:a39e26b935a9 702 // Kiri + lingkaran ----- Lifter Up
be_bryan 52:a39e26b935a9 703 ReloadOn = !ReloadOn;
be_bryan 52:a39e26b935a9 704 isDown = false;
Najib_irvani 43:3807a95aa284 705 break;
Najib_irvani 43:3807a95aa284 706 }
be_bryan 52:a39e26b935a9 707 case (29):
Najib_irvani 43:3807a95aa284 708 {
be_bryan 52:a39e26b935a9 709 // Kanan + kotak ----- Lifter Down
be_bryan 52:a39e26b935a9 710 ReloadOn = !ReloadOn;
be_bryan 52:a39e26b935a9 711 isDown = true;
Najib_irvani 43:3807a95aa284 712 break;
Najib_irvani 43:3807a95aa284 713 }
be_bryan 52:a39e26b935a9 714 case (30):
Najib_irvani 43:3807a95aa284 715 {
be_bryan 52:a39e26b935a9 716 // Kiri + kotak ----- Lifter Down
be_bryan 52:a39e26b935a9 717 ReloadOn = !ReloadOn;
be_bryan 52:a39e26b935a9 718 isDown = true;
Najib_irvani 43:3807a95aa284 719 break;
Najib_irvani 43:3807a95aa284 720 }
be_bryan 52:a39e26b935a9 721 case (13) :
Najib_irvani 43:3807a95aa284 722 {
Najib_irvani 43:3807a95aa284 723 // Serong kanan maju
be_bryan 52:a39e26b935a9 724 awal = true;
Najib_irvani 43:3807a95aa284 725 motorDpn.speed(-serong);
Najib_irvani 43:3807a95aa284 726 motorBlk.speed(serong);
be_bryan 52:a39e26b935a9 727 motorR.speed(serong);
be_bryan 52:a39e26b935a9 728 motorL.speed(-serong);
Najib_irvani 43:3807a95aa284 729 break;
Najib_irvani 43:3807a95aa284 730 }
be_bryan 52:a39e26b935a9 731 case (14) :
Najib_irvani 43:3807a95aa284 732 {
Najib_irvani 43:3807a95aa284 733 // Serong kiri maju
be_bryan 52:a39e26b935a9 734 awal = true;
Najib_irvani 43:3807a95aa284 735 motorDpn.speed(serong);
Najib_irvani 43:3807a95aa284 736 motorBlk.speed(-serong);
be_bryan 52:a39e26b935a9 737 motorR.speed(serong);
be_bryan 52:a39e26b935a9 738 motorL.speed(-serong);
Najib_irvani 43:3807a95aa284 739 break;
Najib_irvani 43:3807a95aa284 740 }
be_bryan 52:a39e26b935a9 741 case (15) :
Najib_irvani 43:3807a95aa284 742 {
be_bryan 52:a39e26b935a9 743 // Serong kanan mundur
be_bryan 52:a39e26b935a9 744 awal = true;
Najib_irvani 44:452c214d9cf5 745 motorDpn.speed(-serong);
Najib_irvani 43:3807a95aa284 746 motorBlk.speed(serong);
be_bryan 52:a39e26b935a9 747 motorR.speed(-serong);
be_bryan 52:a39e26b935a9 748 motorL.speed(serong);
Najib_irvani 43:3807a95aa284 749 break;
Najib_irvani 43:3807a95aa284 750 }
be_bryan 52:a39e26b935a9 751 case (16) :
Najib_irvani 43:3807a95aa284 752 {
Najib_irvani 43:3807a95aa284 753 // Serong kiri mundur
be_bryan 52:a39e26b935a9 754 awal = true;
Najib_irvani 43:3807a95aa284 755 motorDpn.speed(serong);
Najib_irvani 43:3807a95aa284 756 motorBlk.speed(-serong);
be_bryan 52:a39e26b935a9 757 motorR.speed(-serong);
be_bryan 52:a39e26b935a9 758 motorL.speed(serong);
gustavaditya 31:d5cbda07fd95 759 break;
gustavaditya 31:d5cbda07fd95 760 }
be_bryan 52:a39e26b935a9 761 case (3) :
gustavaditya 31:d5cbda07fd95 762 {
gustavaditya 31:d5cbda07fd95 763 // Kanan
be_bryan 52:a39e26b935a9 764 if (awal)
be_bryan 52:a39e26b935a9 765 {
be_bryan 52:a39e26b935a9 766 tuneAksel = 0.6;
be_bryan 52:a39e26b935a9 767 awal = false;
be_bryan 52:a39e26b935a9 768 }
be_bryan 52:a39e26b935a9 769 jarakXnow = getX();
be_bryan 52:a39e26b935a9 770 if(jarakXnow-jarakXbefore>5.0)
be_bryan 52:a39e26b935a9 771 {
be_bryan 52:a39e26b935a9 772 tuneAksel = tuneAksel+0.05;
be_bryan 52:a39e26b935a9 773 jarakXbefore = jarakXnow;
be_bryan 52:a39e26b935a9 774 }
be_bryan 52:a39e26b935a9 775 if (tuneAksel>1.0) tuneAksel = 1.0;
gustavaditya 45:964ae71a30e3 776
gustavaditya 45:964ae71a30e3 777 motorDpn.speed(-tuneAksel);
gustavaditya 45:964ae71a30e3 778 motorBlk.speed(tuneAksel);
Najib_irvani 43:3807a95aa284 779 motorR.brake(1);
Najib_irvani 43:3807a95aa284 780 motorL.brake(1);
gustavaditya 31:d5cbda07fd95 781 break;
be_bryan 52:a39e26b935a9 782 }
be_bryan 52:a39e26b935a9 783 case (4) :
gustavaditya 31:d5cbda07fd95 784 {
gustavaditya 31:d5cbda07fd95 785 // Kiri
be_bryan 52:a39e26b935a9 786 if (awal)
be_bryan 52:a39e26b935a9 787 {
be_bryan 52:a39e26b935a9 788 tuneAksel = 0.6;
be_bryan 52:a39e26b935a9 789 awal = false;
be_bryan 52:a39e26b935a9 790 }
be_bryan 52:a39e26b935a9 791 jarakXnow = getX();
be_bryan 52:a39e26b935a9 792 if(jarakXnow-jarakXbefore<-5.0)
be_bryan 52:a39e26b935a9 793 {
be_bryan 52:a39e26b935a9 794 tuneAksel = tuneAksel+0.05;
be_bryan 52:a39e26b935a9 795 jarakXbefore = jarakXnow;
be_bryan 52:a39e26b935a9 796 }
be_bryan 52:a39e26b935a9 797 if (tuneAksel>1.0) tuneAksel = 1.0;
be_bryan 52:a39e26b935a9 798
gustavaditya 45:964ae71a30e3 799 motorDpn.speed(tuneAksel);
gustavaditya 45:964ae71a30e3 800 motorBlk.speed(-tuneAksel);
Najib_irvani 43:3807a95aa284 801 motorR.brake(1);
Najib_irvani 43:3807a95aa284 802 motorL.brake(1);
Najib_irvani 43:3807a95aa284 803 break;
be_bryan 52:a39e26b935a9 804 }
be_bryan 52:a39e26b935a9 805 case (8) :
Najib_irvani 43:3807a95aa284 806 {
Najib_irvani 43:3807a95aa284 807 // Maju
be_bryan 52:a39e26b935a9 808 if (awal)
be_bryan 52:a39e26b935a9 809 {
be_bryan 52:a39e26b935a9 810 tuneAksel = 0.525;
be_bryan 52:a39e26b935a9 811 awal = false;
be_bryan 52:a39e26b935a9 812 }
be_bryan 52:a39e26b935a9 813 jarakYnow = getY();
be_bryan 52:a39e26b935a9 814 if(jarakYnow-jarakYbefore>5.0)
be_bryan 52:a39e26b935a9 815 {
be_bryan 52:a39e26b935a9 816 tuneAksel = tuneAksel+0.025;
be_bryan 52:a39e26b935a9 817 jarakYbefore = jarakYnow;
be_bryan 52:a39e26b935a9 818 }
be_bryan 52:a39e26b935a9 819 if (tuneAksel>0.675) tuneAksel = 0.675;
be_bryan 52:a39e26b935a9 820
be_bryan 52:a39e26b935a9 821 motorDpn.brake(1);
be_bryan 52:a39e26b935a9 822 motorBlk.brake(1);
Najib_irvani 47:6cac4f1a3c8e 823 motorR.speed(tuneAksel);
Najib_irvani 47:6cac4f1a3c8e 824 motorL.speed(-tuneAksel);
Najib_irvani 43:3807a95aa284 825 break;
Najib_irvani 43:3807a95aa284 826 }
be_bryan 52:a39e26b935a9 827 case (9) :
Najib_irvani 43:3807a95aa284 828 {
Najib_irvani 43:3807a95aa284 829 // Mundur
be_bryan 52:a39e26b935a9 830 if (awal)
be_bryan 52:a39e26b935a9 831 {
be_bryan 52:a39e26b935a9 832 tuneAksel = 0.525;
be_bryan 52:a39e26b935a9 833 awal = false;
be_bryan 52:a39e26b935a9 834 }
be_bryan 52:a39e26b935a9 835 jarakYnow = getY();
be_bryan 52:a39e26b935a9 836 if(jarakYnow-jarakYbefore<-5.0)
be_bryan 52:a39e26b935a9 837 {
be_bryan 52:a39e26b935a9 838 tuneAksel = tuneAksel+0.025;
be_bryan 52:a39e26b935a9 839 jarakYbefore = jarakYnow;
be_bryan 52:a39e26b935a9 840 }
be_bryan 52:a39e26b935a9 841 if (tuneAksel>0.675) tuneAksel = 0.675;
be_bryan 52:a39e26b935a9 842
be_bryan 52:a39e26b935a9 843 motorDpn.brake(1);
be_bryan 52:a39e26b935a9 844 motorBlk.brake(1);
Najib_irvani 47:6cac4f1a3c8e 845 motorR.speed(-tuneAksel);
Najib_irvani 47:6cac4f1a3c8e 846 motorL.speed(tuneAksel);
gustavaditya 31:d5cbda07fd95 847 break;
gustavaditya 31:d5cbda07fd95 848 }
be_bryan 52:a39e26b935a9 849 case (5) :
gustavaditya 31:d5cbda07fd95 850 {
gustavaditya 31:d5cbda07fd95 851 // Aktifkan motor atas
gustavaditya 31:d5cbda07fd95 852 isLauncher = !isLauncher;
gustavaditya 31:d5cbda07fd95 853 break;
Joshua23 8:0711dea61312 854 }
be_bryan 52:a39e26b935a9 855 case (6) :
gustavaditya 31:d5cbda07fd95 856 {
be_bryan 52:a39e26b935a9 857 // Target Pulse PID ++
be_bryan 52:a39e26b935a9 858 target_rpm = target_rpm+100.0;
gustavaditya 37:67d54563af90 859 init_rpm();
gustavaditya 31:d5cbda07fd95 860 break;
gustavaditya 31:d5cbda07fd95 861 }
be_bryan 52:a39e26b935a9 862 case (7) :
gustavaditya 31:d5cbda07fd95 863 {
be_bryan 52:a39e26b935a9 864 // Target Pulse PID --
be_bryan 52:a39e26b935a9 865 target_rpm = target_rpm-100.0;
gustavaditya 37:67d54563af90 866 init_rpm();
gustavaditya 31:d5cbda07fd95 867 break;
gustavaditya 31:d5cbda07fd95 868 }
be_bryan 52:a39e26b935a9 869 case (10) :
gustavaditya 31:d5cbda07fd95 870 {
be_bryan 52:a39e26b935a9 871 // Pneumatik pendorong
Najib_irvani 44:452c214d9cf5 872 if (ready)
Najib_irvani 44:452c214d9cf5 873 {
be_bryan 52:a39e26b935a9 874 Pneumatik = 0;
Najib_irvani 44:452c214d9cf5 875 previousMillis3 = millis();
Najib_irvani 44:452c214d9cf5 876 flag_Pneu = true;
Najib_irvani 44:452c214d9cf5 877 ready = false;
be_bryan 52:a39e26b935a9 878 previousMillis6 = millis();
Najib_irvani 44:452c214d9cf5 879 }
gustavaditya 31:d5cbda07fd95 880 break;
gustavaditya 31:d5cbda07fd95 881 }
be_bryan 52:a39e26b935a9 882 case (11) :
gustavaditya 31:d5cbda07fd95 883 {
be_bryan 52:a39e26b935a9 884 // Lifter Up
gustavaditya 40:5b937cac959a 885 ReloadOn = !ReloadOn;
be_bryan 52:a39e26b935a9 886 isDown = false;
gustavaditya 31:d5cbda07fd95 887 break;
gustavaditya 31:d5cbda07fd95 888 }
be_bryan 52:a39e26b935a9 889 case (31) :
Najib_irvani 44:452c214d9cf5 890 {
Najib_irvani 44:452c214d9cf5 891 // start
be_bryan 52:a39e26b935a9 892 Elevator = 1;
be_bryan 52:a39e26b935a9 893 target_rpm = 3200;
Najib_irvani 44:452c214d9cf5 894 init_rpm();
Najib_irvani 44:452c214d9cf5 895 break;
Najib_irvani 44:452c214d9cf5 896 }
be_bryan 52:a39e26b935a9 897 case (32) :
Najib_irvani 44:452c214d9cf5 898 {
Najib_irvani 44:452c214d9cf5 899 // select
be_bryan 52:a39e26b935a9 900 target_rpm = 1600;
Najib_irvani 44:452c214d9cf5 901 init_rpm();
Najib_irvani 44:452c214d9cf5 902 break;
Najib_irvani 44:452c214d9cf5 903 }
be_bryan 52:a39e26b935a9 904 case (33) :
Najib_irvani 44:452c214d9cf5 905 {
Najib_irvani 44:452c214d9cf5 906 // R3
be_bryan 52:a39e26b935a9 907 target_rpm = 2600;
Najib_irvani 44:452c214d9cf5 908 init_rpm();
Najib_irvani 44:452c214d9cf5 909 break;
Najib_irvani 44:452c214d9cf5 910 }
be_bryan 52:a39e26b935a9 911 case (12) :
gustavaditya 31:d5cbda07fd95 912 {
be_bryan 52:a39e26b935a9 913 // Lifter Down
Najib_irvani 43:3807a95aa284 914 ReloadOn = !ReloadOn;
be_bryan 52:a39e26b935a9 915 if (readySlideFromLeft)
be_bryan 52:a39e26b935a9 916 {
be_bryan 52:a39e26b935a9 917 sliderReady = false;
be_bryan 52:a39e26b935a9 918 readySlideFromLeft = false;
be_bryan 52:a39e26b935a9 919 }
be_bryan 52:a39e26b935a9 920 isDown = true;
gustavaditya 31:d5cbda07fd95 921 break;
gustavaditya 31:d5cbda07fd95 922 }
MarchioKevin 51:df6391c3fa68 923 case (34) :
MarchioKevin 51:df6391c3fa68 924 {
be_bryan 52:a39e26b935a9 925 // Pneumatik naikkan sudut
be_bryan 52:a39e26b935a9 926 naik = !naik;
be_bryan 52:a39e26b935a9 927 Elevator = 0;
be_bryan 52:a39e26b935a9 928 break;
MarchioKevin 51:df6391c3fa68 929 }
be_bryan 52:a39e26b935a9 930 default :
MarchioKevin 51:df6391c3fa68 931 {
be_bryan 52:a39e26b935a9 932 if (tuneAksel>0.4)
be_bryan 52:a39e26b935a9 933 {
be_bryan 52:a39e26b935a9 934 if (getX()>0.0)
be_bryan 52:a39e26b935a9 935 {
be_bryan 52:a39e26b935a9 936 jarakXnow = getX();
be_bryan 52:a39e26b935a9 937 if(jarakXnow-jarakXbefore>4.5)
be_bryan 52:a39e26b935a9 938 {
be_bryan 52:a39e26b935a9 939 tuneAksel = tuneAksel-0.05;
be_bryan 52:a39e26b935a9 940 jarakXbefore = jarakXnow;
be_bryan 52:a39e26b935a9 941 }
be_bryan 52:a39e26b935a9 942 }
be_bryan 52:a39e26b935a9 943 if (getX()<0.0)
be_bryan 52:a39e26b935a9 944 {
be_bryan 52:a39e26b935a9 945 jarakXnow = getX();
be_bryan 52:a39e26b935a9 946 if(jarakXnow-jarakXbefore<-4.5)
be_bryan 52:a39e26b935a9 947 {
be_bryan 52:a39e26b935a9 948 tuneAksel = tuneAksel-0.05;
be_bryan 52:a39e26b935a9 949 jarakXbefore = jarakXnow;
be_bryan 52:a39e26b935a9 950 }
be_bryan 52:a39e26b935a9 951 }
be_bryan 52:a39e26b935a9 952 }
be_bryan 52:a39e26b935a9 953 else
be_bryan 52:a39e26b935a9 954 {
be_bryan 52:a39e26b935a9 955 motorDpn.brake(1);
be_bryan 52:a39e26b935a9 956 motorBlk.brake(1);
be_bryan 52:a39e26b935a9 957 motorR.brake(1);
be_bryan 52:a39e26b935a9 958 motorL.brake(1);
be_bryan 52:a39e26b935a9 959 setCenter();
be_bryan 52:a39e26b935a9 960 jarakXbefore = 0;
be_bryan 52:a39e26b935a9 961 jarakYbefore = 0;
be_bryan 52:a39e26b935a9 962 }
be_bryan 52:a39e26b935a9 963 if(PIVOT>0.2 || serong>0.2)
be_bryan 52:a39e26b935a9 964 {
be_bryan 52:a39e26b935a9 965 motorDpn.brake(1);
be_bryan 52:a39e26b935a9 966 motorBlk.brake(1);
be_bryan 52:a39e26b935a9 967 motorR.brake(1);
be_bryan 52:a39e26b935a9 968 motorL.brake(1);
be_bryan 52:a39e26b935a9 969 }
be_bryan 52:a39e26b935a9 970 awal = true;
Najib_irvani 47:6cac4f1a3c8e 971 }
gustavaditya 31:d5cbda07fd95 972 } // End Switch
gustavaditya 31:d5cbda07fd95 973 }
gustavaditya 31:d5cbda07fd95 974
be_bryan 52:a39e26b935a9 975 void launcher()
gustavaditya 31:d5cbda07fd95 976 {
gustavaditya 31:d5cbda07fd95 977 if (isLauncher)
Sufa 30:d69cc27ac644 978 {
be_bryan 28:2d0746dc2d7d 979 currentMillis = millis();
be_bryan 52:a39e26b935a9 980
Najib_irvani 46:85169ae8659b 981 // PID Launcher Belakang
Najib_irvani 46:85169ae8659b 982 if (currentMillis-previousMillis>=Ts)
be_bryan 26:256160a1a82d 983 {
be_bryan 52:a39e26b935a9 984 rpm = (float)Lenc.getPulses();;
be_bryan 52:a39e26b935a9 985 current_error = target_rpm - rpm;
be_bryan 52:a39e26b935a9 986 a = kpA + kiA*Ts/2 + kdA/Ts;
be_bryan 52:a39e26b935a9 987 b = -kpA + kiA*Ts/2 - 2*kdA/Ts;
be_bryan 52:a39e26b935a9 988 c = kdA/Ts;
be_bryan 52:a39e26b935a9 989 speed = previous_speed + a*current_error + b*previous_error_1 + c*previous_error_2;
gustavaditya 38:3ef6754bd8d8 990 if(speed > maxSpeed){
be_bryan 52:a39e26b935a9 991 Launcher.speed(maxSpeed);
gustavaditya 38:3ef6754bd8d8 992 }
gustavaditya 39:11358f3f61ff 993 else if ( speed < minSpeed){
be_bryan 52:a39e26b935a9 994 Launcher.speed(minSpeed);
gustavaditya 39:11358f3f61ff 995 }
gustavaditya 38:3ef6754bd8d8 996 else {
be_bryan 52:a39e26b935a9 997 Launcher.speed(speed);
gustavaditya 38:3ef6754bd8d8 998 }
be_bryan 52:a39e26b935a9 999 previous_speed = speed;
be_bryan 52:a39e26b935a9 1000 previous_error_2 = previous_error_1;
be_bryan 52:a39e26b935a9 1001 previous_error_1 = current_error;
be_bryan 26:256160a1a82d 1002 previousMillis = currentMillis;
be_bryan 52:a39e26b935a9 1003 Lenc.reset();
be_bryan 26:256160a1a82d 1004 }
rahmadirizki18 6:68293bed71ea 1005 }
Sufa 29:7b372b0aaa61 1006 else
be_bryan 52:a39e26b935a9 1007 {
be_bryan 52:a39e26b935a9 1008 Launcher.brake(1);
be_bryan 52:a39e26b935a9 1009 current_error = 0;
be_bryan 52:a39e26b935a9 1010 previous_error_1 = 0;
be_bryan 52:a39e26b935a9 1011 previous_error_2 = 0;
be_bryan 52:a39e26b935a9 1012 previous_speed = 0;
be_bryan 52:a39e26b935a9 1013 }
rahmadirizki18 5:3aa203218306 1014 }
calmantara186 16:90119f03c5d1 1015
be_bryan 52:a39e26b935a9 1016 // Main Program
gustavaditya 31:d5cbda07fd95 1017 int main (void)
gustavaditya 31:d5cbda07fd95 1018 {
gustavaditya 31:d5cbda07fd95 1019 // Set baud rate - 115200
fanny868 0:9072e932503c 1020 joystick.setup();
rahmadirizki18 23:023b522977b2 1021 pc.baud(115200);
rahmadirizki18 6:68293bed71ea 1022 wait_ms(1000);
gustavaditya 40:5b937cac959a 1023
be_bryan 52:a39e26b935a9 1024 // initializing pneumatik
be_bryan 52:a39e26b935a9 1025 Pneumatik = 1;
be_bryan 52:a39e26b935a9 1026 Elevator = 1;
rahmadirizki18 5:3aa203218306 1027 wait_ms(500);
gustavaditya 40:5b937cac959a 1028
be_bryan 52:a39e26b935a9 1029 while(init_slider)
be_bryan 52:a39e26b935a9 1030 {
be_bryan 52:a39e26b935a9 1031 swipper.speed(balik);
be_bryan 52:a39e26b935a9 1032 if (!(limitKiri))
be_bryan 52:a39e26b935a9 1033 {
be_bryan 52:a39e26b935a9 1034 init_slider = false;
be_bryan 52:a39e26b935a9 1035 swipper.brake(1);
be_bryan 52:a39e26b935a9 1036 }
be_bryan 52:a39e26b935a9 1037 }
be_bryan 52:a39e26b935a9 1038 while(init_lifter)
be_bryan 52:a39e26b935a9 1039 {
be_bryan 52:a39e26b935a9 1040 powerScrew.speed(-1.0);
be_bryan 52:a39e26b935a9 1041 if(!(limitBawah))
be_bryan 52:a39e26b935a9 1042 {
be_bryan 52:a39e26b935a9 1043 init_lifter = false;
be_bryan 52:a39e26b935a9 1044 powerScrew.brake(1);
be_bryan 52:a39e26b935a9 1045 }
be_bryan 52:a39e26b935a9 1046 }
be_bryan 52:a39e26b935a9 1047
gustavaditya 35:69a47b4cb3fc 1048 startMillis();
be_bryan 52:a39e26b935a9 1049 /* USER CODE END 2 */
be_bryan 52:a39e26b935a9 1050
be_bryan 52:a39e26b935a9 1051 /* Infinite loop */
be_bryan 52:a39e26b935a9 1052 /* USER CODE BEGIN WHILE */
be_bryan 52:a39e26b935a9 1053 //idle();
be_bryan 52:a39e26b935a9 1054 setCenter();
be_bryan 52:a39e26b935a9 1055 joystick.idle();
be_bryan 52:a39e26b935a9 1056 while (1)
be_bryan 52:a39e26b935a9 1057 {
gustavaditya 31:d5cbda07fd95 1058 if(joystick.readable())
gustavaditya 31:d5cbda07fd95 1059 {
fanny868 0:9072e932503c 1060 joystick.baca_data();
be_bryan 52:a39e26b935a9 1061 joystick.olah_data();
gustavaditya 31:d5cbda07fd95 1062 case_joy = case_joystick();
be_bryan 52:a39e26b935a9 1063 if (naik)
be_bryan 52:a39e26b935a9 1064 {
be_bryan 52:a39e26b935a9 1065 swipper_dorong1 = lempar_naik1;
be_bryan 52:a39e26b935a9 1066 swipper_dorong2 = lempar2_naik;
be_bryan 52:a39e26b935a9 1067 }
be_bryan 52:a39e26b935a9 1068 else
be_bryan 52:a39e26b935a9 1069 {
be_bryan 52:a39e26b935a9 1070 swipper_dorong1 = lempar1;
be_bryan 52:a39e26b935a9 1071 swipper_dorong2 = lempar2;
be_bryan 52:a39e26b935a9 1072 }
rahmadirizki18 3:1287fccc11be 1073 aktuator();
gustavaditya 31:d5cbda07fd95 1074 launcher();
be_bryan 52:a39e26b935a9 1075 lifterMove();
be_bryan 52:a39e26b935a9 1076
be_bryan 52:a39e26b935a9 1077 if ((millis()-previousMillis3 >= 500)&&(flag_Pneu))
be_bryan 52:a39e26b935a9 1078 {
be_bryan 52:a39e26b935a9 1079 Pneumatik = 1;
gustavaditya 31:d5cbda07fd95 1080 flag_Pneu = false;
be_bryan 52:a39e26b935a9 1081 slideNowLeft = true;
be_bryan 52:a39e26b935a9 1082 slideNowMid = true;
be_bryan 52:a39e26b935a9 1083 ready = true;
be_bryan 52:a39e26b935a9 1084 delay = true;
be_bryan 52:a39e26b935a9 1085 previousMillis7 = millis(); //delay
be_bryan 52:a39e26b935a9 1086 }
be_bryan 52:a39e26b935a9 1087 if((millis()-previousMillis7>=200) && delay)
be_bryan 52:a39e26b935a9 1088 {
be_bryan 52:a39e26b935a9 1089 delay = false;
be_bryan 26:256160a1a82d 1090 }
be_bryan 26:256160a1a82d 1091 }
gustavaditya 31:d5cbda07fd95 1092 else
gustavaditya 31:d5cbda07fd95 1093 {
gustavaditya 31:d5cbda07fd95 1094 joystick.idle();
MarchioKevin 21:da2f3d04468f 1095 }
Najib_irvani 44:452c214d9cf5 1096 if(millis() - previousMillis5 >= 400)
be_bryan 52:a39e26b935a9 1097 {
be_bryan 52:a39e26b935a9 1098 display.write(0,((int)rpm) / 1000);
be_bryan 52:a39e26b935a9 1099 display.write(1,((((int)rpm)%1000)/ 100));
be_bryan 52:a39e26b935a9 1100 display.write(2,((int)target_rpm) / 1000);
be_bryan 52:a39e26b935a9 1101 display.write(3,((((int)target_rpm)%1000)/ 100));
franshendri 42:6caf8bd5abbc 1102 display.setColon(true);
be_bryan 52:a39e26b935a9 1103
franshendri 42:6caf8bd5abbc 1104 previousMillis5 = millis();
franshendri 42:6caf8bd5abbc 1105 }
fanny868 0:9072e932503c 1106 }
be_bryan 28:2d0746dc2d7d 1107 }