Fix pisan inimah plis jangan revisi ultimate mantep

Dependencies:   DigitDisplay Motor PID mbed millis

Fork of DagonFly__RoadToJapan_15Mei by KRAI 2017

Revision:
5:3aa203218306
Parent:
4:483c07cc22e1
Child:
6:68293bed71ea
diff -r 483c07cc22e1 -r 3aa203218306 main.cpp
--- a/main.cpp	Tue Oct 25 13:21:57 2016 +0000
+++ b/main.cpp	Tue Nov 22 15:30:31 2016 +0000
@@ -1,24 +1,23 @@
-/**
-Case Gerak
-1.  cw
-2.  ccw
-3.  Maju
-4.  Mundur
-5.  Serong Atas Kanan
-6.  Serong Bawah Kanan
-7.  Serong Atas Kiri
-8.  Serong Bawah Kiri
-9.  Kanan
-10. Kiri
-11. Analog kiri base
-12. stop
+/****************************************************************************/
+/*                  PROGRAM UNTUK PID CLOSED LOOP                           */
+/*                                                                          */
+/*  - Digunakan encoder autonics                                            */
+/*  - Konfigurasi Motor dan Encoder sbb :                                   */
+/*        _________________                                                 */
+/*        |     DEPAN      |                                                */
+/*        | 1.    e     .2 |    Angka ==> Motor                             */
+/*        | `            ` |    e     ==> Encoder                           */
+/*        | e            e |                                                */
+/*        | .            . |                                                */
+/*        | 4`    e     `3 |                                                */
+/*        |________________|                                                */
+/*                                                                          */
+/*   SETTINGS (WAJIB!) :                                                    */
+/*   1. Settings Pin Encoder, Resolusi, dan Tipe encoding di omniPos.h      */
+/*   2. Deklarasi penggunaan library omniPos pada bagian deklarasi encoder  */
+/*                                                                          */
+/****************************************************************************/
 
-Urutan motor 1234 searah jarum jam
-
-Source Code dari
-Bima Sahbani            EL'12
-Fanny Achmad Hindrarta  EL'12
-**/
 #include "mbed.h"
 #include "JoystickPS3.h"
 #include "Motor.h"
@@ -29,23 +28,80 @@
 #define vmaxanalog 0.9
 #define ax 0.005
 //#define koefperlambatan 0.8
+#include "encoderKRAI.h"
+
+#define pi 22/7
+#define diaEncoder 0.058
+#define PPR 1000
+#define diaRobot 0.64
+
+float K_enc = pi*diaEncoder;
+float K_robot = pi*diaRobot;
+
+float speed1=0.6;
+float speed2=0.6;
+float speed3=0.6;
+float speed4=0.6;
+
+float KpX=0.5, KpY=0.5, Kp_tetha=0.03;
+//float jarakGlobalY;
+
+// Deklarasi variabel PID
+//PID PID(0.992,0.000,0.81,0.001); //(P,I,D, time sampling)
+
+// Deklarasi variabel encoder
+encoderKRAI encoderDepan( D3, D4, 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);
 
 // Deklarasi variabel motor
-Motor motor1(PB_6, PB_4 , PB_5); // pwm, fwd, rev
-Motor motor2(PB_7, PA_4, PC_1); // pwm, fwd, rev
-Motor motor3(PB_8, PC_12, PD_2); // pwm, fwd, rev
-Motor motor4(PB_9, PC_10 , PC_11); // pwm, fwd, rev
+Motor motor1(PB_8, PB_1 , PA_13); // pwm, fwd, rev
+Motor motor2(PB_9, PA_12, PC_5); // pwm, fwd, rev
+Motor motor3(PB_6, PA_7 , PB_12); // pwm, fwd, rev
+Motor motor4(PB_7, PA_14 ,PA_15); // pwm, fwd, rev
+
+// 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;
+
+void detect_encoder(float speed);
+void setCenter();
+float getY();
+float getX();
+float getTetha();
+
+
+
 
 // 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;
 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;
 float jLX,jLY;
-double vcurr=0;
+double vcurr;
 float x,y; // untuk analoghat kiri
 float errorx=0,errory=0;
 
@@ -147,525 +203,269 @@
 
 **/
 
+
 // Pergerakan dari base
 void aktuator()
 {
-    double koef;
-    double s1=0,s2=0,s3=0,s4=0,s1t=0,s2t=0,s3t=0,s4t=0;
     
     // MOTOR
     switch (case_ger) 
     {
 case (1): 
         {       
-            if (pivka) {
-                if(vcurr<0.1) {
-                    vcurr=0.1;
-                } else {
-                    vcurr+=ax;
-                }
-                //perlambatan=0;
-            } else { 
-                //perlambatan=1;
-            } 
-            
-            if (vcurr>=vmaxpivot) {
-                vcurr=vmaxpivot; 
-            }
-            
-            if(joystick.R2==255 && joystick.L2==0) {
-                koef=2;
-            } else if (joystick.L2==255 && joystick.R2==0) {
-                koef=0.5;
-            }
-            else { 
-                koef=1;
-            }
-            
-            s1 = (float)(-0.5*koef*vcurr);
-            s2 = (float)(-0.5*koef*vcurr);
-            s3 = (float)(-0.5*koef*vcurr);
-            s4 = (float)(-0.5*koef*vcurr);    
-            
+           Tetha = Tetha - 0.5;
             pivka=true;         
             maju=mundur=analog=kiri=kanan=saka=saki=sbka=sbki=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
             
-            pc.printf("pivKa\n");
+            pc.printf("pivKa  Xt =%.2f x=%.2f YT=%.2f y=%.2f errx=%.2f erry=%.2f \n",XT,x,YT,y,errX,errY);
             
-            motor1.speed(s1);
-            motor2.speed(s2);
-            motor3.speed(s3);
-            motor4.speed(s4);
-
+         
             break;
         }
     case (2):
            {
-            if (pivki){
-                if(vcurr<0.1) {
-                    vcurr=0.1;
-                } else {
-                    vcurr+=ax;
-                }
-                //perlambatan=0;
-            } else { 
-                //perlambatan=1;
-            } 
-            
-            if (vcurr>=vmaxpivot) {
-                vcurr=vmaxpivot; 
-            }
-            
-            if(joystick.R2==255 && joystick.L2==0) {
-                koef=2;
-            } else if (joystick.L2==255 && joystick.R2==0) {
-                koef=0.5;
-            } else {
-                koef=1;
-            }
-            
-            s1 = (float)(0.5*koef*vcurr);
-            s2 = (float)(0.5*koef*vcurr);
-            s3 = (float)(0.5*koef*vcurr); 
-            s4 = (float)(0.5*koef*vcurr);    
+             Tetha = Tetha + 0.5; 
             
             pivki=true; 
             maju=mundur=kiri=analog=kanan=saka=saki=sbka=sbki=pivka=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
             
-            pc.printf("pivKi\n");
+            pc.printf("pivKi  Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
             
-            motor1.speed(s1);
-            motor2.speed(s2);
-            motor3.speed(s3);
-            motor4.speed(s4);
-    
+         
             break;
            }
     case (3):
         {   
-            if (maju) { 
-                if(vcurr<0.1) {
-                    vcurr=0.1;
-                } else {
-                    vcurr+=ax;
-                }
-                //perlambatan=0;
-            } else {
-                //perlambatan=1;
-            } 
-            
-            if (vcurr>=vmax) { 
-                vcurr=vmax; 
-            }
-            
-            if(joystick.R2==255 && joystick.L2==0) {
-                koef=2;
-            }  else if (joystick.L2==255 && joystick.R2==0)  { koef=0.5;}
-            else {
-                koef=1;  
-            }
-            
-            //Case s1 untuk mode L2 lebih lambat
-            s1 = (float)(-1*koef*(vcurr+0.005));
-            
-            s2 = (float)(1.0*koef*vcurr); 
-            s3 = (float)(1.0*koef*vcurr); 
-            s4 = (float)(-1*koef*vcurr);  
-            
-            //s1 =-0.8*koef*vcurr;
-            //s2 =koef*vcurr; 
-            //s3 =-koef*vcurr; 
-            //s4 =koef*vcurr;    
+           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\n");
+            pc.printf("maju Xt =%.2f x=%.2f YT=%.2f y=%.2f  errx=%.2f erry=%.2f \n",XT,x,YT,y,errX,errY);
 
-            motor1.speed(s1);
-            motor2.speed(s2);
-            motor3.speed(s3);
-            motor4.speed(s4);
-    
             break;
         }
     case (4):
         { 
-            if (mundur) {
-                if(vcurr<0.1) {
-                    vcurr=0.1;
-                } else {
-                    vcurr+=ax;
-                }
-                //perlambatan=0;
-            } else {
-                //perlambatan=1;
-            } 
-            
-            if (vcurr>=vmax) {
-                vcurr=vmax; 
-            }
-            
-            if(joystick.R2==255 && joystick.L2==0) { 
-                koef=2;
-            } else if (joystick.L2==255 && joystick.R2==0) { 
-                koef=0.5;
-            } else { 
-                koef=1;  
-            }
-            //Motor 4  telat mulai
-            s1 = (float)(1*koef*(vcurr-0.008));
-            s2 = (float)(-1*koef*(vcurr-0.005)); 
-            s3 = (float)(-1*koef*(vcurr-0.005)); 
-            s4 = (float)(1*koef*(vcurr+0.005));
+            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\n");
+            pc.printf("mundur Xt =%.2f x=%.2f YT=%.2f y=%.2f errx=%.2f erry=%.2f \n",XT,x,YT,y,errX,errY);
 
-            motor1.speed(s1);
-            motor2.speed(s2);
-            motor3.speed(s3);
-            motor4.speed(s4);
     
             break;
         }
     case (5) :
         {   
-            if (saka) { 
-                if(vcurr<0.1) {
-                    vcurr=0.1;
-                } else {
-                    vcurr+=ax;
-                }
-                //perlambatan=0;
-            } else { 
-                //perlambatan=1;
-            } 
-            
-            if (vcurr>=vmax) {
-                vcurr=vmax; 
-            } if(joystick.R2==255 && joystick.L2==0)  { 
-                koef=2;  
-            } else if (joystick.L2==255 && joystick.R2==0) { 
-                koef=0.5;
-            } else { 
-                koef=1;  
-            }
-            
-            s1 = (float)(-koef*vcurr);
-            s2 = (float)(0);                 //koef*0.1*vcurr;
-            s3 = (float)(koef*vcurr); 
-            s4 = (float)(0);                 //-koef*0.1*vcurr;                
+            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\n");
+            pc.printf("saka Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
             
-            motor1.speed(s1);
-            motor2.brake(1);
-            //motor2.speed(s2);
-            motor3.speed(s3);
-            motor4.brake(1);
-            //motor4.speed(s4);
             
             break;
         }
     case (6) :
         {   
-            if (sbka){
-                if(vcurr<0.1) {
-                    vcurr=0.1;
-                } else {
-                    vcurr+=ax;
-                }
-                //perlambatan=0;
-            } else {
-                //perlambatan=1;
-            } 
-            
-            if (vcurr>=vmaxserong) {
-                vcurr=vmaxserong; 
-            }
-            
-            if(joystick.R2==255 && joystick.L2==0) { 
-                koef=2;  
-            } else if (joystick.L2==255 && joystick.R2==0) { 
-                koef=0.5;
-            } else { 
-                koef=1;  
-            }
-            
-            s1 = (float)(0);                 //koef*0.1*vcurr;
-            s2 = (float)(-koef*vcurr); 
-            s3 = (float)(0);                 //-koef*0.1*vcurr;
-            s4 = (float)(koef*vcurr);
+             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\n");
+            pc.printf("sbka  Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
 
-            //motor1.speed(s1);
-            motor1.brake(1);
-            motor2.speed(s2);
-            //motor3.speed(s3);
-            motor3.brake(1);
-            motor4.speed(s4);
     
             break;
         }
     case (7) :
         {   
-            if (saki) {
-                if(vcurr<0.1) {
-                    vcurr=0.1;
-                } else {
-                    vcurr+=ax;
-                }
-                //perlambatan=0;
-            } else {
-                //perlambatan=1;
-            } 
-            
-            if (vcurr>=vmaxserong) { 
-                vcurr=vmaxserong; 
-            }
-            
-            if(joystick.R2==255 && joystick.L2==0) { 
-                koef=2;  
-            } else if (joystick.L2==255 && joystick.R2==0)  { 
-                koef=0.5;
-            } else { 
-                koef=1;  
-            }
-            
-            s1 = (float)(0);                     //-koef*0.1*vcurr;
-            s2 = (float)(koef*vcurr); 
-            s3 = (float)(0);                     //koef*0.1*vcurr; 
-            s4 = (float)(-koef*vcurr);  
+         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\n");
+            pc.printf("saki Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
             
-            //motor1.speed(s1);
-            motor1.brake(1);
-            motor2.speed(s2);
-            //motor3.speed(s3);
-            motor3.brake(1);
-            motor4.speed(s4);
     
             break;
         }
     case (8) :
         {   
-            if (sbki) {
-                if(vcurr<0.1) {
-                    vcurr=0.1;
-                } else {
-                    vcurr+=ax;
-                }
-                //perlambatan=0;
-            } else {
-                //perlambatan=1;
-            } 
-            
-            if (vcurr>=vmaxserong) { 
-                vcurr=vmaxserong; 
-            }
+           XT = XT - 0.01;
+         YT = YT - 0.01;
+         
+            pc.printf("sbki Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
             
-            if(joystick.R2==255 && joystick.L2==0) { 
-                koef=2;  
-            } else if (joystick.L2==255 && joystick.R2==0)  { 
-                koef=0.5;
-            } else { 
-                koef=1;  
-            }
-            
-            s1 = (float)(koef*vcurr);
-            s2 = (float)(0);                 //-koef*0.1*vcurr;
-            s3 = (float)(-koef*vcurr); 
-            s4 = (float)(0);                 //koef*0.1*vcurr;
-            
-            sbki=true; 
-            maju=kiri=kanan=saka=saki=sbka=mundur=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
-            
-            pc.printf("sbki\n");
-            
-            motor1.speed(s1);
-            //motor2.speed(s2);
-            motor2.brake(1);
-            motor3.speed(s3);
-            //motor4.speed(s4);
-            motor4.brake(1);
     
             break;
         }
     case (9) :
         {   
-            if (kanan) {
-                if(vcurr<0.1) {
-                    vcurr=0.1;
-                } else {
-                    vcurr+=ax;
-                }
-                //perlambatan=0;
-            } else {
-                //perlambatan=1;
-            } 
-            
-            if (vcurr>=vmax) { 
-                vcurr=vmax; 
-            }
-            
-            if(joystick.R2==255 && joystick.L2==0) { 
-                koef=2;  
-            } else if (joystick.L2==255 && joystick.R2==0)  { 
-                koef=0.5;
-            } else { 
-                koef=1;  
-            }
-            
-            s1 =(float)(-1*koef*vcurr);
-            s2 =(float)(-1.0*koef*vcurr); 
-            s3 =(float)(1*koef*vcurr); 
-            s4 =(float)(1.0*koef*vcurr);           
-            
+            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\n");
+            pc.printf("Kanan Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
             
-            motor1.speed(s1);
-            motor2.speed(s2);
-            motor3.speed(s3);
-            motor4.speed(s4);
             break;
         }
     case (10) :
         {   
-            if (kiri) { 
-                if(vcurr<0.1) {
-                    vcurr=0.1;
-                } else {
-                    vcurr+=ax;
-                }
-                //perlambatan=1;
-            } else {
-                //perlambatan=1;
-            } 
-            
-            if (vcurr>=vmax) {
-                vcurr=vmax; 
-            }
-
-            if(joystick.R2==255 && joystick.L2==0)  { 
-                koef=2;  
-            } else if (joystick.L2==255 && joystick.R2==0)  { 
-                koef=0.5;
-            } else { 
-                koef=1;  
-            }
-            
-            s1 =(float)(1*koef*vcurr);
-            s2 =(float)(1*koef*vcurr); 
-            s3 =(float)(-1*koef*vcurr); 
-            s4 =(float)(-1.0*koef*vcurr);
+          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\n");
+            pc.printf("Kiri Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
             
-            motor1.speed(s1);
-            motor2.speed(s2);
-            motor3.speed(s3);
-            motor4.speed(s4);
               
             break;
         }
     case (11): 
         {          
                      
-            if(joystick.R2==255 && joystick.L2==0) {
-                koef=2;
-            } else if (joystick.L2==255 && joystick.R2==0) {
-                koef=0.5;
-            }
-            else { 
-                koef=1;
-            }
-            
-            s1t = (vmaxanalog*(-x+y));
-            s2t = (vmaxanalog*(-x-y));
-            s3t = (vmaxanalog*(x-y));
-            s4t = (vmaxanalog*(x+y));
-            
-            s1 = (float)(0.5*koef*s1t);
-            s2 = (float)(0.5*koef*s2t);
-            s3 = (float)(0.5*koef*s3t);
-            s4 = (float)(0.5*koef*s4t);  
-            
-                                  
+         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 X =%.2f   Y =%.2f \n  ",x,y);
+            pc.printf("analog  Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
             
-       
-                motor1.speed(s1);
-                motor2.speed(s2);
-                motor3.speed(s3);
-                motor4.speed(s4);
             break;
     }
         default :
         {
-            //if (mundur||kiri||kanan||saka||saki||sbka||sbki||pivki||pivka||cw1||ccw1||cw2||ccw2||cw3||ccw3) wait_ms(100);
-            //if (maju && (vcurr>=0.5)) wait_ms(100); 
-            //else if (maju && (vcurr<0.5)) wait_ms(50);  
-            /*      
-            if(s1>0.2 || s1<-0.2 || s2>0.2 || s2<-0.2) {
-                s1 = koefperlambatan * s1;
-                s2 = koefperlambatan * s2;
-                s3 = koefperlambatan * s3;
-                s4 = koefperlambatan * s4;
-
-                motor1.speed(s1);
-                motor2.speed(s2);
-                motor3.speed(s3);
-                motor4.speed(s4);   
-                
-                
-            } else {
-            */
-                motor1.brake(1);
-                motor2.brake(1);
-                motor3.brake(1);
-                motor4.brake(1);
-            //}
-
+          
             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\n");
+        //    pc.printf("Stop V1=%.2f V2=%.2f V3=%.2f V4=%.2f\n",V1,V2,V3,V4);
         }  
-    }
+    }  
+}
+
+//Begin Encoder
+
+void setCenter()
+{
+    encoderDepan.reset();
+    encoderBelakang.reset();
+    encoderKanan.reset();
+    encoderKiri.reset();
+}
+
+float getX()
+{
+    float  jarakEncDpn, jarakEncBlk;
+    jarakEncDpn = (encoderDepan.getPulses())/(float)(2000.0)*K_enc;
+    jarakEncBlk = (encoderBelakang.getPulses())/(float)(2000.0)*K_enc;
+    return (jarakEncDpn-jarakEncBlk)/2;
+}
+
+float getY()
+{
+    float  jarakEncKir, jarakEncKan;
+    jarakEncKir = (encoderKiri.getPulses())/(float)(2000.0)*K_enc;
+    jarakEncKan = (encoderKanan.getPulses())/(float)(720.0)*K_enc;
+    return (jarakEncKir-jarakEncKan)/2;   
+}
+
+float getTetha()
+{
+    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;
+    
+    return -(busurDpn+busurBlk+busurKir+busurKan)/4;    
 }
 
+void gotoXYT(float xa, float ya, float Ta)
+{
+
+        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
+
 
 int main (void)
 {
     // Set baud rate - 115200
     joystick.setup();
     pc.baud(115200);
+        wait_ms(1000);
+    setCenter();
+    wait_ms(500);
+    
+    //Posisi Awal
+    XT = 0;
+    YT = 0;
+    Tetha = 0;
     pc.printf("Ready...\n");
     kalibrasi();
 
@@ -687,9 +487,19 @@
              // analog switch mode
             if (joystick.SELECT_click) {analog=!analog;}  
             //pc.printf(" X =%.2f   Y =%.2f \n  ",x,y);
+            if (joystick.silang) {
+                XT = 0;
+                YT = 0;
+                Tetha = 0;
+                pc.printf("x..\n");
+                }
+              
+       
         } else {
             joystick.idle();
             
         }                           
+            gotoXYT(XT,YT,Tetha);
+
     }
 }