File content as of revision 18:1da121ddb7c1:

/****************************************************************************/
/*                  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  */
/*                                                                          */
/****************************************************************************/
/*                                                                          */
/*    Joystick                                                              */
/*  kanan => posisi target x ditambah 0.01                                  */
/*  kiri  => posisi target x dikurang 0.01                                  */
/*  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                              */
/*                                                                          */
/*  Bismillahirahmanirrahim                                                 */
/*  Jagalah Kebersihan kodingan                                             */
/****************************************************************************/


#include "mbed.h"
#include "JoystickPS3.h"
#include "Motor.h"
#include "Servo.h"
#include "encoderKRAI.h"

/***********************************************/
/*          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 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 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

/* 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

/* Deklarasi Servo Launcher */
Servo servoS(PB_2);
Servo servoB(PA_5);

/**
 *  posX dan posY berdasarkan arah robot
 *  encoder Depan & Belakang sejajar sumbu Y
 *  encoder Kanan & Kiri sejajar sumbu X 
 **/

/* 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

/* 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);

/* Variabel Stick */
char case_ger;
bool launcher = false, servoGo = false, manual = true;
int  caseSebelum;

/***********************************************/
/*         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;
    if (!joystick.L1 && joystick.R1) {
        // Pivot Kanan
        casegerak = 1;
    } 
    else if (!joystick.R1 && joystick.L1) {
        // 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;
    } 
    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)) {  
        casegerak = 12; 
    }
    return(casegerak);
}

void aktuator(){
/* Fungsi untuk menggerakkan servo */
    // Servo
    if (servoGo){
        servoS.position(20);
        wait_ms(500);
        servoS.position(-28);
        wait_ms(500);
        servoS.position(20);
        wait_ms(500);
        for (int i = -0; i<=70; i++){
        servoB.position(i);
        wait_ms(10);
        }
        wait_ms(500);
        servoB.position(0);
        servoGo = false;
    }
    else{
        servoS.position(20);
        servoB.position(0);
    }
    
    // Motor Atas
    if (launcher) {
            motorld.speed(speedL);
            motorlb.speed(speedB);
    }else{
            motorld.speed(0);
            motorlb.speed(0);
    }
    
    // MOTOR Bawah
    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): {
            // 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);
            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) : {
            // Kiri
            motor1.speed(VMAX);
            motor2.speed(VMAX);
            motor3.speed(-VMAX);
            motor4.speed(-VMAX);
            break;
            }
        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 (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;
    }
}

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;
    
    return -(busurDpn+busurBlk+busurKir+busurKan)/4;    
}

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);
    }
}

void speedLauncher(){
/* Fungsi untuk speed launcher */
    if (joystick.R3_click and speedL < 0.8){
        speedL = speedL + 0.01;
    }
    if (joystick.L3_click and speedL > 0.1){
        speedL = speedL - 0.01;
    } 
    if (joystick.R2_click and speedB < 0.8 ){
        speedB = speedB + 0.01;
    }  
    if (joystick.L2_click and speedB > 0.1 ){
        speedB = speedB - 0.01;
    }    
}
   
/***********************************************/
/*              Main Function                  */
/***********************************************/

int main (void){
    /* Set baud rate - 115200 */
    joystick.setup();
    wait_ms(1000);
    setCenter();
    wait_ms(500);
    
    /* Posisi Awal */
    XT = 0;
    YT = 0;
    Tetha = 0;

    /* Untuk mendapatkan serial dari Arduino */
    while(1)
    {
        // Interrupt Serial
        joystick.idle();        
        
        if(joystick.readable() ) {
            // Panggil fungsi pembacaan joystik
            joystick.baca_data();
            
            // Panggil fungsi pengolahan data joystik
            joystick.olah_data();
            
            // Masuk ke case gerak
            case_ger = case_gerak();
            aktuator();

            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();    
        }                           
            gotoXYT(XT,YT,Tetha);

    }
}