Diff: main.cpp

Revision:

17:e4229d77a5ab

Parent:

16:90119f03c5d1

Child:

18:1da121ddb7c1

--- a/main.cpp	Tue Jan 24 12:34:29 2017 +0000
+++ b/main.cpp	Fri Jan 27 14:42:17 2017 +0000
@@ -47,6 +47,10 @@
 #define PI 3.14159265
 #define D_ENCODER 0.058
 #define D_ROBOT 0.64
+#define VMAX 0.3            // Maju, Mundur, Kiri Kanan
+#define SAMPING 0.3         // Saka, Saki, Sbka, Sbki
+#define PIVOT 0.4           // Pivka, Pivki
+#define PERPINDAHAN 1       // Perpindahan ke kanan dan kiri
 
 float k_enc     = PI*D_ENCODER;
 float k_robot   = PI*D_ROBOT;
@@ -103,7 +107,7 @@
 
 /* Variabel Stick */
 char case_ger;
-bool launcher = false,servoGo = false;
+bool launcher = false, servoGo = false, manual = true, caseTadi = false, caseSekarang = false;
 
 /***********************************************/
 /*         Deklarasi Fungsi dan Procedure      */
@@ -159,10 +163,12 @@
     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri))  {   
         // Kanan
         casegerak = 9;
+        caseSekarang = true;
     } 
     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {   
         // Kiri
-        casegerak = 10;        
+        casegerak = 10; 
+        caseSekarang = true;       
     } 
     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {  
         casegerak = 12; 
@@ -203,53 +209,151 @@
     }
     
     // MOTOR Bawah
-    switch (case_ger) {
-        case (1):{       
-            Tetha = Tetha - 0.05;
+    if (manual) { 
+        // Mode Manual
+        switch (case_ger) {
+        case (1): {       
+            // Pivot Kanan
+            motor1.speed(-PIVOT);
+            motor2.speed(-PIVOT);
+            motor3.speed(-PIVOT);
+            motor4.speed(-PIVOT);
             break;
-        }
-        case (2):{
-            Tetha = Tetha + 0.05;     
+            }
+        case (2): {
+            // Pivot Kiri
+            motor1.speed(PIVOT);
+            motor2.speed(PIVOT);
+            motor3.speed(PIVOT);
+            motor4.speed(PIVOT);
+            break;
+           }
+        case (3): {   
+            // Maju
+            motor1.speed(-VMAX);
+            motor2.speed(VMAX);
+            motor3.speed(VMAX);
+            motor4.speed(-VMAX);
             break;
         }
-        case (3):{   
-            YT = YT + 0.01;
+         case (4): { 
+            // Mundur
+            motor1.speed(VMAX);
+            motor2.speed(-VMAX);
+            motor3.speed(-VMAX);
+            motor4.speed(VMAX);
+            break;
+        }
+         case (5) : {   
+            // Samping Atas Kanan  
+            motor1.speed(-SAMPING);
+            motor2.brake(1);
+            motor3.speed(SAMPING);
+            motor4.brake(1);
             break;
         }
-        case (4):{ 
-            YT = YT - 0.01;
+         case (6) : {   
+            // Samping Bawah Kanan
+            motor1.brake(1);
+            motor2.speed(-SAMPING);
+            motor3.brake(1);
+            motor4.speed(SAMPING);
             break;
         }
-        case (5) :{   
-            XT = XT + 0.01;
-            YT = YT + 0.01;     
+        case (7) : {   
+            // Samping Atas Kiri
+            motor1.brake(1);
+            motor2.speed(SAMPING);
+            motor3.brake(1);
+            motor4.speed(-SAMPING);
+            break;
+        }
+         case (8) : {   
+            // Samping Bawah Kiri
+            motor1.speed(SAMPING);
+            motor2.brake(1);
+            motor3.speed(-SAMPING);
+            motor4.brake(1);
             break;
         }
-        case (6) :{   
-            XT = XT + 0.01;
-            YT = YT - 0.01;
-            break;
-        }
-        case (7) :{   
-            XT = XT - 0.01;
-            YT = YT + 0.01;
+        case (9) : {
+            // Kanan
+            motor1.speed(-VMAX);
+            motor2.speed(-VMAX);
+            motor3.speed(VMAX);
+            motor4.speed(VMAX);
             break;
         }
-        case (8) :{   
-            XT = XT - 0.01;
-            YT = YT - 0.01;
-            break;
-        }
-        case (9) :{   
-            XT = XT + 0.01;
+        case (10) : {
+            // Kiri
+            motor1.speed(VMAX);
+            motor2.speed(VMAX);
+            motor3.speed(-VMAX);
+            motor4.speed(-VMAX);
             break;
-        }
-        case (10) :{   
-            XT = XT - 0.01;
+            }
+        default : {
+                motor1.brake(1);
+                motor2.brake(1);
+                motor3.brake(1);
+                motor4.brake(1);
+            }   
+        } // End Switch 
+    } else { 
+        //Mode Encoder
+        switch (case_ger) {
+            case (1):{       
+                Tetha = Tetha - 0.05;
+                break;
+            }
+            case (2):{
+                Tetha = Tetha + 0.05;     
+                break;
+            }
+            case (3):{   
+                YT = YT + 0.01;
+                break;
+            }
+            case (4):{ 
+                YT = YT - 0.01;
+                break;
+            }
+            case (5) :{   
+                XT = XT + 0.01;
+                YT = YT + 0.01;     
+                break;
+            }
+            case (6) :{   
+                XT = XT + 0.01;
+                YT = YT - 0.01;
+                break;
+            }
+            case (7) :{   
+                XT = XT - 0.01;
+                YT = YT + 0.01;
+                break;
+            }
+            case (8) :{   
+                XT = XT - 0.01;
+                YT = YT - 0.01;
+                break;
+            }
+            case (9) :{   
+            // Kanan
+                if (caseSekarang != caseTadi)   XT = XT + PERPINDAHAN;
+                caseTadi = caseSekarang;
+                
             break;
-        }
-        default :{}  
-    }   //end of switch  
+            }
+            case (10) :{  
+            // Kiri 
+                if (caseSekarang!= caseTadi)    XT = XT - PERPINDAHAN;
+                caseTadi = caseSekarang;
+            break;
+            }
+            default :{}  
+        }   //end of switch
+    }
 }
 
 void setCenter(){
@@ -386,8 +490,9 @@
             case_ger = case_gerak();
             aktuator();
 
-            if (joystick.segitiga_click)    launcher  = !launcher;
-            if (joystick.lingkaran_click)   servoGo = true;
+            if (joystick.segitiga_click)    launcher    = !launcher;
+            if (joystick.lingkaran_click)   servoGo     = true;
+            if (joystick.SELECT_click)      manual      = !manual;
             if (joystick.silang) {
                 XT = 0;
                 YT = 0;