Reloader Siap. Bulat = naik lifter. Kotak = turun Lifter. Slider otomatis. Silang = pneumatik.
Dependencies: Motor mbed millis
main.cpp
- Committer:
- gustavaditya
- Date:
- 2017-06-12
- Revision:
- 1:26fbc9316523
- Parent:
- 0:e708f9673603
File content as of revision 1:26fbc9316523:
#include "Motor.h" #include "millis.h" #include "mbed.h" #include "JoystickPS3.h" bool isReload = false; bool ReloadOn = false; bool flag_Pneu = false; bool readySlideFromLeft = false; bool readySlideFromMiddle = false; bool getBack = false; 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.8, lempar2 = -0.8, balik = 0.6; DigitalOut pneumatik(PA_4, PullUp); DigitalIn limitAtasLifter(PB_3, PullUp); // Vertikal Atas: Lifter DigitalIn limitAtasSlider(PB_2, PullUp); // Vertikal Atas: Saucer DigitalIn limitBawah(PB_10, PullUp); // Vertikal Bawah DigitalIn limitKiri(PA_5, PullUp); // Horizontal Kiri DigitalIn limitTengah(PC_9, PullUp);// Horizontal Tengah DigitalIn limitKanan(PC_8, PullUp); // Horizontal Kanan (Frisbee keluar) 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 */ joysticknucleo joystick(PA_0,PA_1); Serial pc(USBTX,USBRX); int case_joystick() { if (joystick.silang_click){ // Pnemuatik ON caseJoystick = 1; } 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; } void aktuator() { switch (case_joy) { case (1): // Pneumatik if (ready) { pneumatik = 0; previousMillis3 = millis(); flag_Pneu = true; ready = false; previousMillis6 = millis(); } break; case (2): // Lifter Up ReloadOn = !ReloadOn; //isUp = 1; isDown = false; pc.printf("%d\n",isUp); break; case (3): // Lifter Down ReloadOn = !ReloadOn; //isUp = 0; isDown = true; break; } } 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(isDown) { lifter.speed(-1.0); if(!limitBawah) { 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); startMillis(); while(1) { //COBA ROTASI //pc.printf("MASUK PROGRAM UTAMA\n"); joystick.idle(); if(joystick.readable()) { // Panggil fungsi pembacaan joystik joystick.baca_data(); // Panggil fungsi pengolahan data joystik joystick.olah_data(); // Masuk ke case joystick case_joy = case_joystick(); //pc.printf("CASE JOYSTICK\n"); aktuator(); 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(); } } }