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