File content as of revision 48:3006386dc8df:

/*tuning motor baru untuk konstanta pid baru                                */
/****************************************************************************/
/*                  PROGRAM UNTUK PID CLOSED LOOP                           */
/*                                                                          */
/*                  Last Update : 16 April 2017                             */
/*                                                                          */
/*  - Digunakan encoder autonics                                            */
/*  - Konfigurasi Motor dan Encoder sbb :                                   */
/*                      ______________________                              */
/*                     /                      \    Rode Depan Belakang:     */
/*                    /      2 (Belakang)      \   Omniwheel                */
/*                   |                          |                           */
/*                   | 3 (kiri)       4 (kanan) |  Roda Kiri Kanan:         */
/*                   |                          |  Omniwheel                */
/*                    \       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 =>                                                                */
/*  Kiri  =>                                                                */
/*                                                                          */
/*  Tombol silang    => Pneumatik aktif                                     */
/*  Tombol segitiga  => Aktif motor Launcher                                */
/*  Tombol lingkaran => Reloader naik                                       */
/*  Tombol kotak     => Reloader turun                                      */
/*  Tombol L1        => Pivot Kiri                                          */
/*  Tombol R1        => Pivot Kanan                                         */
/*  Tombol L2        => Kurang PWM Motor Launcher                           */
/*  Tombol R2        => Tambah PWM Motor Launcher                           */
/*                                                                          */
/*  Bismillahirahmanirrahim                                                 */
/*  Jagalah Kebersihan Kodingan                                             */
/****************************************************************************/

#include "mbed.h"
#include "JoystickPS3.h"
#include "Motor.h"
#include "encoderKRAI.h"
#include "millis.h"
#include "Ping.h"
#include "DigitDisplay.h"

/***********************************************/
/*          Konstanta dan Variabel             */
/***********************************************/
#define PI 3.14159265
#define D_ENCODER 10        // Diameter Roda Encoder
#define D_ROBOT 80          // Diameter Roda Robot

// Variable Atas 
// indek 2 untuk motor depan, 1 untuk motor belakang
double speed, speed2;
const double maxSpeed = 0.95, minSpeed = -0.95, Ts = 12.5;
const double kpA1=0.1478, kdA1=0.9295, kiA1=0.0004226;
const double kpA2=0.13978, kdA2=0.9222, kiA2=0.00038636;
double a1,b1,c1;
double a2,b2,c2;
double current_error1, previous_error1_1 = 0, previous_error1_2 = 0;
double current_error2, previous_error2_1 = 0, previous_error2_2 = 0;
double previous_speed1 = 0;
double previous_speed2 = 0;

float rpm, rpm2;
double target_rpm = 18.0, target_rpm2 = 18.0; // selisih 4 bagus, sama bagus
const float maxRPM = 35, minRPM = 0; // Limit 25 atau 27

const float pwmPowerUp        = 1.0;
const float pwmPowerDown      = -1.0;

double jarak_ping=0;
double ping_target = 16;
double ping_current_error, ping_previous_error1 = 0, ping_sum_error=0;
double ping_Kp = -0.3747, ping_Ki =0, ping_Ts=10;
double ping_pwm, ping_previous_pwm = 0; 

// Variable Bawah
float PIVOT             = 0.17;      // PWM Pivot Kanan, Pivot Kiri
float tuneDpn           = 1.0;     // Tunning PWM motor Depan
float tuneBlk           = 1.0;     // Tunning PWM motor belakang
float tuneAksel         = 0.6;
int aksel               = 0;
float tuneAkselBlk      = 0.4;
float tuneR             = 1.0;
float tuneL             = 1.0;
float serong            = 0.4;
float rasio             = 1.4545;
float t_new             = 0.1;

/* variable tunning */
float tunning_L;
float tunning_R;
float tunning_Dpn;
float tunning_Blk;

/* Deklarasi Variable Millis */
unsigned long int previousMillis = 0;   // PID launcher
unsigned long int currentMillis;
unsigned long int previousMillis2 = 0;  // PID launcher
unsigned long int currentMillis2;
unsigned long int previousMillis3 = 0;  // Pneumatik
unsigned long int previousMillis4 = 0;  // Ping
unsigned long int previousMillis5 = 0;  // Display

/* Variabel Stick */
//Logic untuk masuk aktuator
int case_joy;
bool isLauncher = false;
bool isReload = false;
bool ReloadOn = false;
bool flag_Pneu = false;
bool flag_paku = false;
bool ready = false;

/*****************************************************/
/*   Definisi Prosedur, Fungsi dan Setting Pinout    */
/*****************************************************/

/* Fungsi dan Procedur Encoder */
void init_speed();                  // 
void aktuator();                    // Pergerakan aktuator berdasarkan case joystick
int case_joystick();                // Mendapatkan case dari joystick
//void speedKalibrasiMotor();       // Pertambahan target RPM motor atas melalui joystick

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

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

/* Deklarasi Encoder Launcher */
encoderKRAI encLauncherDpn( PC_10, PC_11, 28, encoderKRAI::X4_ENCODING);
encoderKRAI encLauncherBlk( PC_12, PD_2, 28, encoderKRAI::X4_ENCODING);

/* Deklarasi Motor Base */
Motor motorDpn(PB_7, PC_3, PC_0); //(PB_9, PA_12, PC_5);
//Motor motorBlk(PB_6, PC_14, PC_13);  //(PB_6, PB_1, PB_12); (PC_7, PC_14, PC_13); 
Motor motorBlk(PB_2, PB_15, PB_1);
Motor motorL  (PB_9, PA_12, PA_6);  
Motor motorR  (PB_8, PC_6, PC_5);   //(PC_6, PB_4, PB_5);

/* Deklarasi Motor Launcher */
Motor launcherDpn(PA_5,PA_11,PB_12);    // pwm ,fwd, rev
Motor launcherBlk(PB_3, PC_4, PA_10); // pwm, fwd, rev 
Motor powerScrew(PB_10, PB_14, PB_13); // pwm, fwd, rev                

/* Deklarasi Penumatik Launcher */
DigitalOut pneumatik(PA_4, PullUp);
DigitalOut pneu_paku(PC_2, PullUp);

/*Dekalrasi Limit Switch */
//DigitalIn infraAtas(PC_9, PullUp);
DigitalIn limitTengah(PA_9, PullUp);
DigitalIn limitBawah(PC_7, PullUp);
DigitalIn limitBawah1(PA_7, PullUp);

/*deklarasi PING ultrasonic*/
Ping pingAtas(PC_15);

/*Deklarasi Display*/
DigitDisplay display (PA_8, PC_8);

/****************************************************/
/*         Deklarasi Fungsi dan Procedure           */
/****************************************************/
int case_joystick()
{
//---------------------------------------------------//
//  Gerak Motor Base                                 //
//   Case 1  : Pivot kanan                           //
//   Case 2  : Pivot Kiri                            //
//   Case 3  : Kanan                                 //
//   Case 4  : Kiri                                  //
//   Case 5  : Break                                 //
//---------------------------------------------------//
    
    int caseJoystick;
    if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
        // Pivot Kanan
        caseJoystick = 1;
    } 
    else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
        // Pivot Kiri
        caseJoystick = 2;
    }
    else if ((joystick.START_click)&&(!joystick.SELECT_click)&&(!joystick.R3_click)) {   
        // tambah rpm dengan nilai tertentu
        caseJoystick = 31;     
    }
    else if ((!joystick.START_click)&&(joystick.SELECT_click)&&(!joystick.R3_click)) {   
        // kurangi rpm dengan nilai tertentu
        caseJoystick = 32;     
    }
    else if ((!joystick.START_click)&&(!joystick.SELECT_click)&&(joystick.R3_click)) {   
        // kurangi rpm dengan nilai tertentu
        caseJoystick = 33;     
    }
    else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri))  {  
        // Kanan + Rotasi kanan
        caseJoystick = 17;
    } 
    else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri))  {  
        // Kanan + Rotasi kiri
        caseJoystick = 18;
    }
    else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri))  {  
        // Kiri + Rotasi kanan
        caseJoystick = 19;
    } 
    else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri))  {  
        // Kanan + Rotasi kiri
        caseJoystick = 20;
    }
    else if ((joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
        // Maju + Rotasi kanan
        caseJoystick = 21;
    } 
    else if ((!joystick.R1)&&(joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
        // Maju + Rotasi kiri
        caseJoystick = 22;
    }
    else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
        // Mundur + Rotasi kanan
        caseJoystick = 23;
    } 
    else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
        // Mundur + Rotasi kiri
        caseJoystick = 24;
    }
    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.segitiga_click))  {  
        // Kanan + segitiga
        caseJoystick = 25;
    }
    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.segitiga_click))  {  
        // Kiri + segitiga
        caseJoystick = 26;
    }
    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.lingkaran_click))  {  
        // Kanan + lingkaran
        caseJoystick = 27;
    }
    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.lingkaran_click))  {  
        // Kiri + lingkaran
        caseJoystick = 28;
    }
    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.kotak_click))  {  
        // Kanan + kotak
        caseJoystick = 29;
    }
    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.kotak_click))  {  
        // Kiri + kotak
        caseJoystick = 30;
    }
    else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {   
        // Serong kanan maju
        caseJoystick = 13;     
    }
    else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {   
        // Serong kiri maju
        caseJoystick = 14;      
    }
    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {   
        // Serong kanan mundur
        caseJoystick = 15;       
    }
    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {   
        // Serong kiri mundur
        caseJoystick = 16;       
    } 
    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri))  {   
        // Kanan
        caseJoystick = 3;
    } 
    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {   
        // Kiri
        caseJoystick = 4;
    }
    else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {   
        // Atas -- Maju
        caseJoystick = 8;       
    }
    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {   
        // Bawah -- Mundur
        caseJoystick = 9;       
    } 
    else if (joystick.segitiga_click){
        // Motor Launcher
        caseJoystick = 5;
    }
    else if (joystick.R2_click){
        // Target Pulse PID ++ Motor Depan
         caseJoystick = 6;
    }  
    else if (joystick.L2_click){
        // Target Pulse PID -- Motor 
         caseJoystick = 7;
    }
    else if (joystick.silang_click){
        // Pnemuatik ON
        caseJoystick = 10;
    }
    else if ((joystick.lingkaran_click)&&(!joystick.kotak_click))  {   
        // Power Screw Up
        caseJoystick = 11;
    } 
    else if ((joystick.kotak_click)&&(!joystick.lingkaran_click)) {   
        // Power Screw Down
        caseJoystick = 12;       
    } 
    else if (joystick.L3){
        // Paku Bumi ON/OFF
        caseJoystick = 34;
    }
    else
    {
        tuneAksel = 0.6;
        aksel = 0;
    }
    
    return(caseJoystick);
}


void init_rpm (){
    if (target_rpm>maxRPM-2){
        target_rpm = maxRPM-2;
    }
    if (target_rpm<minRPM){
        target_rpm = minRPM;
    }
    if (target_rpm2>maxRPM){
        target_rpm2 = maxRPM;
    }
    if (target_rpm2<minRPM+2){
        target_rpm2 = minRPM+2;
    }
}

void aktuator()
{
    switch (case_joy) {
        case (1): 
        {       
            // Pivot Kanan
            motorDpn.speed(-PIVOT);
            motorBlk.speed(-PIVOT);
            motorR.speed(-rasio*PIVOT-t_new);
            motorL.speed(-rasio*PIVOT-t_new);
            break;
        }
        case (2): 
        {
            // Pivot Kiri
            motorDpn.speed(PIVOT);
            motorBlk.speed(PIVOT);
            motorR.speed(rasio*PIVOT+t_new);
            motorL.speed(rasio*PIVOT+t_new);
            break;
        }
        case (17): 
        {       
            // Kanan + Rotasi Kanan
            motorDpn.speed(-PIVOT);
            motorBlk.speed(-PIVOT);
            motorR.speed(-rasio*PIVOT-t_new);
            motorL.speed(-rasio*PIVOT-t_new);
            break;
        }
        case (18): 
        {
            // Kanan + Rotasi Kiri
            motorDpn.speed(PIVOT);
            motorBlk.speed(PIVOT);
            motorR.speed(rasio*PIVOT+t_new);
            motorL.speed(rasio*PIVOT+t_new);
            break;
        }
        case (19): 
        {       
            // Kiri + Rotasi Kanan
            motorDpn.speed(-PIVOT);
            motorBlk.speed(-PIVOT);
            motorR.speed(-rasio*PIVOT-t_new);
            motorL.speed(-rasio*PIVOT-t_new);
            break;
        }
        case (20): 
        {
            // Kiri + Rotasi Kiri
            motorDpn.speed(PIVOT);
            motorBlk.speed(PIVOT);
            motorR.speed(rasio*PIVOT+t_new);
            motorL.speed(rasio*PIVOT+t_new);
            break;
        }
        case (21): 
        {       
            // Maju + Rotasi Kanan
            motorDpn.speed(-PIVOT);
            motorBlk.speed(-PIVOT);
            motorR.speed(-rasio*PIVOT-t_new);
            motorL.speed(-rasio*PIVOT-t_new);
            break;
        }
        case (22): 
        {
            // Maju + Rotasi Kiri
            motorDpn.speed(PIVOT);
            motorBlk.speed(PIVOT);
            motorR.speed(rasio*PIVOT+t_new);
            motorL.speed(rasio*PIVOT+t_new);
            break;
        }
        case (23): 
        {       
            // Mundur + Rotasi Kanan
            motorDpn.speed(-PIVOT);
            motorBlk.speed(-PIVOT);
            motorR.speed(-rasio*PIVOT-t_new);
            motorL.speed(-rasio*PIVOT-t_new);
            break;
        }
        case (24): 
        {
            // Mundur + Rotasi Kiri
            motorDpn.speed(PIVOT);
            motorBlk.speed(PIVOT);
            motorR.speed(rasio*PIVOT+t_new);
            motorL.speed(rasio*PIVOT+t_new);
            break;
        }
        case (25): 
        {
            // Kanan + segitiga
            isLauncher = !isLauncher;
            break;
        }
        case (26): 
        {
            // Kiri + segitiga
            isLauncher = !isLauncher;
            break;
        }
        case (27): 
        {
            // Kanan + lingkaran
            ReloadOn = !ReloadOn;
            isReload = false;
            break;
        }
        case (28): 
        {
            // Kiri + lingkaran
            ReloadOn = !ReloadOn;
            isReload = false;
            break;
        }
        case (29): 
        {
            // Kanan + kotak
            ReloadOn = !ReloadOn;
            isReload = true;
            break;
        }
        case (30): 
        {
            // Kiri + kotak
            ReloadOn = !ReloadOn;
            isReload = true;
            break;
        }
        case (13) : 
        {
            // Serong kanan maju
            motorDpn.speed(-serong);
            motorL.speed(-serong-t_new);
            motorBlk.speed(serong);
            motorR.speed(serong+t_new);
            break;
        }
        case (14) : 
        {
            // Serong kiri maju
            motorDpn.speed(serong);
            motorR.speed(serong+t_new);
            motorBlk.speed(-serong);
            motorL.speed(-serong-t_new);
            break;
        }
        case (15) : 
        {
            // Serong kanan mundur 
            motorDpn.speed(-serong);
            motorR.speed(-serong-t_new);
            motorBlk.speed(serong);
            motorL.speed(serong+t_new);
            break;
        }
        case (16) : 
        {
            // Serong kiri mundur
            motorDpn.speed(serong);
            motorL.speed(serong+t_new);
            motorBlk.speed(-serong);
            motorR.speed(-serong-t_new);
            break;
        }
        case (3) : 
        {
            // Kanan
            aksel++;
            if (aksel>300)
                tuneAksel = 0.9;
            
            motorDpn.speed(-tuneAksel);
            motorBlk.speed(tuneAksel);
            motorR.brake(1);
            motorL.brake(1);
            break;
        }
        case (4) : 
        {
            // Kiri
            aksel++;
            if (aksel>300)
                tuneAksel = 0.9;
                
            motorDpn.speed(tuneAksel);
            motorBlk.speed(-tuneAksel);
            motorR.brake(1);
            motorL.brake(1);
            break;
        }
        case (8) : 
        {
            // Maju
            aksel++;
            if (aksel>300)
                tuneAksel = 0.9;
            
            motorR.speed(tuneAksel);
            motorL.speed(-tuneAksel);
            motorDpn.brake(1);
            motorBlk.brake(1);
            break;
        }
        case (9) : 
        {
            // Mundur
            aksel++;
            if (aksel>300)
                tuneAksel = 0.9;
            
            motorR.speed(-tuneAksel);
            motorL.speed(tuneAksel);
            motorDpn.brake(1);
            motorBlk.brake(1);
            break;
        }
        case (5) : 
        {
            // Aktifkan motor atas
            isLauncher = !isLauncher;
            break;
        }
        case (6) : 
        {
            // Target Pulse PID ++ Motor Depan
            target_rpm2 = target_rpm2+1.0;
            target_rpm = target_rpm+1.0;
            init_rpm();
            break;
        }
        case (7) : 
        {
            // Target Pulse PID -- Motor Depan
            target_rpm2 = target_rpm2-1.0;
            target_rpm = target_rpm-1.0;
            init_rpm();
            break;
        }
        case (10) : 
        {
            // Pneumatik
            if (ready)
            {
                pneumatik = 0;
                previousMillis3 = millis();
                flag_Pneu = true;
                ready = false;
                
            }
            break;
        }
        case (11) : 
        {
            // Power Screw Up
            ReloadOn = !ReloadOn;
            isReload = false;
            break;
        }
        case (31) : 
        {
            // start
            target_rpm2 = 23;
            target_rpm = 23;
            init_rpm();
            break;
        }
        case (32) : 
        {
            // select
            target_rpm2 = 11;
            target_rpm = 11;
            init_rpm();
            break;
        }
        case (33) : 
        {
            // R3
            target_rpm2 = 18;
            target_rpm = 18;
            init_rpm();
            break;
        }
        case (12) : 
        {
            // Power Screw Down
            ReloadOn = !ReloadOn;
            isReload = true;
            break;
        }
        case (34) :{
            pneu_paku = !pneu_paku;
            wait_ms(50);
            if (pneu_paku == 1){
                PIVOT = 0.17;
            }else{
                PIVOT = 0.8;
            }
        }
        default : 
        {
            tuneAksel = 0.6;
            aksel = 0;
            motorDpn.brake(1);
            motorBlk.brake(1);
            motorR.brake(1);
            motorL.brake(1);
        }   
    } // End Switch
 }
 
void reloader()
{
    if(ReloadOn){
        if(isReload){
            powerScrew.speed(pwmPowerDown);
            pc.printf("%.2f\n", jarak_ping);
            if((!limitBawah)||(!limitBawah1)){
                isReload = false;
                ReloadOn = false;
            }
        }
        else if(!limitTengah){
            isReload = true;
        }
        else if(!flag_Pneu){
            //pc.printf("%.2f\n", ping_pwm);
            ping_current_error =  (double) (ping_target-jarak_ping);
               
            ping_sum_error += ping_current_error*ping_Ts;
            ping_pwm = (double) ping_Kp*ping_current_error + ping_Ki*(ping_sum_error+ping_current_error)*ping_Ts;   
            ping_sum_error = ping_sum_error+ping_current_error;
            
            pc.printf("%.2f\n", jarak_ping);
            powerScrew.speed(ping_pwm);
            
            ping_previous_error1 = ping_current_error;
        }
        if ((jarak_ping>(ping_target-2))&&(jarak_ping<(ping_target+2))){
            ready = true;
        }else{
            ready = false;
        }
    }
    else{
        powerScrew.brake(1);
    }
}
 
 
void launcher()
{
    if (isLauncher)
    {
        currentMillis  = millis();
        currentMillis2 = millis();
        
        // PID Launcher Belakang
        if (currentMillis-previousMillis>=Ts)
        {
            rpm = (float)encLauncherBlk.getPulses();    
            current_error1 = target_rpm - rpm;
            a1 = kpA1 + kiA1*Ts/2 + kdA1/Ts;
            b1 = -kpA1 + kiA1*Ts/2 - 2*kdA1/Ts;
            c1 = kdA1/Ts;
            speed = previous_speed1 + a1*current_error1 + b1*previous_error1_1 + c1*previous_error1_2;
            //init_speed();
            if(speed > maxSpeed){
                launcherBlk.speed(maxSpeed);
            }
            else if ( speed < minSpeed){
                launcherBlk.speed(minSpeed);
            }
            else {
                launcherBlk.speed(speed);
            }
            previous_speed1 = speed;
            previous_error1_2 = previous_error1_1;
            previous_error1_1 = current_error1;
            encLauncherBlk.reset();
            previousMillis = currentMillis;
            
        }
        // PID Launcher Depan
        if (currentMillis2-previousMillis2>=Ts)
        {
            rpm2 = (float)encLauncherDpn.getPulses();    
            current_error2 = target_rpm2 - rpm2;
            a2 = kpA2 + kiA2*Ts/2 + kdA2/Ts;
            b2 = -kpA2 + kiA2*Ts/2 - 2*kdA2/Ts;
            c2 = kdA2/Ts;
            speed2 = previous_speed2 + a2*current_error2 + b2*previous_error2_1 + c2*previous_error2_2;
            //init_speed();
            if (speed2 > maxSpeed){
                launcherDpn.speed(maxSpeed);
            }
            else if ( speed2 < minSpeed){
                launcherDpn.speed(minSpeed);
            }
            else{
                launcherDpn.speed(speed2);
            }
            previous_speed2 = speed2;
            previous_error2_2 = previous_error2_1;
            previous_error2_1 = current_error2;
            encLauncherDpn.reset();
            previousMillis2 = currentMillis2;
        }
        //pc.printf("%.2f\t%.2f\n",rpm,rpm2);
    }
    else
    {
        launcherDpn.brake(1);
        launcherBlk.brake(1);
        
        previous_error1_1 = 0;
        previous_error1_2 = 0;
        previous_error2_1 = 0;
        previous_error2_2 = 0;
        previous_speed1 = 0;
        previous_speed2 = 0;
    }     
}
  
/*********************************************************/
/*                  Main Function                        */
/*********************************************************/

int main (void)
{
    // Set baud rate - 115200
    joystick.setup();
    pc.baud(115200);
    wait_ms(1000);
    
    // initializing encoder
    pneumatik =1;
    
    wait_ms(500);
    
    //initializing PING
    pingAtas.Send();
    
    pc.printf("ready....");
    startMillis();
    while(1)
    {   
        // interupsi pembacaan PING setiap 30 ms
        if(millis() - previousMillis4 >= 10){    //30
            jarak_ping = (float)pingAtas.Read_cm();
            
            pingAtas.Send();
            previousMillis4 = millis();
        }
        
        // 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 joystick
            case_joy = case_joystick();
            //pc.printf("%d\n",case_joy);
            aktuator();
            launcher();
            reloader();
            if ((millis()-previousMillis3 >= 320)&&(flag_Pneu)){
                pneumatik = 1;
                flag_Pneu = false;
            }
        }
        else
        {
            joystick.idle();
        }
        
        if(millis() - previousMillis5 >= 400)
        {    
            display.write(0,((int) rpm2) / 10);
            display.write(1,((int)rpm2) % 10);
            display.write(2, (int)target_rpm2 / 10);
            display.write(3, (int)target_rpm2 % 10);
            display.setColon(true);
            
            previousMillis5 = millis();
        }
    }
}