![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
tugas akhir
Dependencies: Motor mbed millis
Fork of DagonFly__RoadToJapan_15Mei_Ultimate by
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
- Child:
- 53:11515147caf9
Convert_KeilToMbed
Who changed what in which revision?
User | Revision | Line number | New 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 | } |