Diff: main.cpp

Revision:

1:e8fcbd118f4d

Parent:

0:f510a9c32a2c

diff -r f510a9c32a2c -r e8fcbd118f4d main.cpp
--- 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