File content as of revision 26:256160a1a82d:

/****************************************************************************/
/*                  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"
#include "millis.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

// Variable Atas
double speed, maxSpeed = 0.8, minSpeed = -0.8;
double kpA=0.6757, kdA=0.6757, kiA=0.00006757;
double p,i,d;
double last_error = 0, current_error, sum_error = 0;
double rpm, target_rpm = 9.0;
 
// Variable Bawah
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;


/* Deklarasi Encoder Base */
encoderKRAI encoderKiri(PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING);   //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan

/* Deklarasi Encoder Launcher */
encoderKRAI encoderAtas( PB_13, PB_14, 14, encoderKRAI::X2_ENCODING);

/* 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 motor3(PA_8,PC_1,PC_2);    // pwm ,fwd, rev, Motor Atas
//Motor motorld(PA_8, PC_1, PC_2);   // pwm, fwd, rev
//Motor motorlb(PA_0, PA_4, PC_15 ); // pwm, fwd, rev

/* Deklarasi Penumatik Launcher */
DigitalOut pneumatik(PB_2, 0);

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

/* Deklarasi Variable Milis */
unsigned long int previousMillis = 0;
unsigned long int currentMillis;

/* Variabel Stick */
char case_ger;
bool launcher = false;
//bool pneumatikGo = false;

/****************************************************/
/*         Deklarasi Fungsi dan Procedure           */
/****************************************************/
void init_speed();
void speedKalibrasiMotor();
float pidBase(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 Penumatik */
    // Pneumatik
/*    if (pneumatikGo){
        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) {
        startMillis();
        currentMillis = millis();
        if (currentMillis-previousMillis>=10)
        {
            rpm = (double)encoderAtas.getPulses();    
            current_error = target_rpm - rpm;
            sum_error = sum_error + current_error;
            p = current_error*kpA;
            d = (current_error-last_error)*kdA/10.0;
            i = sum_error*kiA*10.0;
            speed = p + d + i;
            init_speed();
            motor3.speed(speed);
            last_error = current_error;
            encoderAtas.reset();
          //pc.printf("%.04lf\n",rpm);
            previousMillis = currentMillis;
        }
        else
        {
            motor3.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+pidBase(0.09,0,0));
            motor2.speed(0.46+pidBase(0.09,0,0));
            break;
            }
        case (4) : {
            // Kiri
            //motor1.speed(VMAX-vpid);
            //motor2.speed(-0.2+vpid);
            motor1.speed(0.365+pidBase(0.09,0,0));//belakang
            motor2.speed(-0.46+pidBase(0.09,0,0));//depan
            break;
            }
        default : {
            motor1.brake(1);
            motor2.brake(1);
            break;
            }   
    } // 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);    
}

void init_speed (){
    if (speed>maxSpeed){
        speed = maxSpeed;
    }
    
    if (speed<minSpeed){
        speed = minSpeed;
    }
}  
/*********************************************************/
/*                  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){
                pneumatik = 1;
                wait_ms(500);
                pneumatik = 0;
            }
            speedKalibrasiMotor();  
        }
        else {
            joystick.idle();    
            motor1.brake(1);
            motor2.brake(1);
        }
    }
}