File content as of revision 7:8d8eb4676356:

////////////////////////////////////////////////////////////////////////////////
// Robo Taker 2018                                                            //
// -------------------------------------------------------------------------- //
// Todo List:                                                                 //
// - Perhalus Gerakan                                                         //
// - Pasang Mode Joystick Kabel/BT                                            //
// - Program Pneumatic                                                        //
// - Tata code yang ada, Lengkapi dengan Komentar (Penjelasan)                //
// Gerakan Robot:                                                             //
// - Arah   -> Gerak                                                          //
// - Silang -> Pneumatic                                                      //
// - Start  -> Ubah Mode                                                      //
////////////////////////////////////////////////////////////////////////////////
#include "mbed.h"
#include "Motor.h"
#include "encoderKRAI.h"
#include "JoystickPS3.h"
#include "pinList.h"
#include "millis.h"

////////////////////////////////////////////////////////////////////////////////
// Konstanta Program                                                          //
////////////////////////////////////////////////////////////////////////////////
#define PI 3.141592653593
#define RAD_TO_DEG  57.2957795131
#define MAX_W_SPEED 15000           //max angular speed of robot
#define TOLERANCET 0.8              //theta tolerance
#define PULSE_TO_JARAK 0.581776     //kll roda / pulses
#define L 298.0                     //roda to center of robot
#define TS 2.0                      //time sampling
#define LIMITPWM 0.4                //limit pwm motor

//Konstanta PID Theta
#define KP_W 1.0
#define KI_W 0.0065
#define KD_W 175

#define MOTOR_LIMIT_MAX 1
#define MOTOR_LIMIT_MIN -1

// Set 1 untuk aktifkan fitur pc.print
#define DEBUG   0

////////////////////////////////////////////////////////////////////////////////
// Object Program                                                               //
////////////////////////////////////////////////////////////////////////////////
// Serial
Serial pc(USBTX, USBRX, 115200);
joysticknucleo stick(PIN_TX, PIN_RX);

// Pneumatik
DigitalOut pneumatik[3]{PIN_PNEUMATIK_1, PIN_PNEUMATIK_2, PIN_PNEUMATIK_3};
//DigitalOut pneumatik2(PIN_PNEUMATIK_2);
//DigitalOut pneumatik3(PIN_PNEUMATIK_3);

// Encoder
encoderKRAI encoder_A(PIN_A_CHANNEL_A, PIN_A_CHANNEL_B, 540, encoderKRAI::X4_ENCODING);
encoderKRAI encoder_B(PIN_B_CHANNEL_A, PIN_B_CHANNEL_B, 540, encoderKRAI::X4_ENCODING);
encoderKRAI encoder_C(PIN_C_CHANNEL_A, PIN_C_CHANNEL_B, 540, encoderKRAI::X4_ENCODING);

// Motor
Motor motor1(PIN_PWM_A, PIN_FWD_A, PIN_REV_A);   
Motor motor2(PIN_PWM_B, PIN_FWD_B, PIN_REV_B);   
Motor motor3(PIN_PWM_C, PIN_FWD_C, PIN_REV_C);   

////////////////////////////////////////////////////////////////////////////////
// Deklarasi Prosedure dan Fungsi                                             //
////////////////////////////////////////////////////////////////////////////////
void gerakMotor();
void hitungPID(double theta_s);
void hitungParameter();
void printPulse();
void case_gerak();

////////////////////////////////////////////////////////////////////////////////
// Variable-variable                                                          //
////////////////////////////////////////////////////////////////////////////////
int joystick;
int pn = 0;
double pulse_A=0, pulse_B=0, pulse_C=0;
double Vr = 0, Vr_max = 0;
double Vw = 0;
double a = 0;
double w = 0;
double x =0, x_s=0, y=0, y_s=0, x_prev=0, y_prev=0;
double theta=0, theta_s=0;
double theta_error = 0, theta_prev=0, theta_error_prev=0, sum_theta_error=0;
unsigned long last_mt_print, last_mt_pid, last_mt_rotasi;
unsigned long last_mt_aksel, last_mt_desel;
bool modeauto = 1;
bool print_pulse = 0;

////////////////////////////////////////////////////////////////////////////////
// Main Program                                                               //
////////////////////////////////////////////////////////////////////////////////
int main(){
    encoder_A.reset();
    encoder_B.reset();
    encoder_C.reset();
    for (int i = 0; i<3; i++){
        pneumatik[i] = 0;
    }
    startMillis();
    stick.setup();
    stick.idle();
    
    while(1){
        if(stick.readable() ) {
            // Panggil fungsi pembacaan joystik
            stick.baca_data();
            // Panggil fungsi pengolahan data joystik
            stick.olah_data();
            // Ambil data joystick
            case_gerak();
            
            // Pengatur Gerak Motor
            gerakMotor();
            
            // Penghitungan Theta, X, dan Y
            if (millis() - last_mt_pid > TS){
                hitungParameter();
                last_mt_pid = millis();
            }
            
            // Program PID
            if (!(fabs(theta_s - (theta*RAD_TO_DEG))<TOLERANCET) && modeauto ){
                hitungPID(theta_s);
            }
            if (fabs(theta_s - (theta*RAD_TO_DEG))<TOLERANCET || !modeauto){
                if(modeauto) Vw = 0;
            }
            
            // Fungsi Serial Print
            if (millis() - last_mt_print > TS*10){
                if (print_pulse && DEBUG)
                    printPulse();
                last_mt_print = millis();
            }
        }
   }
}


////////////////////////////////////////////////////////////////////////////////
// Prosedure dan Fungsi                                             //
////////////////////////////////////////////////////////////////////////////////
void hitungParameter(){
    pulse_A = encoder_A.getPulses()*PULSE_TO_JARAK;
    pulse_B = encoder_B.getPulses()*PULSE_TO_JARAK;
    pulse_C = encoder_C.getPulses()*PULSE_TO_JARAK;

    //Compute value
    x = x_prev + (2*pulse_A - pulse_C - pulse_B)/3*cos(theta_prev) - (-pulse_C+pulse_B)*0.5773*sin(theta_prev);
    y = y_prev + (2*pulse_A - pulse_C - pulse_B)/3*sin(theta_prev) + (-pulse_C+pulse_B)*0.5773*cos(theta_prev);
    theta = theta_prev + (pulse_A + pulse_C + pulse_B)/(3.0*L);
    
    //Update value
    x_prev = x;
    y_prev = y;
    theta_prev = theta;

    encoder_A.reset();
    encoder_B.reset();
    encoder_C.reset();
}

void hitungPID(double theta_s){
    //menghitung error jarak x,y terhaadap xs,ys
    theta_error = theta_s - (theta*RAD_TO_DEG);
    sum_theta_error += theta_error;

    //kalkulasi PID Theta
    w = KP_W*theta_error + KI_W*TS*sum_theta_error + KD_W*(theta_error - theta_error_prev)/TS;
    Vw += (w*L/MAX_W_SPEED)*LIMITPWM;

    //update
    theta_error_prev = theta_error;
    
    //saturasi vw
    if (Vw > 0.2){
        Vw = 0.2;
    }
    else if ( Vw < -0.2){
        Vw = -0.2;
    }
}

void gerakMotor(){
    if ((Vw == 0) && (Vr_max == 0)){
        motor1.brake(BRAKE_HIGH);
        motor2.brake(BRAKE_HIGH);
        motor3.brake(BRAKE_HIGH);
        
        // Pengaturan variable saat berhenti
        print_pulse = 0;
        Vr = 0;
    } else {
        // Akselerasi
        if ((millis() - last_mt_aksel > 150) && Vr < Vr_max){
            if (Vr < 0.35) 
                Vr = 0.35;
            else
                Vr += 0.05;
            last_mt_aksel = millis();
        }
        // Limit
        if (Vr > Vr_max && Vr_max >= 0.000)
            Vr = Vr_max;
        
        // Control Motor
        motor1.speed((Vr*cos(a) + Vw));
        motor2.speed((Vr*(-0.5*cos(a) - 0.866*sin(a)) + Vw));
        motor3.speed((Vr*(-0.5*cos(a) + 0.866*sin(a)) + Vw));
        print_pulse = 1;
    } 
}

void printPulse(){
    pc.printf("%.2f\t%.2f\n", theta*RAD_TO_DEG, theta_s);    
    //pc.printf("%.2d\t%.2d\t%.2d\n", pulse_A, pulse_B, pulse_C);
}

void case_gerak(){
    // Pengubah Mode
    if(stick.START_click){
        modeauto = !modeauto;
        theta = 0.0;
        theta_prev = 0.0;
        theta_s = 0;
        theta_error_prev = 0;
        sum_theta_error = 0;
        theta_error = 0;
    }
    
    // Rotasi
    if(modeauto){
        if (!stick.L1 && stick.R1){        // Pivot Kanan
            theta = 0.0;
            theta_prev = 0.0;
            theta_s = 0;
            theta_error_prev = 0;
            sum_theta_error = 0;
            theta_error = 0;
            Vw = 0.2;
        }
        else if (!stick.R1 && stick.L1){   // Pivot Kiri
            theta = 0.0;
            theta_prev = 0.0;
            theta_s = 0;
            theta_error_prev = 0;
            sum_theta_error = 0;
            theta_error = 0;
            Vw = -0.2;
        }
    } else if(!modeauto){
        if (!stick.L1 && stick.R1)        // Pivot Kanan
            Vw = 0.3;
        else if (!stick.R1 && stick.L1)   // Pivot Kiri
            Vw = -0.3;
        else
            Vw = 0.0;
    }
    
    // Linier
    if ((stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)){
        a = -90/RAD_TO_DEG; // Maju
        Vr_max = 0.8;
    }
    else if ((!stick.atas)&&(stick.bawah)&&(!stick.kanan)&&(!stick.kiri)){
        a = 90/RAD_TO_DEG; // Mundur
        Vr_max = 0.8;
    }
    else if ((stick.atas)&&(!stick.bawah)&&(!stick.kiri)&&(stick.kanan)){
        a = -135/RAD_TO_DEG; // Serong Atas Kanan
        Vr_max = 0.7;
    }
    else if ((!stick.atas)&&(stick.bawah)&&(!stick.kiri)&&(stick.kanan)){
        a = 135/RAD_TO_DEG; // Serong Bawah Kanan
        Vr_max = 0.7;
    }
    else if ((stick.atas)&&(!stick.bawah)&&(stick.kiri)&&(!stick.kanan)){
        a = -45/RAD_TO_DEG; // Serong Atas Kiri
        Vr_max = 0.7;
    }
    else if ((!stick.atas)&&(stick.bawah)&&(stick.kiri)&&(!stick.kanan)){
        a = 45/RAD_TO_DEG; // Serong Bawah Kiri
        Vr_max = 0.7;
    }
    else if ((!stick.atas)&&(!stick.bawah)&&(stick.kanan)&&(!stick.kiri)){
        a = 180/RAD_TO_DEG; // Kanan
        Vr_max = 0.5;
    }
    else if ((!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(stick.kiri)){
        a = 0/RAD_TO_DEG; // Kiri
        Vr_max = 0.5;
    }
    else {
        Vr_max = 0;
    }
    
    // Control Pneumatic
    if ((stick.silang_click)&&(!stick.kotak)&&(!stick.segitiga)&&(!stick.lingkaran)){
        pneumatik[pn]   = 1; 
        pn++;
        if (pn > 2) pn = 2;
    }
    if ((!stick.silang_click)&&(!stick.kotak)&&(!stick.segitiga)&&(stick.lingkaran)){
        pneumatik[pn]   = 0; 
        pn--;
        if (pn < 0) pn = 0;
    }
    if ((!stick.silang_click)&&(!stick.kotak)&&(stick.segitiga)&&(!stick.lingkaran))
        for (int i = 0; i<3; i++){
            pneumatik[i] = 0;
            pn = 0;
        }
    if ((!stick.silang_click)&&(stick.kotak)&&(!stick.segitiga)&&(!stick.lingkaran))
        for (int i = 0; i<3; i++){
            pneumatik[i] = 1;
            pn = 0;
        }

}