Reloader Siap. Bulat = naik lifter. Kotak = turun Lifter. Slider otomatis. Silang = pneumatik.
Dependencies: Motor mbed millis
Revision 1:26fbc9316523, committed 2017-06-12
- Comitter:
- gustavaditya
- Date:
- Mon Jun 12 09:22:28 2017 +0000
- Parent:
- 0:e708f9673603
- Commit message:
- Reloader Siap
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r e708f9673603 -r 26fbc9316523 main.cpp --- a/main.cpp Sun Jun 11 22:22:03 2017 +0000 +++ b/main.cpp Mon Jun 12 09:22:28 2017 +0000 @@ -12,13 +12,18 @@ bool isUp = false, isDown = false, sliderOn = false; bool ready = true; bool init_slider = true; +bool init_lifter = true; +bool sliderReady = false; +bool flag_tengah = true; +bool delay = true; static volatile uint32_t previousMillis3 = 0; // Pneumatik static volatile uint32_t previousMillis6 = 0; // pneu +static volatile uint32_t prevMillis = 0; // delay int caseJoystick, case_joy; -float lempar = -0.6, balik = 0.6; +float lempar = -0.8, lempar2 = -0.8, balik = 0.6; DigitalOut pneumatik(PA_4, PullUp); DigitalIn limitAtasLifter(PB_3, PullUp); // Vertikal Atas: Lifter @@ -28,7 +33,7 @@ DigitalIn limitTengah(PC_9, PullUp);// Horizontal Tengah DigitalIn limitKanan(PC_8, PullUp); // Horizontal Kanan (Frisbee keluar) -Motor lifter(PA_8, PC_1, PC_2); // pwm, fwd, rev +Motor lifter(PC_7, PC_13, PC_14); // pwm, fwd, rev Motor slider(PA_10, PC_3, PC_0); /* Inisialisasi Pin TX-RX Joystik dan PC */ @@ -44,10 +49,16 @@ else if ((joystick.lingkaran_click)&&(!joystick.kotak_click)) { // Lifter Up caseJoystick = 2; + pc.printf("LINGKARAN\n"); } else if ((joystick.kotak_click)&&(!joystick.lingkaran_click)) { // Lifter Down caseJoystick = 3; + pc.printf("KOTAK\n"); + } + else{ + caseJoystick = 0; + //pc.printf("DO NOTHING\n"); } return caseJoystick; @@ -70,115 +81,140 @@ case (2): // Lifter Up ReloadOn = !ReloadOn; - isUp = true; + //isUp = 1; isDown = false; + pc.printf("%d\n",isUp); + break; case (3): // Lifter Down ReloadOn = !ReloadOn; - isUp = false; + //isUp = 0; isDown = true; break; } } -void reloader() +void sliderMove() +{ + if (readySlideFromLeft) + { + slider.speed(lempar2); + if(!limitTengah && flag_tengah) + { + slider.brake(1); + readySlideFromLeft = false; + readySlideFromMiddle = false; + getBack = false; + flag_tengah = false; + } + else { flag_tengah = true;} + } + else if (readySlideFromMiddle) + { + slider.speed(lempar); + if(!limitKanan) + { + readySlideFromMiddle = false; + getBack = true; + } + } + else if (getBack) + { + slider.speed(balik); + if(!limitKiri) + { + slider.brake(1); + readySlideFromLeft = false; + readySlideFromMiddle = false; + getBack = false; + sliderReady = false; + } + } + else + { + slider.brake(1); + } +} + +void lifterMove() { if(ReloadOn) { - if (isUp && !isDown && !sliderOn) - { - lifter.speed(1.0); - if(!limitAtasSlider) - { - sliderOn = true; - isUp = false; - isDown = false; - readySlideFromLeft = true; - } - - if(!limitAtasLifter) - { - isUp = false; - sliderOn = false; - isDown = true; - } - } - - if (!isUp && !isDown && sliderOn) - { - if (readySlideFromLeft) - { - slider.speed(lempar); - if (!limitTengah) - { - readySlideFromLeft = false; - slider.brake(1); - } - } - else if (readySlideFromMiddle) - { - slider.speed(lempar); - if (!limitKanan) - { - readySlideFromMiddle = false; - getBack = true; - slider.brake(1); - } - } - else if (getBack) - { - slider.speed(balik); - if (!limitKiri) - { - readySlideFromLeft = true; - sliderOn = false; - isUp = true; - isDown = false; - getBack = false; - slider.brake(1); - } - } - else - { slider.brake(1);} - } - - if (!isUp && isDown && !sliderOn) + if(isDown) { lifter.speed(-1.0); if(!limitBawah) { - ReloadOn = !ReloadOn; - lifter.brake(1); + ReloadOn = false; + isDown = false; } } + else if (!limitAtasLifter) + { + isDown = true; + } + else if(sliderReady) + { + lifter.brake(1); + if (!delay) + { + sliderMove(); + } + } + else if(!limitAtasSlider) + { + sliderReady = true; + readySlideFromLeft = true; + delay = true; + prevMillis = millis(); + } + else + { + lifter.speed(1.0); + pc.printf("NAIK\n"); + } } else { lifter.brake(1); - } + } } int main(void) { + while(init_slider) + { + slider.speed(balik); + pc.printf("init_slider\n"); + if(!limitKiri) + { + init_slider = false; + slider.brake(1); + pc.printf("init slider selesai\n"); + } + } + while(init_lifter) + { + lifter.speed(-1.0); + pc.printf("init_lifter\n"); + slider.brake(1); + if(!limitBawah) + { + init_lifter = false; + lifter.brake(1); + pc.printf("init lifter selesai\n"); + } + } joystick.setup(); pc.baud(115200); wait_ms(1000); - /*while (init_slider) - { - slider.speed(balik); - if (!limitKiri) - { - readySlideFromLeft = true; - slider.brake(1); - init_slider = false; - } - }*/ startMillis(); while(1) { //COBA ROTASI - /*joystick.idle(); + //pc.printf("MASUK PROGRAM UTAMA\n"); + joystick.idle(); if(joystick.readable()) { // Panggil fungsi pembacaan joystik @@ -187,24 +223,29 @@ joystick.olah_data(); // Masuk ke case joystick case_joy = case_joystick(); + //pc.printf("CASE JOYSTICK\n"); aktuator(); - reloader(); + lifterMove(); if ((millis()-previousMillis3 >= 230)&&(flag_Pneu)){ pneumatik = 1; flag_Pneu = false; ready = true; readySlideFromMiddle = true; + pc.printf("PNEUMATIK ON"); + prevMillis = millis(); //wait_ms(1000); } + + if((millis()-prevMillis>=500) && delay) + { + delay = false; + //prevMillis = millis(); + } } else { joystick.idle(); - }*/ - if (!limitKiri) - { - pc.printf("limit kiri\n"); } } } \ No newline at end of file