Reloader Siap. Bulat = naik lifter. Kotak = turun Lifter. Slider otomatis. Silang = pneumatik.
Dependencies: Motor mbed millis
main.cpp@1:26fbc9316523, 2017-06-12 (annotated)
- Committer:
- gustavaditya
- Date:
- Mon Jun 12 09:22:28 2017 +0000
- Revision:
- 1:26fbc9316523
- Parent:
- 0:e708f9673603
Reloader Siap
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
gustavaditya | 0:e708f9673603 | 1 | #include "Motor.h" |
gustavaditya | 0:e708f9673603 | 2 | #include "millis.h" |
gustavaditya | 0:e708f9673603 | 3 | #include "mbed.h" |
gustavaditya | 0:e708f9673603 | 4 | #include "JoystickPS3.h" |
gustavaditya | 0:e708f9673603 | 5 | |
gustavaditya | 0:e708f9673603 | 6 | bool isReload = false; |
gustavaditya | 0:e708f9673603 | 7 | bool ReloadOn = false; |
gustavaditya | 0:e708f9673603 | 8 | bool flag_Pneu = false; |
gustavaditya | 0:e708f9673603 | 9 | bool readySlideFromLeft = false; |
gustavaditya | 0:e708f9673603 | 10 | bool readySlideFromMiddle = false; |
gustavaditya | 0:e708f9673603 | 11 | bool getBack = false; |
gustavaditya | 0:e708f9673603 | 12 | bool isUp = false, isDown = false, sliderOn = false; |
gustavaditya | 0:e708f9673603 | 13 | bool ready = true; |
gustavaditya | 0:e708f9673603 | 14 | bool init_slider = true; |
gustavaditya | 1:26fbc9316523 | 15 | bool init_lifter = true; |
gustavaditya | 1:26fbc9316523 | 16 | bool sliderReady = false; |
gustavaditya | 1:26fbc9316523 | 17 | bool flag_tengah = true; |
gustavaditya | 1:26fbc9316523 | 18 | bool delay = true; |
gustavaditya | 0:e708f9673603 | 19 | |
gustavaditya | 0:e708f9673603 | 20 | static volatile uint32_t previousMillis3 = 0; // Pneumatik |
gustavaditya | 0:e708f9673603 | 21 | static volatile uint32_t previousMillis6 = 0; // pneu |
gustavaditya | 1:26fbc9316523 | 22 | static volatile uint32_t prevMillis = 0; // delay |
gustavaditya | 0:e708f9673603 | 23 | |
gustavaditya | 0:e708f9673603 | 24 | int caseJoystick, case_joy; |
gustavaditya | 0:e708f9673603 | 25 | |
gustavaditya | 1:26fbc9316523 | 26 | float lempar = -0.8, lempar2 = -0.8, balik = 0.6; |
gustavaditya | 0:e708f9673603 | 27 | |
gustavaditya | 0:e708f9673603 | 28 | DigitalOut pneumatik(PA_4, PullUp); |
gustavaditya | 0:e708f9673603 | 29 | DigitalIn limitAtasLifter(PB_3, PullUp); // Vertikal Atas: Lifter |
gustavaditya | 0:e708f9673603 | 30 | DigitalIn limitAtasSlider(PB_2, PullUp); // Vertikal Atas: Saucer |
gustavaditya | 0:e708f9673603 | 31 | DigitalIn limitBawah(PB_10, PullUp); // Vertikal Bawah |
gustavaditya | 0:e708f9673603 | 32 | DigitalIn limitKiri(PA_5, PullUp); // Horizontal Kiri |
gustavaditya | 0:e708f9673603 | 33 | DigitalIn limitTengah(PC_9, PullUp);// Horizontal Tengah |
gustavaditya | 0:e708f9673603 | 34 | DigitalIn limitKanan(PC_8, PullUp); // Horizontal Kanan (Frisbee keluar) |
gustavaditya | 0:e708f9673603 | 35 | |
gustavaditya | 1:26fbc9316523 | 36 | Motor lifter(PC_7, PC_13, PC_14); // pwm, fwd, rev |
gustavaditya | 0:e708f9673603 | 37 | Motor slider(PA_10, PC_3, PC_0); |
gustavaditya | 0:e708f9673603 | 38 | |
gustavaditya | 0:e708f9673603 | 39 | /* Inisialisasi Pin TX-RX Joystik dan PC */ |
gustavaditya | 0:e708f9673603 | 40 | joysticknucleo joystick(PA_0,PA_1); |
gustavaditya | 0:e708f9673603 | 41 | Serial pc(USBTX,USBRX); |
gustavaditya | 0:e708f9673603 | 42 | |
gustavaditya | 0:e708f9673603 | 43 | int case_joystick() |
gustavaditya | 0:e708f9673603 | 44 | { |
gustavaditya | 0:e708f9673603 | 45 | if (joystick.silang_click){ |
gustavaditya | 0:e708f9673603 | 46 | // Pnemuatik ON |
gustavaditya | 0:e708f9673603 | 47 | caseJoystick = 1; |
gustavaditya | 0:e708f9673603 | 48 | } |
gustavaditya | 0:e708f9673603 | 49 | else if ((joystick.lingkaran_click)&&(!joystick.kotak_click)) { |
gustavaditya | 0:e708f9673603 | 50 | // Lifter Up |
gustavaditya | 0:e708f9673603 | 51 | caseJoystick = 2; |
gustavaditya | 1:26fbc9316523 | 52 | pc.printf("LINGKARAN\n"); |
gustavaditya | 0:e708f9673603 | 53 | } |
gustavaditya | 0:e708f9673603 | 54 | else if ((joystick.kotak_click)&&(!joystick.lingkaran_click)) { |
gustavaditya | 0:e708f9673603 | 55 | // Lifter Down |
gustavaditya | 0:e708f9673603 | 56 | caseJoystick = 3; |
gustavaditya | 1:26fbc9316523 | 57 | pc.printf("KOTAK\n"); |
gustavaditya | 1:26fbc9316523 | 58 | } |
gustavaditya | 1:26fbc9316523 | 59 | else{ |
gustavaditya | 1:26fbc9316523 | 60 | caseJoystick = 0; |
gustavaditya | 1:26fbc9316523 | 61 | //pc.printf("DO NOTHING\n"); |
gustavaditya | 0:e708f9673603 | 62 | } |
gustavaditya | 0:e708f9673603 | 63 | |
gustavaditya | 0:e708f9673603 | 64 | return caseJoystick; |
gustavaditya | 0:e708f9673603 | 65 | } |
gustavaditya | 0:e708f9673603 | 66 | |
gustavaditya | 0:e708f9673603 | 67 | void aktuator() |
gustavaditya | 0:e708f9673603 | 68 | { |
gustavaditya | 0:e708f9673603 | 69 | switch (case_joy) { |
gustavaditya | 0:e708f9673603 | 70 | case (1): |
gustavaditya | 0:e708f9673603 | 71 | // Pneumatik |
gustavaditya | 0:e708f9673603 | 72 | if (ready) |
gustavaditya | 0:e708f9673603 | 73 | { |
gustavaditya | 0:e708f9673603 | 74 | pneumatik = 0; |
gustavaditya | 0:e708f9673603 | 75 | previousMillis3 = millis(); |
gustavaditya | 0:e708f9673603 | 76 | flag_Pneu = true; |
gustavaditya | 0:e708f9673603 | 77 | ready = false; |
gustavaditya | 0:e708f9673603 | 78 | previousMillis6 = millis(); |
gustavaditya | 0:e708f9673603 | 79 | } |
gustavaditya | 0:e708f9673603 | 80 | break; |
gustavaditya | 0:e708f9673603 | 81 | case (2): |
gustavaditya | 0:e708f9673603 | 82 | // Lifter Up |
gustavaditya | 0:e708f9673603 | 83 | ReloadOn = !ReloadOn; |
gustavaditya | 1:26fbc9316523 | 84 | //isUp = 1; |
gustavaditya | 0:e708f9673603 | 85 | isDown = false; |
gustavaditya | 1:26fbc9316523 | 86 | pc.printf("%d\n",isUp); |
gustavaditya | 1:26fbc9316523 | 87 | |
gustavaditya | 0:e708f9673603 | 88 | break; |
gustavaditya | 0:e708f9673603 | 89 | case (3): |
gustavaditya | 0:e708f9673603 | 90 | // Lifter Down |
gustavaditya | 0:e708f9673603 | 91 | ReloadOn = !ReloadOn; |
gustavaditya | 1:26fbc9316523 | 92 | //isUp = 0; |
gustavaditya | 0:e708f9673603 | 93 | isDown = true; |
gustavaditya | 0:e708f9673603 | 94 | break; |
gustavaditya | 0:e708f9673603 | 95 | } |
gustavaditya | 0:e708f9673603 | 96 | } |
gustavaditya | 0:e708f9673603 | 97 | |
gustavaditya | 1:26fbc9316523 | 98 | void sliderMove() |
gustavaditya | 1:26fbc9316523 | 99 | { |
gustavaditya | 1:26fbc9316523 | 100 | if (readySlideFromLeft) |
gustavaditya | 1:26fbc9316523 | 101 | { |
gustavaditya | 1:26fbc9316523 | 102 | slider.speed(lempar2); |
gustavaditya | 1:26fbc9316523 | 103 | if(!limitTengah && flag_tengah) |
gustavaditya | 1:26fbc9316523 | 104 | { |
gustavaditya | 1:26fbc9316523 | 105 | slider.brake(1); |
gustavaditya | 1:26fbc9316523 | 106 | readySlideFromLeft = false; |
gustavaditya | 1:26fbc9316523 | 107 | readySlideFromMiddle = false; |
gustavaditya | 1:26fbc9316523 | 108 | getBack = false; |
gustavaditya | 1:26fbc9316523 | 109 | flag_tengah = false; |
gustavaditya | 1:26fbc9316523 | 110 | } |
gustavaditya | 1:26fbc9316523 | 111 | else { flag_tengah = true;} |
gustavaditya | 1:26fbc9316523 | 112 | } |
gustavaditya | 1:26fbc9316523 | 113 | else if (readySlideFromMiddle) |
gustavaditya | 1:26fbc9316523 | 114 | { |
gustavaditya | 1:26fbc9316523 | 115 | slider.speed(lempar); |
gustavaditya | 1:26fbc9316523 | 116 | if(!limitKanan) |
gustavaditya | 1:26fbc9316523 | 117 | { |
gustavaditya | 1:26fbc9316523 | 118 | readySlideFromMiddle = false; |
gustavaditya | 1:26fbc9316523 | 119 | getBack = true; |
gustavaditya | 1:26fbc9316523 | 120 | } |
gustavaditya | 1:26fbc9316523 | 121 | } |
gustavaditya | 1:26fbc9316523 | 122 | else if (getBack) |
gustavaditya | 1:26fbc9316523 | 123 | { |
gustavaditya | 1:26fbc9316523 | 124 | slider.speed(balik); |
gustavaditya | 1:26fbc9316523 | 125 | if(!limitKiri) |
gustavaditya | 1:26fbc9316523 | 126 | { |
gustavaditya | 1:26fbc9316523 | 127 | slider.brake(1); |
gustavaditya | 1:26fbc9316523 | 128 | readySlideFromLeft = false; |
gustavaditya | 1:26fbc9316523 | 129 | readySlideFromMiddle = false; |
gustavaditya | 1:26fbc9316523 | 130 | getBack = false; |
gustavaditya | 1:26fbc9316523 | 131 | sliderReady = false; |
gustavaditya | 1:26fbc9316523 | 132 | } |
gustavaditya | 1:26fbc9316523 | 133 | } |
gustavaditya | 1:26fbc9316523 | 134 | else |
gustavaditya | 1:26fbc9316523 | 135 | { |
gustavaditya | 1:26fbc9316523 | 136 | slider.brake(1); |
gustavaditya | 1:26fbc9316523 | 137 | } |
gustavaditya | 1:26fbc9316523 | 138 | } |
gustavaditya | 1:26fbc9316523 | 139 | |
gustavaditya | 1:26fbc9316523 | 140 | void lifterMove() |
gustavaditya | 0:e708f9673603 | 141 | { |
gustavaditya | 0:e708f9673603 | 142 | if(ReloadOn) |
gustavaditya | 0:e708f9673603 | 143 | { |
gustavaditya | 1:26fbc9316523 | 144 | if(isDown) |
gustavaditya | 0:e708f9673603 | 145 | { |
gustavaditya | 0:e708f9673603 | 146 | lifter.speed(-1.0); |
gustavaditya | 0:e708f9673603 | 147 | if(!limitBawah) |
gustavaditya | 0:e708f9673603 | 148 | { |
gustavaditya | 1:26fbc9316523 | 149 | ReloadOn = false; |
gustavaditya | 1:26fbc9316523 | 150 | isDown = false; |
gustavaditya | 0:e708f9673603 | 151 | } |
gustavaditya | 0:e708f9673603 | 152 | } |
gustavaditya | 1:26fbc9316523 | 153 | else if (!limitAtasLifter) |
gustavaditya | 1:26fbc9316523 | 154 | { |
gustavaditya | 1:26fbc9316523 | 155 | isDown = true; |
gustavaditya | 1:26fbc9316523 | 156 | } |
gustavaditya | 1:26fbc9316523 | 157 | else if(sliderReady) |
gustavaditya | 1:26fbc9316523 | 158 | { |
gustavaditya | 1:26fbc9316523 | 159 | lifter.brake(1); |
gustavaditya | 1:26fbc9316523 | 160 | if (!delay) |
gustavaditya | 1:26fbc9316523 | 161 | { |
gustavaditya | 1:26fbc9316523 | 162 | sliderMove(); |
gustavaditya | 1:26fbc9316523 | 163 | } |
gustavaditya | 1:26fbc9316523 | 164 | } |
gustavaditya | 1:26fbc9316523 | 165 | else if(!limitAtasSlider) |
gustavaditya | 1:26fbc9316523 | 166 | { |
gustavaditya | 1:26fbc9316523 | 167 | sliderReady = true; |
gustavaditya | 1:26fbc9316523 | 168 | readySlideFromLeft = true; |
gustavaditya | 1:26fbc9316523 | 169 | delay = true; |
gustavaditya | 1:26fbc9316523 | 170 | prevMillis = millis(); |
gustavaditya | 1:26fbc9316523 | 171 | } |
gustavaditya | 1:26fbc9316523 | 172 | else |
gustavaditya | 1:26fbc9316523 | 173 | { |
gustavaditya | 1:26fbc9316523 | 174 | lifter.speed(1.0); |
gustavaditya | 1:26fbc9316523 | 175 | pc.printf("NAIK\n"); |
gustavaditya | 1:26fbc9316523 | 176 | } |
gustavaditya | 0:e708f9673603 | 177 | } |
gustavaditya | 0:e708f9673603 | 178 | else |
gustavaditya | 0:e708f9673603 | 179 | { |
gustavaditya | 0:e708f9673603 | 180 | lifter.brake(1); |
gustavaditya | 1:26fbc9316523 | 181 | } |
gustavaditya | 0:e708f9673603 | 182 | } |
gustavaditya | 0:e708f9673603 | 183 | |
gustavaditya | 0:e708f9673603 | 184 | int main(void) |
gustavaditya | 0:e708f9673603 | 185 | { |
gustavaditya | 1:26fbc9316523 | 186 | while(init_slider) |
gustavaditya | 1:26fbc9316523 | 187 | { |
gustavaditya | 1:26fbc9316523 | 188 | slider.speed(balik); |
gustavaditya | 1:26fbc9316523 | 189 | pc.printf("init_slider\n"); |
gustavaditya | 1:26fbc9316523 | 190 | if(!limitKiri) |
gustavaditya | 1:26fbc9316523 | 191 | { |
gustavaditya | 1:26fbc9316523 | 192 | init_slider = false; |
gustavaditya | 1:26fbc9316523 | 193 | slider.brake(1); |
gustavaditya | 1:26fbc9316523 | 194 | pc.printf("init slider selesai\n"); |
gustavaditya | 1:26fbc9316523 | 195 | } |
gustavaditya | 1:26fbc9316523 | 196 | } |
gustavaditya | 1:26fbc9316523 | 197 | while(init_lifter) |
gustavaditya | 1:26fbc9316523 | 198 | { |
gustavaditya | 1:26fbc9316523 | 199 | lifter.speed(-1.0); |
gustavaditya | 1:26fbc9316523 | 200 | pc.printf("init_lifter\n"); |
gustavaditya | 1:26fbc9316523 | 201 | slider.brake(1); |
gustavaditya | 1:26fbc9316523 | 202 | if(!limitBawah) |
gustavaditya | 1:26fbc9316523 | 203 | { |
gustavaditya | 1:26fbc9316523 | 204 | init_lifter = false; |
gustavaditya | 1:26fbc9316523 | 205 | lifter.brake(1); |
gustavaditya | 1:26fbc9316523 | 206 | pc.printf("init lifter selesai\n"); |
gustavaditya | 1:26fbc9316523 | 207 | } |
gustavaditya | 1:26fbc9316523 | 208 | } |
gustavaditya | 0:e708f9673603 | 209 | joystick.setup(); |
gustavaditya | 0:e708f9673603 | 210 | pc.baud(115200); |
gustavaditya | 0:e708f9673603 | 211 | wait_ms(1000); |
gustavaditya | 0:e708f9673603 | 212 | startMillis(); |
gustavaditya | 0:e708f9673603 | 213 | while(1) |
gustavaditya | 0:e708f9673603 | 214 | { |
gustavaditya | 0:e708f9673603 | 215 | //COBA ROTASI |
gustavaditya | 1:26fbc9316523 | 216 | //pc.printf("MASUK PROGRAM UTAMA\n"); |
gustavaditya | 1:26fbc9316523 | 217 | joystick.idle(); |
gustavaditya | 0:e708f9673603 | 218 | if(joystick.readable()) |
gustavaditya | 0:e708f9673603 | 219 | { |
gustavaditya | 0:e708f9673603 | 220 | // Panggil fungsi pembacaan joystik |
gustavaditya | 0:e708f9673603 | 221 | joystick.baca_data(); |
gustavaditya | 0:e708f9673603 | 222 | // Panggil fungsi pengolahan data joystik |
gustavaditya | 0:e708f9673603 | 223 | joystick.olah_data(); |
gustavaditya | 0:e708f9673603 | 224 | // Masuk ke case joystick |
gustavaditya | 0:e708f9673603 | 225 | case_joy = case_joystick(); |
gustavaditya | 1:26fbc9316523 | 226 | //pc.printf("CASE JOYSTICK\n"); |
gustavaditya | 0:e708f9673603 | 227 | aktuator(); |
gustavaditya | 1:26fbc9316523 | 228 | lifterMove(); |
gustavaditya | 0:e708f9673603 | 229 | |
gustavaditya | 0:e708f9673603 | 230 | if ((millis()-previousMillis3 >= 230)&&(flag_Pneu)){ |
gustavaditya | 0:e708f9673603 | 231 | pneumatik = 1; |
gustavaditya | 0:e708f9673603 | 232 | flag_Pneu = false; |
gustavaditya | 0:e708f9673603 | 233 | ready = true; |
gustavaditya | 0:e708f9673603 | 234 | readySlideFromMiddle = true; |
gustavaditya | 1:26fbc9316523 | 235 | pc.printf("PNEUMATIK ON"); |
gustavaditya | 1:26fbc9316523 | 236 | prevMillis = millis(); |
gustavaditya | 0:e708f9673603 | 237 | //wait_ms(1000); |
gustavaditya | 0:e708f9673603 | 238 | } |
gustavaditya | 1:26fbc9316523 | 239 | |
gustavaditya | 1:26fbc9316523 | 240 | if((millis()-prevMillis>=500) && delay) |
gustavaditya | 1:26fbc9316523 | 241 | { |
gustavaditya | 1:26fbc9316523 | 242 | delay = false; |
gustavaditya | 1:26fbc9316523 | 243 | //prevMillis = millis(); |
gustavaditya | 1:26fbc9316523 | 244 | } |
gustavaditya | 0:e708f9673603 | 245 | } |
gustavaditya | 0:e708f9673603 | 246 | else |
gustavaditya | 0:e708f9673603 | 247 | { |
gustavaditya | 0:e708f9673603 | 248 | joystick.idle(); |
gustavaditya | 0:e708f9673603 | 249 | } |
gustavaditya | 0:e708f9673603 | 250 | } |
gustavaditya | 0:e708f9673603 | 251 | } |