Diff: main.cpp

Revision:

22:4632f58461e0

Parent:

21:da2f3d04468f

Child:

23:023b522977b2

--- a/main.cpp	Sat Jan 28 07:24:07 2017 +0000
+++ b/main.cpp	Sat Jan 28 10:19:36 2017 +0000
@@ -3,15 +3,15 @@
 /*                                                                          */
 /*  - Digunakan encoder autonics                                            */
 /*  - Konfigurasi Motor dan Encoder sbb :                                   */
-/*        _________________                                                 */
-/*        |     DEPAN      |                                                */
-/*        | 1.    e     .2 |    Angka ==> Motor                             */
-/*        | `            ` |    e     ==> Encoder                           */
-/*        | e            e |                                                */
-/*        | .            . |                                                */
-/*        | 4`    e     `3 |                                                */
-/*        |________________|                                                */
-/*                                                                          */
+/*                      ______________________                              */
+/*                     /                      \    Rode Depan Belakang:     */
+/*                    /      2 (Belakang)      \   Omniwheel                */
+/*                   |                          |                           */
+/*                   | 3 (Encoder)            4 |  Roda Kiri Kanan:         */
+/*                   |                          |  Fixed Wheel              */
+/*                    \       1 (Depan)        /                            */
+/*                     \______________________/    Putaran berlawanan arah  */
+/*                                                 jarum jam positif        */
 /*   SETTINGS (WAJIB!) :                                                    */
 /*   1. Settings Pin Encoder, Resolusi, dan Tipe encoding di omniPos.h      */
 /*   2. Deklarasi penggunaan library omniPos pada bagian deklarasi encoder  */
@@ -21,14 +21,14 @@
 /*  Joystick                                                                */
 /*  Kanan => Posisi target x ditambah 'perpindahan'                         */
 /*  Kiri  => Posisi target x dikurang 'perpindahan'                         */
-/*  Atas  => Posisi target y ditambah 0.01                                  */
-/*  Bawah => Posisi target y dikurang 0.01                                  */
 /*                                                                          */
 /*  Tombol silang   => Kembali keposisi Awal                                */
 /*  Tombol segitiga => Aktif motor Launcher                                 */
 /*  Tombol lingkaran=> Aktif servo Launcher                                 */
-/*  Tombol L3       => PWM Launcher dikurangin                              */
-/*  Tombol R3       => PWM Launcher ditambahin                              */
+/*  Tombol L1       => Pivot Kiri                                           */
+/*  Tombol R1       => Pivot Kanan                                          */
+/*  Tombol L3       => PWM Launcher Belakang dikurangin                     */
+/*  Tombol R3       => PWM Launcher Belakang ditambahin                     */
 /*                                                                          */
 /*  Bismillahirahmanirrahim                                                 */
 /*  Jagalah Kebersihan Kodingan                                             */
@@ -44,36 +44,28 @@
 /*          Konstanta dan Variabel             */
 /***********************************************/
 #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 D_ENCODER 0.997     // Diameter Roda Encoder
+#define D_ROBOT             // Diameter Roda Robot
+#define VMAX 0.3            // Kiri Kanan
 #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;
 
-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 speedT    = 0.2;
+float speedB    = 0.43;
+float speedL    = 0.4;
 
-float kpX=0.5, kpY=0.5, kp_tetha=0.03;
+float vpid = 0;
+
+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 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
+Motor motor1(PB_7, PA_14, PA_15);  // pwm, fwd, rev, Motor Depan
+Motor motor2(PB_8, PA_13, PB_0);   // pwm, fwd, rev, Motor Belakang
 
 /* Deklarasi Motor Launcher */
 Motor motorld(PA_8, PC_1, PC_2);   // pwm, fwd, rev
@@ -90,15 +82,10 @@
  **/
 
 /* 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
+float errT, Tetha;                  // Variabel yang didapatkan encoder
 
 /* 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 */
@@ -106,27 +93,21 @@
 
 /* Variabel Stick */
 char case_ger;
-bool launcher = false, servoGo = false, manual = true;
-int  caseSebelum;
+bool launcher = false, servoGo = false;
 
-/***********************************************/
-/*         Deklarasi Fungsi dan Procedure      */
-/***********************************************/
+/****************************************************/
+/*         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
+ **  Case 3  : Kanan
+ **  Case 4  : Kiri
+ **  Case 5  : Break
  ****************************************************/ 
+    
     int casegerak;
     if (!joystick.L1 && joystick.R1) {
         // Pivot Kanan
@@ -136,40 +117,17 @@
         // Pivot Kiri
         casegerak = 2;
     } 
-    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))  {  
-        // Mundur
-        casegerak = 4;
-    } 
-    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)) {   
-        // Serong Bawah Kanan
-        casegerak = 6;
-    } 
-    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)) {   
-        // Serong Bawah Kiri
-        casegerak = 8;
-    } 
     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri))  {   
         // Kanan
-        casegerak = 9;
+        casegerak = 3;
     } 
     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {   
         // Kiri
-        casegerak = 10;       
+        casegerak = 4;       
     } 
     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {  
-        casegerak = 12; 
+        // Break
+        casegerak = 5; 
     }
     return(casegerak);
 }
@@ -207,257 +165,90 @@
     }
     
     // MOTOR Bawah
-    if (manual) { 
-        // Mode Manual
-        switch (case_ger) {
+    switch (case_ger) {
         case (1): {       
             // Pivot Kanan
             motor1.speed(-PIVOT);
             motor2.speed(-PIVOT);
-            motor3.speed(-PIVOT);
-            motor4.speed(-PIVOT);
             break;
             }
         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 (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 (6) : {   
-            // Samping Bawah Kanan
-            motor1.brake(1);
-            motor2.speed(-SAMPING);
-            motor3.brake(1);
-            motor4.speed(SAMPING);
-            break;
-        }
-        case (7) : {   
-            // Samping Atas Kiri
-            motor1.brake(1);
-            motor2.speed(SAMPING);
-            motor3.brake(1);
-            motor4.speed(-SAMPING);
+            }
+        case (3) : {
+            // Kanan
+            motor1.speed(-VMAX-vpid);
+            motor2.speed(VMAX+vpid);
             break;
-        }
-         case (8) : {   
-            // Samping Bawah Kiri
-            motor1.speed(SAMPING);
-            motor2.brake(1);
-            motor3.speed(-SAMPING);
-            motor4.brake(1);
-            break;
-        }
-        case (9) : {
-            // Kanan
-            motor1.speed(-VMAX);
-            motor2.speed(-VMAX);
-            motor3.speed(VMAX);
-            motor4.speed(VMAX);
-            break;
-        }
-        case (10) : {
+            }
+        case (4) : {
             // Kiri
-            motor1.speed(VMAX);
-            motor2.speed(VMAX);
-            motor3.speed(-VMAX);
-            motor4.speed(-VMAX);
+            motor1.speed(VMAX-vpid);
+            motor2.speed(-VMAX+vpid);
             break;
             }
         default : {
-                motor1.brake(1);
-                motor2.brake(1);
-                motor3.brake(1);
-                motor4.brake(1);
+            motor1.brake(1);
+            motor2.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 (case_ger != caseSebelum) 
-                    XT = XT + PERPINDAHAN;
-            break;
-            }
-            case (10) :{  
-            // Kiri 
-                if (case_ger != caseSebelum) 
-                    XT = XT - PERPINDAHAN;
-            break;
-            }
-            default :{}  
-        }   // End of Switch
-        caseSebelum = case_ger;
-    }
+    } // End Switch 
 }
 
 void setCenter(){
 /* Fungsi untuk menentukan center dari robot */
-    encoderDepan.reset();
-    encoderBelakang.reset();
-    encoderKanan.reset();
     encoderKiri.reset();
 }
 
-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;
-    return (jarakEncDpn-jarakEncBlk)/2;
-}
-
-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;
-    return (jarakEncKir-jarakEncKan)/2;   
-}
-
 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;
+    float busurKir;
+    busurKir = ((encoderKiri.getPulses())/(float)(2000.0)*k_enc);
     
-    return -(busurDpn+busurBlk+busurKir+busurKan)/4;    
+    return -(busurKir);    
 }
 
-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;
+void gotoXYT(float Ta){
+/* Fungsi untuk bergerak ke target */
+    float vt;
     
     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 (vt>speed1)
+    { vt = speed1; }
+    else if (vt<-speed1)
+    { vt = -speed1; }
     
-    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);    
+    if ((((errT > 0.05) || (errT<-0.05))){ 
+        vpid = vt;    
     }
     else{
-        motor1.brake(1);
-        motor2.brake(1);
-        motor3.brake(1);
-        motor4.brake(1);
+        vpid = 0;
     }
 }
 
 void speedLauncher(){
 /* Fungsi untuk speed launcher */
     if (joystick.R3_click and speedL < 0.8){
-        speedL = speedL + 0.01;
+        speedL = speedL + 0.01; // PWM++ Motor Belakang
     }
     if (joystick.L3_click and speedL > 0.1){
-        speedL = speedL - 0.01;
+        speedL = speedL - 0.01; // PWM-- Motor Belakang
     } 
     if (joystick.R2_click and speedB < 0.8 ){
-        speedB = speedB + 0.01;
+        speedB = speedB + 0.01; // PWM++ Motor Depan
     }  
     if (joystick.L2_click and speedB > 0.1 ){
-        speedB = speedB - 0.01;
+        speedB = speedB - 0.01; // PWM-- Motor Depan
     }    
 }
    
-/***********************************************/
-/*              Main Function                  */
-/***********************************************/
+/*********************************************************/
+/*                  Main Function                        */
+/*********************************************************/
 
 int main (void){
     /* Set baud rate - 115200 */
@@ -467,8 +258,6 @@
     wait_ms(500);
     
     /* Posisi Awal */
-    XT = 0;
-    YT = 0;
     Tetha = 0;
 
     /* Untuk mendapatkan serial dari Arduino */
@@ -490,18 +279,10 @@
 
             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;
-                Tetha = 0;
-            }
             speedLauncher();
         } 
         else {
             joystick.idle();    
         }
-        if (!manual)                            
-            gotoXYT(XT,YT,Tetha);
     }
 }