KRAI 2017
/
Test_LimitSwitch_Nasional
Test Limit Switch Reloader
Diff: main.cpp
- Revision:
- 1:e8fcbd118f4d
- Parent:
- 0:f510a9c32a2c
--- a/main.cpp Sun Jun 11 08:10:57 2017 +0000 +++ b/main.cpp Sun Jun 11 09:26:47 2017 +0000 @@ -2,8 +2,9 @@ #include "JoystickPS3.h" #include "Motor.h" -const float pwmPowerUp = 1.0; -const float pwmPowerDown = -1.0; +const float pwmPowerUp = 1.0; +const float pwmPowerDown = -1.0; +const float pwmPulley = 0.1; int case_joy; bool isLauncher = false; @@ -15,18 +16,22 @@ bool ready = false; int case_joystick(); +void motorPulley(); +void reloader(); joysticknucleo joystick(PA_0,PA_1); Serial pc(USBTX,USBRX); //DigitalIn infraAtas(PC_9, PullUp); -DigitalIn limitAtas(PA_9, PullUp); -DigitalIn limitBawah(PC_7, PullUp); -DigitalIn limitKiri(PA_7, PullUp); -DigitalIn limitTengah(PB_7, PullUp); -DigitalIn limitKanan(PB_9, PullUp); +DigitalIn limitAtasLifter(PB_3, PullUp); // Vertikal Atas: Lifter +DigitalIn limitAtasSaucer(PB_10, PullUp); // Vertikal Atas: Saucer +DigitalIn limitBawah(PB_2, 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 powerScrew(PB_10, PB_14, PB_13); // pwm, fwd, rev +Motor powerScrew(PB_6, PA_13, PB_0); // pwm, fwd, rev +Motor pulley(PB_7, PA_14, PA_15); int case_joystick() { @@ -50,37 +55,112 @@ { // Power Screw Up ReloadOn = !ReloadOn; - isReload = false; +// isReload = false; break; } case (12) : { // Power Screw Down ReloadOn = !ReloadOn; - isReload = true; +// isReload = true; break; } + // tambahin yang manual ya ton :) } } void reloader() { - if(ReloadOn){ - if(isReload){ - powerScrew.speed(pwmPowerDown); - //pc.printf("%.2f\n", jarak_ping); - if(!limitBawah){ - isReload = false; - ReloadOn = false; + if(ReloadOn) + { + if(limitAtasSaucer && !limitKiri) + powerScrew.speed(pwmPowerUp); + else + { + powerScrew.brake(1); + } + + if(!limitAtasLifter) + { + if (!limitBawah) + { + powerScrew.brake(1); + } + else + { + powerScrew.speed(pwmPowerDown); } } - else if(!limitAtas){ - isReload = true; - powerScrew.speed(pwmPowerUp); + } +} + +void motorPulley() +{ + if (!limitKiri) + { + pulley.brake(1); + do + { + pulley.speed(pwmPulley); + }while(limitTengah); + + } + else + { + if(!limitTengah) + { + pulley.brake(1); + } + else + { + if(!limitKanan) + { + pulley.brake(1); + do + { + pulley.speed(-pwmPulley); + }while(limitKiri); + } + else + { + do + { + pulley.speed(pwmPulley); + }while(!limitKiri); + } } } - else{ - powerScrew.brake(1); +} + +int main() +{ + // Set baud rate - 115200 + joystick.setup(); + pc.baud(115200); + wait_ms(1000); + while(1) + { + // Interrupt Serial + 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("%d\n",case_joy); + reloader(); + motorPulley(); + + } + else + { + joystick.idle(); + } } } + + \ No newline at end of file