File content as of revision 25:054d3048dd03:

/****************************************************************************/
/*                  PROGRAM UNTUK PID CLOSED LOOP                           */
/*                                                                          */
/*  - Digunakan encoder autonics                                            */
/*  - Konfigurasi Motor dan Encoder sbb :                                   */
/*                      ______________________                              */
/*                     /                      \    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  */
/*                                                                          */
/****************************************************************************/
/*                                                                          */
/*  Joystick                                                                */
/*  Kanan => Posisi target x ditambah 'perpindahan'                         */
/*  Kiri  => Posisi target x dikurang 'perpindahan'                         */
/*                                                                          */
/*  Tombol silang   => Kembali keposisi Awal                                */
/*  Tombol segitiga => Aktif motor Launcher                                 */
/*  Tombol lingkaran=> Aktif servo Launcher                                 */
/*  Tombol L1       => Pivot Kiri                                           */
/*  Tombol R1       => Pivot Kanan                                          */
/*  Tombol L3       => PWM Motor Belakang Dikurangi                         */
/*  Tombol R3       => PWM Motor Belakang Ditambah                          */
/*  Tombol L2       => PWM Motor Depan Dikurangi                            */
/*  Tombol R2       => PWM Motor Depan Ditambah                             */
/*                                                                          */
/*  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 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;
float speedL    = 0.4;

float vpid = 0;

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

/* Deklarasi Motor Base */
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
//Motor motorlb(PA_0, 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 errT, Tetha;                  // Variabel yang didapatkan encoder

/* Fungsi dan Procedur Encoder */
void setCenter();                   // Fungsi reset agar robot di tengah
float getTetha();                   // Fungsi mendapatkan jarak Tetha

/* Inisialisasi  Pin TX-RX Joystik dan PC */
joysticknucleo joystick(PA_0,PA_1);
Serial pc(USBTX,USBRX);

/* Variabel Stick */
char case_ger;
bool launcher = false, servoGo = false;

/****************************************************/
/*         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
 **  Case 1  : Pivot kanan
 **  Case 2  : Pivot Kiri
 **  Case 3  : Kanan
 **  Case 4  : Kiri
 **  Case 5  : 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))  {   
        // Kanan
        casegerak = 3;
        pc.printf("kanan");
    } 
    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {   
        // Kiri
        casegerak = 4;
        pc.printf("kiri");       
    } 
    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {  
        // Break
        casegerak = 5; 
    }
    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
    switch (case_ger) {
        case (1): {       
            // Pivot Kanan
            motor1.speed(PIVOT);
            motor2.speed(PIVOT);
            setCenter();
            break;
            }
        case (2): {
            // Pivot Kiri
            motor1.speed(-PIVOT);
            motor2.speed(-PIVOT);
            setCenter();
            break;
            }
        case (3) : {
            // Kanan
            //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(0.365+pid(0.09,0,0));//belakang
            motor2.speed(-0.46+pid(0.09,0,0));//depan
            break;
            }
        default : {
            motor1.brake(1);
            motor2.brake(1);
            }   
    } // End Switch 
}

void setCenter(){
/* Fungsi untuk menentukan center dari robot */
    encoderKiri.reset();
}

float getTetha(){
/* Fungsi untuk mendapatkan nilai tetha */
    float busurKir, tetha;
    busurKir = ((encoderKiri.getPulses())/(float)(2000.0)*k_enc);
    tetha = busurKir/k_robot*360;
    
    return -(tetha);    
}

void speedKalibrasiMotor(){
/* Fungsi untuk speed launcher */
    if (joystick.R3_click and speedL < 0.8){
        speedL = speedL + 0.01; // PWM++ Motor Belakang
    }
    if (joystick.L3_click and speedL > 0.1){
        speedL = speedL - 0.01; // PWM-- Motor Belakang
    } 
    if (joystick.R2_click and speedB < 0.8 ){
        speedB = speedB + 0.01; // PWM++ Motor Depan
    }  
    if (joystick.L2_click and speedB > 0.1 ){
        speedB = speedB - 0.01; // PWM-- Motor Depan
    }
    // pc.printf("Pwm depan = %.3f\t  Pwm belakang = %.3f\n", speedL, speedB);    
}
   
/*********************************************************/
/*                  Main Function                        */
/*********************************************************/

int main (void){
    /* Set baud rate - 115200 */
    joystick.setup();
    pc.baud(115200);
    wait_ms(1000);
    setCenter();
    wait_ms(500);
    pc.printf("ready....");

    /* 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();
            
            // 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;
            speedKalibrasiMotor();
        } 
        else {
            joystick.idle();    
            //motor1.brake(1);
            //motor2.brake(1);
        }
    }
}