File content as of revision 27:68efb1622985:

/****************************************************************************/
/*                  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,speed2, maxSpeed = 0.8, minSpeed = 0;
double kpA=0.6757, kdA=0.6757, kiA=0.00006757;
double p,i,d;
double p2,i2,d2;
double last_error = 0, current_error, sum_error = 0;
double last_error2 = 0, current_error2, sum_error2 = 0;
float rpm, target_rpm = 2.0;
float rpm2, target_rpm2 = 4.0;
 
// Variable Bawah
float Vt;
float k_enc     = PI*D_ENCODER;
float k_robot   = PI*D_ROBOT;
float speedT    = 0.2;
float vpid      = 0;
float pwmP      = 0.5;
float pwmT      = -0.8;


/* 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);
encoderKRAI encoderAtas2( PB_15, PC_4, 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 motorL1(PA_8,PC_1,PC_2);    // pwm ,fwd, rev, Motor Launcher1
Motor motorL3(PA_10, PC_0, PC_3); // pwm, fwd, rev, Motor Launcher2 
Motor motorP(PC_7, PC_14, PC_13); // pwm, fwd, rev, Motor Powerscrew                

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

/*Dekalrasi Limit Switch */
DigitalIn limitA (PC_9, PullUp);
DigitalIn limitT(PB_10, PullUp);
DigitalIn limitB (PC_8, PullUp);

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

/**
 *  posX dan posY berdasarkan arah robot
 *  encoder Depan & Belakang sejajar sumbu Y
 *  encoder Kanan & Kirim 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;
unsigned long int previousMillis2 = 0;
unsigned long int currentMillis2;

/* Variabel Stick */
char case_ger;
bool launcher = false;
bool reload = 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 Motor PowerScrew */
    // PowerScrew
    if (reload){
        if (!limitA){
            motorP.brake (1);
        }
        else
        {
            motorP.speed(pwmP);
            if (!limitT)
            {
                while (limitB) 
                {
                    motorP.speed(pwmT);
                }
                motorP.brake(1);
            }
        }
        reload = !reload;
    }
/*Motor Atas*/
    if (launcher) {
        startMillis();
        currentMillis = millis();
        /*Launcher Depan*/
        if (currentMillis-previousMillis>=10)
        {
            rpm = (float)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();
            motorL1.speed(speed);
            last_error = current_error;
            encoderAtas.reset();
          //pc.printf("%.04lf\n",rpm);
            previousMillis = currentMillis;
        }
        else
        {
            encoderAtas.getPulses();
        }      
        if (currentMillis2-previousMillis2>=10)
        {
            rpm2 = (float)encoderAtas2.getPulses();    
            current_error2 = target_rpm2 - rpm2;
            sum_error2 = sum_error2 + current_error2;
            p2 = current_error2*kpA;
            d2 = (current_error2-last_error2)*kdA/10.0;
            i2 = sum_error2*kiA*10.0;
            speed2 = p2 + d2 + i2;
            init_speed();
            motorL3.speed(speed2);
            last_error2 = current_error2;
            encoderAtas2.reset();
          //pc.printf("%.04lf\n",rpm2);
            previousMillis2 = currentMillis2;
        }
        else
        {
            encoderAtas2.getPulses();
        }
    }
    // 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);
            }   
    } // 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 target_rpm < 14){
        target_rpm = target_rpm + 1; // RPM++ Motor Belakang
    }
    if (joystick.L3_click and target_rpm > 1){
        target_rpm = target_rpm - 1; // RPM-- Motor Belakang
    } 
    if (joystick.R2_click and target_rpm2 < 14){
        target_rpm2 = target_rpm2 + 1; // RPM++ Motor Depan
    }  
    if (joystick.L2_click and target_rpm2 > 1 ){
        target_rpm2 = target_rpm2 - 1; // RPM-- Motor Depan
    }
    // pc.printf("Rpm depan = %.4f\t  Rpm belakang = %.4f\n", target_rpm, target_rpm2);    
}

void init_speed (){
    if (speed>maxSpeed){
        speed = maxSpeed;
    }
    
    if (speed<minSpeed){
        speed = minSpeed;
    }
    if (speed2>maxSpeed){
        speed2 = maxSpeed;
    }
    
    if (speed2<minSpeed){
        speed2 = 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();
            if (joystick.silang_click) reload = !reload;  
        }
        else {
            joystick.idle();    
            motor1.brake(1);
            motor2.brake(1);
        }
    }
}