Diff: main.cpp

Revision:

16:90119f03c5d1

Parent:

15:98f0d56b14f0

Child:

17:e4229d77a5ab

diff -r 98f0d56b14f0 -r 90119f03c5d1 main.cpp
--- a/main.cpp	Fri Dec 09 12:03:44 2016 +0000
+++ b/main.cpp	Tue Jan 24 12:34:29 2017 +0000
@@ -28,10 +28,10 @@
 /*  Tombol segitiga => Aktif motor Launcher                                 */
 /*  Tombol lingkaran=> Aktif servo Launcher                                 */
 /*  Tombol L3       => PWM Launcher dikurangin                              */
-/*  Tombol R3       => PWM Launcher ditambahin                                                                        */
+/*  Tombol R3       => PWM Launcher ditambahin                              */
 /*                                                                          */
-/*                                                                          */
-/*                                                                          */
+/*  Bismillahirahmanirrahim                                                 */
+/*  Jagalah Kebersihan kodingan                                             */
 /****************************************************************************/
 
 
@@ -39,209 +39,141 @@
 #include "JoystickPS3.h"
 #include "Motor.h"
 #include "Servo.h"
-
-//#define koefperlambatan 0.8
 #include "encoderKRAI.h"
 
-#define pi 22/7
-#define diaEncoder 0.058
-#define PPR 1000
-#define diaRobot 0.64
-#define pinservo1 PC_8
-//PC 9
-#define pinservo2 PC_9
-
-float K_enc = pi*diaEncoder;
-float K_robot = pi*diaRobot;
+/***********************************************/
+/*          Konstanta dan Variabel             */
+/***********************************************/
+#define PI 3.14159265
+#define D_ENCODER 0.058
+#define D_ROBOT 0.64
 
-float speed1=0.6;
-float speed2=0.6;
-float speed3=0.6;
-float speed4=0.6;
-float speedB=0.43 ;
-float speedL=0.4;
+float k_enc     = PI*D_ENCODER;
+float k_robot   = PI*D_ROBOT;
 
-float KpX=0.5, KpY=0.5, Kp_tetha=0.03;
-
-Timer waktu;
-//float jarakGlobalY;
+float speed1    =0.6;
+float speed2    =0.6;
+float speed3    =0.6;
+float speed4    =0.6;
+float speedB    =0.43;
+float speedL    =0.4;
 
-// Deklarasi variabel PID
-//PID PID(0.992,0.000,0.81,0.001); //(P,I,D, time sampling)
-/*
-// Deklarasi variabel encoder
-encoderKRAI encoderDepan( PB_14, PB_13, 2000, encoderKRAI::X2_ENCODING);   //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
-encoderKRAI encoderBelakang( PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING);
-encoderKRAI encoderKanan( PD_2, PC_12, 720, encoderKRAI::X2_ENCODING);
-encoderKRAI encoderKiri( PC_11, PC_10, 2000, encoderKRAI::X2_ENCODING);
-*/
+float kpX=0.5, kpY=0.5, kp_tetha=0.03;
+
+/* Deklarasi encoder */
+encoderKRAI encoderDepan( PB_13, PB_14, 2000, encoderKRAI::X2_ENCODING);    //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
+encoderKRAI encoderBelakang( PC_11, PC_10, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
+encoderKRAI encoderKanan( PC_12, PD_2, 720, encoderKRAI::X2_ENCODING);      //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
+encoderKRAI encoderKiri( PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING);      //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
 
-// Deklarasi variabel encoder
-encoderKRAI encoderDepan( PB_13, PB_14, 2000, encoderKRAI::X2_ENCODING);   //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
-encoderKRAI encoderBelakang( PC_11, PC_10, 2000, encoderKRAI::X2_ENCODING);
-encoderKRAI encoderKanan( PC_12, PD_2, 720, encoderKRAI::X2_ENCODING);
-encoderKRAI encoderKiri( PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING);
+/* Deklarasi Motor Base */
+Motor motor1(PB_7, PA_14 , PA_15);  // pwm, fwd, rev
+Motor motor2(PB_8, PA_13 ,PB_0);    // pwm, fwd, rev
+Motor motor3(PB_9, PA_12 ,PC_5);    // pwm, fwd, rev
+Motor motor4(PB_6, PB_1 ,PB_12);    // pwm, fwd, rev
 
-// Deklarasi variabel motor
-Motor motor1(PB_7, PA_14 , PA_15); // pwm, fwd, rev
-Motor motor2(PB_8, PA_13 ,PB_0); // pwm, fwd, rev
-Motor motor3(PB_9, PA_12 ,PC_5); // pwm, fwd, rev
-Motor motor4(PB_6, PB_1 ,PB_12); // pwm, fwd, rev
+/* Deklarasi Motor Launcher */
+Motor motorld(PA_8, PC_1 , PC_2);   // pwm, fwd, rev
+Motor motorlb(PA_9, PA_4, PC_15 );  // pwm, fwd, rev
 
-//Motor Atas
-Motor motorld(PA_8, PC_1 , PC_2); // pwm, fwd, rev
-Motor motorlb(PA_9, PA_4, PC_15 ); // pwm, fwd, rev
-
-//Servo Atas
+/* Deklarasi Servo Launcher */
 Servo servoS(PB_2);
 Servo servoB(PA_5);
 
-// Deklarasi variabel posisi robot
-//robotPos posisi();
-
-// Inisialisasi  Pin TX-RX Joystik dan PC
-//Serial pc(PA_0,PA_1);
-//Serial pc(USBTX,USBRX);
-// Deklarasi Variabel Global
-/*
- * posX dan posY berdasarkan arah robot
- * encoder Depan & Belakang sejajar sumbu Y
- * encoder Kanan & Kiri sejajar sumbu X 
- */
-float jarak, posX, posY;
-//float keliling = pi*diameterRoda;
+/**
+ *  posX dan posY berdasarkan arah robot
+ *  encoder Depan & Belakang sejajar sumbu Y
+ *  encoder Kanan & Kiri sejajar sumbu X 
+ **/
 
-void detect_encoder(float speed);
-void setCenter();
-float getY();
-float getX();
-float getTetha();
+/* Variabel Encoder */
+float jarak, posX, posY;            // 
+float XT, YT, Tetha;                // Jarak Target Robot
+float errX, errY, errT, Vt, Vx, Vy; // Variabel yang didapatkan encoder
+float v1, v2, v3, v4;               // Variabel kecepatan motor dari encoder
 
-// Inisialisasi  Pin TX-RX Joystik dan PC
+/* Fungsi dan Procedur Encoder */
+void setCenter();                   // Fungsi reset agar robot di tengah
+float getY();                       // FUngsi mendapatkan jarak Y
+float getX();                       // FUngsi mendapatkan jarak X
+float getTetha();                   // FUngsi mendapatkan jarak Tetha
+
+/* Inisialisasi  Pin TX-RX Joystik dan PC */
 joysticknucleo joystick(PA_0,PA_1);
-Serial pc(USBTX,USBRX);
 
-// Posisi target
-float XT, YT, Tetha;
-
-//encoder variable
-float errX, errY, errT, Vt, Vx, Vy;
-float V1, V2, V3, V4;
-
-//bool perlambatan=0;
+/* Variabel Stick */
 char case_ger;
-bool maju=false,mundur=false,pivka=false,pivki=false,kiri=false,kanan=false,saka=false,saki=false,sbka=false,sbki=false,cw1=false,ccw1=false,cw2=false,ccw2=false,cw3=false,ccw3=false,analog=false;
-bool stop = true;
-bool Launcher = false,ServoGo = false;
-float jLX,jLY;
-double vcurr;
-float x,y; // untuk analoghat kiri
-float errorx=0,errory=0;
-
-// Fungsi mapping 0-255 ke -128 sampai 127
-float mapping(float a,float error)
-{   
-     float hasil,b;
-    b = (float)((a-128)/128);
-    if (b>(error - 0.2) && b<(error + 0.2))
-    {
-        hasil = 0;
-    } else {
-        hasil = b;
-        }
-    return (hasil);
-}
+bool launcher = false,servoGo = false;
 
-// Kalibrasi tombol analog kiri
-void kalibrasi()
-{
-    errorx = (jLX - 128)/128;
-    errory = (jLY - 128)/128;    
-
-}
-
-// simpan data analog
-void baca_analog()
-{
-        jLX = joystick.LX;
-        jLY = joystick.LY;
-        
-        // Pembacaan nilai Y terbalik
-        x = mapping(jLX,errorx);
-        y = -mapping(jLY,errory); 
-}
-
-// Gerak dari Motor base
-int case_gerak()
-{
+/***********************************************/
+/*         Deklarasi Fungsi dan Procedure      */
+/***********************************************/
+int case_gerak(){
+/*****************************************************
+ **  Gerak Motor Base
+ **  Case 1  : Pivot kanan
+ **  Case 2  : Pivot Kiri
+ **  Case 3  : Maju
+ **  Case 4  : Mundur
+ **  Case 5  : Serong Atas Kanan
+ **  Case 6  : Serong Bawah Kanan
+ **  Case 7  : Serong Atas Kiri
+ **  Case 8  : Serong Bawah Kiri
+ **  Case 9  : Kanan
+ **  Case 10 : Kiri
+ **  Case 12 : break
+ ****************************************************/ 
     int casegerak;
-    baca_analog();
-     if (!joystick.L1 && joystick.R1) {
+    if (!joystick.L1 && joystick.R1) {
         // Pivot Kanan
         casegerak = 1;
-    } else if (!joystick.R1 && joystick.L1) {
+    } 
+    else if (!joystick.R1 && joystick.L1) {
         // Pivot Kiri
         casegerak = 2;
-    } else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {  
+    } 
+    else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {  
         // Maju
         casegerak = 3;
-    } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
+    } 
+    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
         // Mundur
         casegerak = 4;
-    } else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan))   {   
+    } 
+    else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan))   {   
         // Serong Atas Kanan
         casegerak = 5;
-    } else if((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan)) {   
+    } 
+    else if((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan)) {   
         // Serong Bawah Kanan
         casegerak = 6;
-    } else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)) {   
+    } 
+    else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)) {   
         // Serong Atas Kiri
         casegerak = 7;
-    } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)) {   
+    } 
+    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)) {   
         // Serong Bawah Kiri
         casegerak = 8;
-    } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri))  {   
+    } 
+    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri))  {   
         // Kanan
         casegerak = 9;
-    } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {   
+    } 
+    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {   
         // Kiri
         casegerak = 10;        
-    } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {  
-        // case gerak analog on off
-        if (analog){
-            casegerak = 11;
-        } else {
-            casegerak = 12;
-        }    
-}
+    } 
+    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {  
+        casegerak = 12; 
+    }
     return(casegerak);
 }
 
-
-
-/**
-
-**  Case 1  : Pivot kanan
-**  Case 2  : Pivot Kiri
-**  Case 3  : Maju
-**  Case 4  : Mundur
-**  Case 5  : Serong Atas Kanan
-**  Case 6  : Serong Bawah Kanan
-**  Case 7  : Serong Atas Kiri
-**  Case 8  : Serong Bawah Kiri
-**  Case 9  : Kanan
-**  Case 10 : Kiri
-**  Case 11 : Analog
-**  Case 11 : break
-
-**/
-
-
-// Pergerakan dari base
-void aktuator()
-{
-    //Servo
-    if (ServoGo){
+void aktuator(){
+/* Fungsi untuk menggerakkan servo */
+    // Servo
+    if (servoGo){
         servoS.position(20);
         wait_ms(500);
         servoS.position(-28);
@@ -254,16 +186,15 @@
         }
         wait_ms(500);
         servoB.position(0);
-        ServoGo = false;
-        
-    }else{
+        servoGo = false;
+    }
+    else{
         servoS.position(20);
         servoB.position(0);
-    
     }
     
     // Motor Atas
-    if (Launcher) {
+    if (launcher) {
             motorld.speed(speedL);
             motorlb.speed(speedB);
     }else{
@@ -272,317 +203,201 @@
     }
     
     // MOTOR Bawah
-    switch (case_ger) 
-    {
-case (1): 
-        {       
-           Tetha = Tetha - 0.05;
-            pivka=true;         
-            maju=mundur=analog=kiri=kanan=saka=saki=sbka=sbki=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
-            
-            //pc.printf("pivKa  Xt =%.2f x=%.2f YT=%.2f y=%.2f errx=%.2f erry=%.2f \n",XT,x,YT,y,errX,errY);
-            
-         
+    switch (case_ger) {
+        case (1):{       
+            Tetha = Tetha - 0.05;
             break;
         }
-    case (2):
-           {
-             Tetha = Tetha + 0.05; 
-            
-            pivki=true; 
-            maju=mundur=kiri=analog=kanan=saka=saki=sbka=sbki=pivka=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
-            
-            //pc.printf("pivKi  Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
-            
-         
-            break;
-           }
-    case (3):
-        {   
-           YT = YT + 0.01;
-            
-            maju=true;             
-            mundur=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
-
-            //pc.printf("maju Xt =%.2f x=%.2f YT=%.2f y=%.2f  errx=%.2f erry=%.2f \n",XT,x,YT,y,errX,errY);
-
+        case (2):{
+            Tetha = Tetha + 0.05;     
             break;
         }
-    case (4):
-        { 
-            YT = YT - 0.01;
-
-            mundur=true; 
-            maju=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
-
-            //pc.printf("mundur Xt =%.2f x=%.2f YT=%.2f y=%.2f errx=%.2f erry=%.2f \n",XT,x,YT,y,errX,errY);
-
-    
+        case (3):{   
+            YT = YT + 0.01;
             break;
         }
-    case (5) :
-        {   
-            XT = XT + 0.01;
-            YT = YT + 0.01;
-            
-            saka=true; 
-            maju=mundur=kiri=kanan=sbka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
-            
-            //pc.printf("saka Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
-            
-            
+        case (4):{ 
+            YT = YT - 0.01;
             break;
         }
-    case (6) :
-        {   
-             XT = XT + 0.01;
-            YT = YT - 0.01;
-
-            
-            sbka=true; 
-            maju=mundur=kiri=kanan=saka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
-            
-            //pc.printf("sbka  Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
-
-    
+        case (5) :{   
+            XT = XT + 0.01;
+            YT = YT + 0.01;     
             break;
         }
-    case (7) :
-        {   
-         XT = XT - 0.01;
-         YT = YT + 0.01;
-
-            
-            saki=true; 
-            maju=kiri=kanan=saka=mundur=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
-            
-            //pc.printf("saki Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
-            
-    
+        case (6) :{   
+            XT = XT + 0.01;
+            YT = YT - 0.01;
             break;
         }
-    case (8) :
-        {   
-           XT = XT - 0.01;
-           YT = YT - 0.01;
-         
-            //pc.printf("sbki Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
-            
-    
+        case (7) :{   
+            XT = XT - 0.01;
+            YT = YT + 0.01;
             break;
         }
-    case (9) :
-        {   
-            XT = XT + 0.01;
-            kanan=true; 
-            maju=kiri=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
-            
-            //pc.printf("Kanan Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
-            
+        case (8) :{   
+            XT = XT - 0.01;
+            YT = YT - 0.01;
             break;
         }
-    case (10) :
-        {   
-          XT = XT - 0.01;
-            
-            kiri=true; 
-            maju=kanan=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
-            
-            //pc.printf("Kiri Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
-            
-              
+        case (9) :{   
+            XT = XT + 0.01;
             break;
         }
-    case (11): 
-        {          
-                     
-         XT = XT + 0.01*x;
-         YT = YT + 0.01*y;
-                       
-            analog=true;
-            maju=mundur=kiri=kanan=saka=saki=sbka=sbki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
-            
-            //pc.printf("analog  Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
-            
+        case (10) :{   
+            XT = XT - 0.01;
             break;
-    }
-        default :
-        {
-          
-            maju=mundur=kiri=kanan=saka=saki=sbka=sbki=analog=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
-            stop = true;
-
-            //s1 = 0;s2 =0; s3 =0; s4 =0;
-
-        //    pc.printf("Stop V1=%.2f V2=%.2f V3=%.2f V4=%.2f\n",V1,V2,V3,V4);
-        }  
-    }  
+        }
+        default :{}  
+    }   //end of switch  
 }
 
-//Begin Encoder
-
-void setCenter()
-{
+void setCenter(){
+/* Fungsi untuk menentukan center dari robot */
     encoderDepan.reset();
     encoderBelakang.reset();
     encoderKanan.reset();
     encoderKiri.reset();
 }
 
-float getX()
-{
+float getX(){
+/* Fungsi untuk mendapatkan jarak X */
     float  jarakEncDpn, jarakEncBlk;
-    jarakEncDpn = (encoderDepan.getPulses())/(float)(2000.0)*K_enc;
-    jarakEncBlk = (encoderBelakang.getPulses())/(float)(2000.0)*K_enc;
+    jarakEncDpn = (encoderDepan.getPulses())/(float)(2000.0)*k_enc;
+    jarakEncBlk = (encoderBelakang.getPulses())/(float)(2000.0)*k_enc;
     return (jarakEncDpn-jarakEncBlk)/2;
 }
 
-float getY()
-{
+float getY(){
+/* Fungsi untuk mendapatkan jarak Y */
     float  jarakEncKir, jarakEncKan;
-    jarakEncKir = (encoderKiri.getPulses())/(float)(2000.0)*K_enc;
-    jarakEncKan = (encoderKanan.getPulses())/(float)(720.0)*K_enc;
+    jarakEncKir = (encoderKiri.getPulses())/(float)(2000.0)*k_enc;
+    jarakEncKan = (encoderKanan.getPulses())/(float)(720.0)*k_enc;
     return (jarakEncKir-jarakEncKan)/2;   
 }
 
-float getTetha()
-{
+float getTetha(){
+/* Fungsi untuk mendapatkan nilai tetha */
     float busurDpn, busurBlk, busurKir, busurKan;
-    busurDpn = ((encoderDepan.getPulses())/(float)(2000.0)*K_enc)/K_robot*360.0;
-    busurBlk = ((encoderBelakang.getPulses())/(float)(2000.0)*K_enc)/K_robot*360.0;
-    busurKir = ((encoderKiri.getPulses())/(float)(2000.0)*K_enc)/K_robot*360.0;
-    busurKan = ((encoderKanan.getPulses())/(float)(720.0)*K_enc)/K_robot*360.0;
+    busurDpn = ((encoderDepan.getPulses())/(float)(2000.0)*k_enc)/k_robot*360.0;
+    busurBlk = ((encoderBelakang.getPulses())/(float)(2000.0)*k_enc)/k_robot*360.0;
+    busurKir = ((encoderKiri.getPulses())/(float)(2000.0)*k_enc)/k_robot*360.0;
+    busurKan = ((encoderKanan.getPulses())/(float)(720.0)*k_enc)/k_robot*360.0;
     
     return -(busurDpn+busurBlk+busurKir+busurKan)/4;    
 }
 
-void gotoXYT(float xa, float ya, float Ta)
-{
+void gotoXYT(float xa, float ya, float Ta){
+/* Fungsi untuk bergerat ke target */
+    errX = xa-getX();
+    Vx = kpX*errX;
+    
+    errY = ya-getY();
+    Vy = kpY*errY;
+    
+    errT = Ta-getTetha();
+    Vt = kp_tetha*errT;
+    
+    v1 = Vx+Vy-Vt;
+    v2 = Vx-Vy-Vt;
+    v3 = -Vx-Vy-Vt;
+    v4 = -Vx+Vy-Vt;
+    
+    if (v1>speed1)
+    { v1 = speed1; }
+    else if (v1<-speed1)
+    { v1 = -speed1; }
+    
+    if (v2>speed2)
+    { v2 = speed2; }
+    else if (v2<-speed2)
+    { v2 = -speed2; }
+    
+    if (v3>speed3)
+    { v3 = speed3; }
+    else if (v3<-speed3)
+    { v3 = -speed3; }
+    
+    if (v4>speed4)
+    { v4 = speed4; }
+    else if (v4<-speed4)
+    { v4 = -speed4; }
+    
+    if (((errX > 0.05) || (errX<-0.05)) || ((errY > 0.05) || (errY<-0.05)) || ((errT > 0.05) || (errT<-0.05))){
+        motor1.speed(v1);
+        motor2.speed(v2);
+        motor3.speed(v3);
+        motor4.speed(v4);    
+    }
+    else{
+        motor1.brake(1);
+        motor2.brake(1);
+        motor3.brake(1);
+        motor4.brake(1);
+    }
+}
 
-        errX = xa-getX();
-        Vx = KpX*errX;
-        
-        errY = ya-getY();
-        Vy = KpY*errY;
-        
-        errT = Ta-getTetha();
-        Vt = Kp_tetha*errT;
-        
-        V1 = Vx+Vy-Vt;
-        V2 = Vx-Vy-Vt;
-        V3 = -Vx-Vy-Vt;
-        V4 = -Vx+Vy-Vt;
-        
-        if (V1>speed1)
-        { V1 = speed1; }
-        else if (V1<-speed1)
-        { V1 = -speed1; }
-        
-        if (V2>speed2)
-        { V2 = speed2; }
-        else if (V2<-speed2)
-        { V2 = -speed2; }
-        
-        if (V3>speed3)
-        { V3 = speed3; }
-        else if (V3<-speed3)
-        { V3 = -speed3; }
-        
-        if (V4>speed4)
-        { V4 = speed4; }
-        else if (V4<-speed4)
-        { V4 = -speed4; }
-        
-        if (((errX > 0.05) || (errX<-0.05)) || ((errY > 0.05) || (errY<-0.05)) || ((errT > 0.05) || (errT<-0.05)))
-        {
-            motor1.speed(V1);
-            motor2.speed(V2);
-            motor3.speed(V3);
-            motor4.speed(V4);    
-      //      pc.printf("V1=%.2f \ ", V1);
-        }
-        else
-        {
-            motor1.brake(1);
-            motor2.brake(1);
-            motor3.brake(1);
-            motor4.brake(1);
-            //_ms(1000);
-        }
-        //pc.printf("%f\t%f\t%f  ", errX*100,errY*100,errT); 
-   //     wait_ms(10);
-
-}
-//End Encoder
-
-void speedLauncher()
-{
+void speedLauncher(){
+/* Fungsi untuk speed launcher */
     if (joystick.R3_click and speedL < 0.8){
-        speedL = speedL + 0.01;}
+        speedL = speedL + 0.01;
+    }
     if (joystick.L3_click and speedL > 0.1){
-        speedL = speedL - 0.01;} 
+        speedL = speedL - 0.01;
+    } 
     if (joystick.R2_click and speedB < 0.8 ){
-            speedB = speedB + 0.01;}  
+        speedB = speedB + 0.01;
+    }  
     if (joystick.L2_click and speedB > 0.1 ){
-        speedB = speedB - 0.01;}    
-    //pc.printf("Pwm depan = %.3f\t  Pwm belakang = %.3f\n", speedL, speedB);
+        speedB = speedB - 0.01;
+    }    
 }
    
-    
-    
-int main (void)
-{
-    // Set baud rate - 115200
+/***********************************************/
+/*              Main Function                  */
+/***********************************************/
+
+int main (void){
+    /* Set baud rate - 115200 */
     joystick.setup();
-    pc.baud(115200);
     wait_ms(1000);
     setCenter();
     wait_ms(500);
     
-    //Posisi Awal
+    /* Posisi Awal */
     XT = 0;
     YT = 0;
     Tetha = 0;
-    pc.printf("Ready...\n");
-    kalibrasi();
-    waktu.start();
+
+    /* Untuk mendapatkan serial dari Arduino */
     while(1)
     {
         // Interrupt Serial
-        joystick.idle();
-       if(joystick.readable() ) {
+        joystick.idle();        
+        
+        if(joystick.readable() ) {
             // Panggil fungsi pembacaan joystik
             joystick.baca_data();
+            
             // Panggil fungsi pengolahan data joystik
             joystick.olah_data();
-            //pc.printf("%3d %3d\n\r",joystick.R2, joystick.L2);
-            //pc.printf("%2x %2x %2x %2x %3d %3d %3d %3d %3d %3d\n\r",joystick.button, joystick.RL, joystick.button_click, joystick.RL_click, joystick.R2, joystick.L2, joystick.RX, joystick.RY, joystick.LX, joystick.LY);
+            
+            // Masuk ke case gerak
             case_ger = case_gerak();
             aktuator();
-            
-            //pc.printf("bacaS = %.2f\tbacaB = %.2f\n",servoS.read(), servoB.read());
-            
-            //kalibrasi
-            if (joystick.START){
-                kalibrasi();}
-             // analog switch mode
-            if (joystick.SELECT_click)      {analog = !analog;}  
-            if (joystick.segitiga_click)    {Launcher = !Launcher;}
-            if (joystick.lingkaran_click){
-                ServoGo = true;
-                waktu.reset();
-                }  
+
+            if (joystick.segitiga_click)    launcher  = !launcher;
+            if (joystick.lingkaran_click)   servoGo = true;
             if (joystick.silang) {
                 XT = 0;
                 YT = 0;
                 Tetha = 0;
-                //pc.printf("x..\n");
-                }
+            }
             speedLauncher();
          
-        } else {
-            joystick.idle();
-            
+        } 
+        else {
+            joystick.idle();    
         }                           
             gotoXYT(XT,YT,Tetha);