Diff: main.cpp

Revision:

0:e708f9673603

Child:

1:26fbc9316523

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Jun 11 22:22:03 2017 +0000
@@ -0,0 +1,210 @@
+#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;
+
+static volatile uint32_t previousMillis3 = 0;  // Pneumatik
+static volatile uint32_t previousMillis6 = 0;  // pneu
+
+int caseJoystick, case_joy;
+
+float lempar = -0.6, 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(PA_8, PC_1, PC_2); // 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;
+    } 
+    else if ((joystick.kotak_click)&&(!joystick.lingkaran_click)) {   
+        // Lifter Down
+        caseJoystick = 3;
+    }
+    
+    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 = true;
+            isDown = false;
+            break;
+        case (3): 
+            // Lifter Down
+            ReloadOn = !ReloadOn;
+            isUp = false;
+            isDown = true;
+            break;
+    }    
+}
+
+void reloader()
+{
+    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)
+        {
+            lifter.speed(-1.0);
+            if(!limitBawah)
+            {
+                ReloadOn = !ReloadOn;  
+                lifter.brake(1); 
+            }   
+        }
+    }
+    else
+    {
+        lifter.brake(1);
+    }
+}
+
+int main(void)
+{
+    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();        
+        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();
+            aktuator();
+            reloader();
+            
+            if ((millis()-previousMillis3 >= 230)&&(flag_Pneu)){
+                pneumatik = 1;
+                flag_Pneu = false;
+                ready = true;
+                readySlideFromMiddle = true;
+                //wait_ms(1000);
+            }
+        }
+        else
+        {
+            joystick.idle();
+        }*/
+        if (!limitKiri)
+        {
+            pc.printf("limit kiri\n");   
+        }
+    }
+}
\ No newline at end of file