Diff: main.cpp

Revision:

25:054d3048dd03

Parent:

24:b3e632cc4533

Child:

26:256160a1a82d

--- a/main.cpp	Thu Feb 02 11:12:43 2017 +0000
+++ b/main.cpp	Fri Feb 10 10:29:45 2017 +0000
@@ -46,13 +46,16 @@
 /*          Konstanta dan Variabel             */
 /***********************************************/
 #define PI 3.14159265
-#define D_ENCODER 0.997     // Diameter Roda Encoder
-#define D_ROBOT             // Diameter Roda Robot
+#define D_ENCODER 10     // Diameter Roda Encoder
+#define D_ROBOT 80             // Diameter Roda Robot
 #define VMAX 0.3            // Kiri Kanan
 #define PIVOT 0.4           // Pivka, Pivki
 #define PERPINDAHAN 1       // Perpindahan ke kanan dan kiri
 
+float Vt;
+
 float k_enc     = PI*D_ENCODER;
+float k_robot   = PI*D_ROBOT;
 
 float speedT    = 0.2;
 float speedB    = 0.43;
@@ -63,11 +66,11 @@
 float kpX = 0.5, kpY = 0.5, kp_tetha = 0.03;
 
 /* Deklarasi encoder */
-encoderKRAI encoderKiri(      PC_4, PB_15, 2000, 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 Motor Base */
-Motor motor1(PB_9, PA_12, PC_5);  // pwm, fwd, rev, Motor Depan
-Motor motor2(PB_6, PB_12, PA_7);   // pwm, fwd, rev, Motor Belakang
+Motor motor1(PB_9, PC_5, PA_12);  // pwm, fwd, rev, Motor Depan
+Motor motor2(PB_6, PB_1, PB_12);   // pwm, fwd, rev, Motor Belakang
 
 /* Deklarasi Motor Launcher */
 //Motor motorld(PA_8, PC_1, PC_2);   // pwm, fwd, rev
@@ -91,7 +94,7 @@
 float getTetha();                   // Fungsi mendapatkan jarak Tetha
 
 /* Inisialisasi  Pin TX-RX Joystik dan PC */
-joysticknucleo joystick(PA_9,PA_10);
+joysticknucleo joystick(PA_0,PA_1);
 Serial pc(USBTX,USBRX);
 
 /* Variabel Stick */
@@ -101,6 +104,16 @@
 /****************************************************/
 /*         Deklarasi Fungsi dan Procedure           */
 /****************************************************/
+
+float pid(float Kp, float Ki, float Kd)
+{
+    int errorP;
+    
+    errorP = getTetha();
+    
+    return (float)Kp*errorP;    
+}
+
 int case_gerak(){
 /****************************************************
  **  Gerak Motor Base
@@ -173,26 +186,32 @@
     switch (case_ger) {
         case (1): {       
             // Pivot Kanan
-            motor1.speed(-PIVOT);
-            motor2.speed(-PIVOT);
+            motor1.speed(PIVOT);
+            motor2.speed(PIVOT);
+            setCenter();
             break;
             }
         case (2): {
             // Pivot Kiri
-            motor1.speed(PIVOT);
-            motor2.speed(PIVOT);
+            motor1.speed(-PIVOT);
+            motor2.speed(-PIVOT);
+            setCenter();
             break;
             }
         case (3) : {
             // Kanan
-            motor1.speed(-VMAX-vpid);
-            motor2.speed(0.2+vpid);
+            //motor1.speed(-VMAX-vpid);
+            //motor2.speed(0.2+vpid);
+            motor1.speed(-0.365+pid(0.09,0,0));
+            motor2.speed(0.46+pid(0.09,0,0));
             break;
             }
         case (4) : {
             // Kiri
-            motor1.speed(VMAX-vpid);
-            motor2.speed(-0.2+vpid);
+            //motor1.speed(VMAX-vpid);
+            //motor2.speed(-0.2+vpid);
+            motor1.speed(0.365+pid(0.09,0,0));//belakang
+            motor2.speed(-0.46+pid(0.09,0,0));//depan
             break;
             }
         default : {
@@ -209,30 +228,11 @@
 
 float getTetha(){
 /* Fungsi untuk mendapatkan nilai tetha */
-    float busurKir;
+    float busurKir, tetha;
     busurKir = ((encoderKiri.getPulses())/(float)(2000.0)*k_enc);
-    
-    return -(busurKir);    
-}
-
-void gotoXYT(float Ta){
-/* Fungsi untuk bergerak ke target */
-    float vt;
+    tetha = busurKir/k_robot*360;
     
-    errT = Ta-getTetha();
-    vt = kp_tetha*errT;
-    
-    if (vt>speedT)
-    { vt = speedT; }
-    else if (vt<-speedT)
-    { vt = -speedT; }
-    
-    if (((errT > 0.05) || (errT<-0.05))){ 
-        vpid = vt;    
-    }
-    else{
-        vpid = 0;
-    }
+    return -(tetha);    
 }
 
 void speedKalibrasiMotor(){
@@ -264,15 +264,13 @@
     setCenter();
     wait_ms(500);
     pc.printf("ready....");
-    /* Posisi Awal */
-    Tetha = 0;
 
     /* Untuk mendapatkan serial dari Arduino */
     while(1)
     {
         // Interrupt Serial
         joystick.idle();        
-        
+        //pc.printf("enco : %d \n",encoderKiri.getPulses());
         if(joystick.readable() ) {
             // Panggil fungsi pembacaan joystik
             joystick.baca_data();
@@ -290,6 +288,8 @@
         } 
         else {
             joystick.idle();    
+            //motor1.brake(1);
+            //motor2.brake(1);
         }
     }
 }