File content as of revision 38:3ef6754bd8d8:

/****************************************************************************/
/*                  PROGRAM UNTUK PID CLOSED LOOP                           */
/*                                                                          */
/*                  Last Update : 18 Februari 2017                          */
/*                                                                          */
/*  - 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 =>                                                                */
/*  Kiri  =>                                                                */
/*                                                                          */
/*  Tombol silang    => Power Screw Aktif                                   */
/*  Tombol segitiga  => Aktif motor Launcher                                */
/*  Tombol lingkaran => Aktif Pneumatik 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 PERPINDAHAN 1       // Perpindahan ke kanan dan kiri

// Variable Atas
double speed, speed2;
const double maxSpeed = 0.95;
const 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, rpm2;
float target_rpm = 12.0, target_rpm2 = 16.0; 
const float maxRPM = 27, minRPM = 0; // Limit 25 atau 27

const float pwmPowerUp        = 0.78;
const float pwmPowerDown      = -0.9;
 
// Variable Bawah
float Vt;
float keliling_enc      = PI*D_ENCODER;
float keliling_robot    = PI*D_ROBOT;
float speedT            = 0.2;
float vpid              = 0;
float PIVOT             = 0.27;      // PWM Pivot Kanan, Pivot Kiri
float tuneDpn           = 0.35;    // Tunning PWM motor Depan
float tuneBlk           = 0.3;      // Tunning PWM motor belakang

/* Variabel Encoder Bawah */
float errTetha, Tetha;    // Variabel yang didapatkan encoder

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

/* Variabel Stick */
//Logic untuk masuk aktuator
int case_joy;
bool isLauncher = false;
bool isReload = false;
bool ReloadOn = false;
bool flag_Pneu = 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
void setCenter();                   // Prosedur reset encoder, posisi saat itu diset jadi titik (0,0)
float getTetha();                   // Fungsi mendapatkan error Tetha

/* 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 encLauncherBlk( PC_10, PC_11, 28, encoderKRAI::X4_ENCODING);
encoderKRAI encLauncherDpn( PD_2, PC_12, 28, encoderKRAI::X4_ENCODING);

/* Deklarasi Motor Base */
Motor motorDpn(PB_9, PA_12, PC_5);    // pwm, fwd, rev
Motor motorBlk(PB_6, PB_1, PB_12);    // pwm, fwd, rev

/* Deklarasi Motor Launcher */
Motor launcherDpn(PA_8,PC_2,PC_1);    // pwm ,fwd, rev
Motor launcherBlk(PA_10, PC_3, PC_0); // pwm, fwd, rev 
Motor powerScrew(PB_7, PA_14, PA_15); // pwm, fwd, rev                

/* Deklarasi Penumatik Launcher */
DigitalOut pneumatik(PB_3, PullUp);

/*Dekalrasi Limit Switch */
DigitalIn infraAtas(PC_9, PullUp);
DigitalIn limitTengah(PB_10, PullUp);
DigitalIn limitBawah(PC_8, PullUp);


/****************************************************/
/*         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.L1 && joystick.R1) {
        // Pivot Kanan
        caseJoystick = 1;
    } 
    else if (!joystick.R1 && joystick.L1) {
        // Pivot Kiri
        caseJoystick = 2;
    } 
    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri))  {   
        // Kanan
        caseJoystick = 3;
         //pc.printf("kanan");
    } 
    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {   
        // Kiri
        caseJoystick = 4;
         //pc.printf("kiri");       
    } 
    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 Depan
         caseJoystick = 7;
    }
    /*else if (joystick.R3_click){
        // Target Pulse PID ++ Motor Belakang
         caseJoystick = 8;
    }
    else if (joystick.L3_click){
        // Target Pulse PID -- Motor Belakang
         caseJoystick = 9;
    }*/
    else if (joystick.silang_click){
        // Pnemuatik ON
        caseJoystick = 10;
    }
    else if ((joystick.atas)&&(!joystick.bawah))  {   
        // Power Screw Up
        caseJoystick = 11;
    } 
    else if ((!joystick.atas)&&(joystick.bawah)) {   
        // Power Screw Down
        caseJoystick = 12;       
    } 
    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {  
        // Break
        caseJoystick = 13; 
    }
    
    return(caseJoystick);
}

float getTetha(){
// Fungsi untuk mendapatkan nilai tetha
    float busur, tetha;
    busur = ((encoderBase.getPulses())/(float)(2000.0)*keliling_enc);
    tetha = busur/keliling_robot*360;
    
    return -(tetha);    
}

float pidBase(float Kp, float Ki, float Kd)
{
    int errorP;
    errorP = getTetha();
    if (errorP<3.5 && errorP>(-3.5))
        errorP = 0;
    return (float)Kp*errorP;    
}
/*
void init_speed (){
    if (speed>maxSpeed){
        speed = maxSpeed;
    }
    if (speed<minSpeed){
        speed = minSpeed;
    }
    if (speed2>maxSpeed){
        speed2 = maxSpeed;
    }
    if (speed2<minSpeed){
        speed2 = minSpeed;
    }
}*/

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

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);
            setCenter();
            break;
        }
        case (2): 
        {
            // Pivot Kiri
            motorDpn.speed(PIVOT);
            motorBlk.speed(PIVOT);
            setCenter();
            break;
        }
        case (3) : 
        {
            // Kanan
            motorDpn.speed(-tuneDpn + pidBase(0.009,0,0));
            motorBlk.speed(tuneBlk + pidBase(0.009,0,0));
            //speedDpn = tuneDpn + pidBase(0.009,0,0)
            //speedBlk = tuneBlk + pidBase(0.009,0,0)
            //motorDpn.speed(-tuneDpn);
            //motorBlk.speed(tuneBlk);
            break;
        }
        case (4) : 
        {
            // Kiri
            motorDpn.speed(tuneDpn + pidBase(0.009,0,0));
            motorBlk.speed(-tuneBlk + pidBase(0.009,0,0));
            //motorDpn.speed(tuneDpn);
            //motorBlk.speed(-tuneBlk);
            break;
        }
        case (5) : 
        {
            // Aktifkan motor atas
            isLauncher = !isLauncher;
            break;
        }
        case (6) : 
        {
            // Target Pulse PID ++ Motor Depan
            target_rpm2 = target_rpm2++;
            target_rpm = target_rpm++;
            init_rpm();
            break;
        }
        case (7) : 
        {
            // Target Pulse PID -- Motor Depan
            target_rpm2 = target_rpm2--;
            target_rpm = target_rpm--;
            init_rpm();
            break;
        }
        /*case (8) : 
        {
            // Target Pulse PID ++ Motor Belakang=
            //init_rpm();
            break;
        }
        case (9) : 
        {
            // Target Pulse PID -- Motor Belakang
            //init_rpm();
            break;
        }*/
        case (10) : 
        {
            // Pneumatik
            pneumatik = 0;
            previousMillis3 = millis();
            flag_Pneu = true;
            break;
        }
        case (11) : 
        {
            // Power Screw Up
            //powerScrew.speed(pwmPowerUp);
            ReloadOn = !ReloadOn;
            break;
        }
        case (12) : 
        {
            // Power Screw Down
            //powerScrew.speed(pwmPowerDown);
            break;
        }
        default : 
        {
            motorDpn.brake(1);
            motorBlk.brake(1);
           // aktuator();
         /*   if (!infraAtas)
            {
                powerScrew.brake(1);
            }
            if (!limitTengah)
            {
                case_joy = 12;
                aktuator();   
            }
            if (!limitBawah)
            {
                case_joy = 11;
                aktuator();   
            }*/
        }   
    } // End Switch
 }
 
 void reloader()
{
    if(ReloadOn){
        if(isReload){
            powerScrew.speed(pwmPowerDown);
            if(!limitBawah){
                isReload = false;
                ReloadOn = false;
            }
        }
        else if(!limitTengah){
            isReload = true;
        }
        else if(!infraAtas){
            powerScrew.brake(1);
        }
        else{
            powerScrew.speed(pwmPowerUp);
        }
    }
    else{
        powerScrew.brake(1);
    }
}
 
 
void launcher()
{
    if (isLauncher)
    {
        currentMillis  = millis();
        currentMillis2 = millis();
        
        // PID Launcher Depan
        if (currentMillis-previousMillis>=12.5)
        {
            rpm = (float)encLauncherBlk.getPulses();    
            current_error = target_rpm - rpm;
            sum_error = sum_error + current_error;
            p = current_error*kpA;
            d = (current_error-last_error)*kdA/12.5;
            i = sum_error*kiA*12.5;
            speed = p + d + i;
            //init_speed();
            if(speed > maxSpeed){
                launcherBlk.speed(maxSpeed);
            }
            else {
                launcherBlk.speed(speed);
            }
            last_error = current_error;
            encLauncherBlk.reset();
          //pc.printf("%.04lf\n",rpm);
            previousMillis = currentMillis;
        }
        if (currentMillis2-previousMillis2>=12.5)
        {
            rpm2 = (float)encLauncherDpn.getPulses();    
            current_error2 = target_rpm2 - rpm2;
            sum_error2 = sum_error2 + current_error2;
            p2 = current_error2*kpA;
            d2 = (current_error2-last_error2)*kdA/12.5;
            i2 = sum_error2*kiA*12.5;
            speed2 = p2 + d2 + i2;
            //init_speed();
            if (speed2 > maxSpeed){
                launcherDpn.speed(maxSpeed);
            }
            else{
                launcherDpn.speed(speed2);
            }
            last_error2 = current_error2;
            encLauncherDpn.reset();
            previousMillis2 = currentMillis2;
        }
    }
    else
    {
        launcherDpn.brake(1);
        launcherBlk.brake(1);
    }     
}
  
/*********************************************************/
/*                  Main Function                        */
/*********************************************************/

int main (void)
{
    // Set baud rate - 115200
    joystick.setup();
    pc.baud(115200);
    wait_ms(1000);
    setCenter();
    wait_ms(500);
    pc.printf("ready....");
    startMillis();
    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 joystick
            case_joy = case_joystick();
            aktuator();
            launcher();
            reloader();
            if ((millis()-previousMillis3 >= 425)&&(flag_Pneu)){
                pneumatik = 1;
                flag_Pneu = false;
            }
        }
        else
        {
            joystick.idle();
        }
    }
}