Diff: main.cpp

Revision:

1:26fbc9316523

Parent:

0:e708f9673603

--- a/main.cpp	Sun Jun 11 22:22:03 2017 +0000
+++ b/main.cpp	Mon Jun 12 09:22:28 2017 +0000
@@ -12,13 +12,18 @@
 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.6, balik = 0.6;
+float lempar = -0.8, lempar2 = -0.8, balik = 0.6;
  
 DigitalOut pneumatik(PA_4, PullUp);
 DigitalIn limitAtasLifter(PB_3, PullUp);  // Vertikal Atas: Lifter
@@ -28,7 +33,7 @@
 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 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 */
@@ -44,10 +49,16 @@
     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;    
@@ -70,115 +81,140 @@
         case (2): 
             // Lifter Up
             ReloadOn = !ReloadOn;
-            isUp = true;
+            //isUp = 1;
             isDown = false;
+            pc.printf("%d\n",isUp);
+            
             break;
         case (3): 
             // Lifter Down
             ReloadOn = !ReloadOn;
-            isUp = false;
+            //isUp = 0;
             isDown = true;
             break;
     }    
 }
 
-void reloader()
+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 (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)
+        if(isDown)
         {
             lifter.speed(-1.0);
             if(!limitBawah)
             {
-                ReloadOn = !ReloadOn;  
-                lifter.brake(1); 
+                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);
-    /*while (init_slider)
-    {
-        slider.speed(balik);
-        if (!limitKiri)
-        {
-            readySlideFromLeft = true;
-            slider.brake(1);
-            init_slider = false;   
-        }   
-    }*/
     startMillis();
     while(1)
     {
         //COBA ROTASI
-        /*joystick.idle();        
+        //pc.printf("MASUK PROGRAM UTAMA\n");
+        joystick.idle();        
         if(joystick.readable()) 
         {
             // Panggil fungsi pembacaan joystik
@@ -187,24 +223,29 @@
             joystick.olah_data();
             // Masuk ke case joystick
             case_joy = case_joystick();
+            //pc.printf("CASE JOYSTICK\n");
             aktuator();
-            reloader();
+            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();
-        }*/
-        if (!limitKiri)
-        {
-            pc.printf("limit kiri\n");   
         }
     }
 }
\ No newline at end of file