File content as of revision 1:e8fcbd118f4d:

#include "mbed.h"
#include "JoystickPS3.h"
#include "Motor.h"

const float pwmPowerUp   = 1.0;
const float pwmPowerDown = -1.0;
const float pwmPulley = 0.1;

int  case_joy;
bool isLauncher = false;
bool isReload = false;
bool ReloadOn = false;
bool flag_Pneu = false;
bool flag_paku = false;

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 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_6, PA_13, PB_0); // pwm, fwd, rev
Motor pulley(PB_7, PA_14, PA_15);                

int case_joystick()
{
    int caseJoystick;
    if ((joystick.lingkaran_click)&&(!joystick.kotak_click))  {   
        // Power Screw Up
        caseJoystick = 11;
    } 
    else if ((joystick.kotak_click)&&(!joystick.lingkaran_click)) {   
        // Power Screw Down
        caseJoystick = 12;       
    }
    return(caseJoystick);
}

void aktuator() 
{
    switch (case_joy)
    {
        case (11) : 
        {
           // Power Screw Up
           ReloadOn = !ReloadOn;
//           isReload = false;
           break;
        }
        case (12) : 
        {
           // Power Screw Down
           ReloadOn = !ReloadOn;
//           isReload = true;
           break;
        }
        // tambahin yang manual ya ton :)
    }
}

void reloader()
{
    if(ReloadOn)
    {
        if(limitAtasSaucer && !limitKiri)
            powerScrew.speed(pwmPowerUp);
        else
        {
            powerScrew.brake(1);
        }
        
        if(!limitAtasLifter) 
        {
            if (!limitBawah) 
            {
                powerScrew.brake(1);
            }
            else
            {
                powerScrew.speed(pwmPowerDown);
            }
        }
    }       
}

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);
            }
        }
    }
}

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();
        }
    }
}