File content as of revision 0:22acd37ed695:

#include "mbed.h"
#include "Motor.h"
#include "encoderKRAI.h"
#include "JoystickPS3.h"
#include "rtos.h"
#include "pinList.h"

#define PI 3.141592653593
#define RAD_TO_DEG  57.2957795131
#define PULSE_TO_MM 0.1177    //rev/pulse * K_lingkaran_roda
#define L 298.0

#define MOTOR_LIMIT_MAX 1
#define MOTOR_LIMIT_MIN -1

#define DEBUG   1

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

// Pneumatik
DigitalOut pneumatik(PIN_PNEUMATIK);

// 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);   

// List Thread
Thread thread1;
Thread thread2;
Thread thread3;
//Thread thread4;

// Fungsi dan Prosedur
void getJoystick();
void gerakMotor();
void hitungPID();
void printPulse();
void case_gerak();

// Variable-variable
int joystick;
int pulse_A=0;
int pulse_B=0;
int pulse_C=0;
float Vr = 0;
float Vw = 0;
float a = 0;
double pid_t=0;
short sp_teta;
int sum=0;

int main(){
    encoder_A.reset();
    encoder_B.reset();
    encoder_C.reset();
    pc.baud(9600);
    stick.setup();
    stick.idle();
    pneumatik = 0;

    // Thread
    // thread1.start(getJoystick);
    thread2.start(gerakMotor);
    //thread3.start(printPulse);
    
    while(1){
        // do nothing
        if(stick.readable() ) {
            // Panggil fungsi pembacaan joystik
            stick.baca_data();
            // Panggil fungsi pengolahan data joystik
            stick.olah_data();

            case_gerak();            
            
            pc.printf("Rotasi: %.2f\t", Vw);
            pc.printf("Tarnslasi: %.2f, %.1f\n", Vr, a);

        } else {
            Vr=0;
            Vw=0;
            pc.printf("Masalah Joystick\n");
        }
    }
}

void getJoystick(){
    while(1){
        
        Thread::wait(1);
    }
}

void hitungPID(){
    
}

void gerakMotor(){
    while(1){
        if(Vw != 0){
            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));
        } else if (Vw == 0) { // mungkin bisa (a+pid_t)
            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));
        } else {
           motor1.brake(BRAKE_HIGH);
           motor2.brake(BRAKE_HIGH);
           motor3.brake(BRAKE_HIGH);
        } 
        Thread::wait(1);
    }
}

void printPulse(){
    while(1){
        pulse_A = encoder_A.getPulses();
        pulse_B = encoder_B.getPulses();
        pulse_C = encoder_C.getPulses();
        pc.printf("%d\t%d\t%d\n", pulse_A, pulse_B, pulse_C);    
        encoder_A.reset();
        encoder_B.reset();
        encoder_C.reset();//*/
        Thread::wait(20);
    }
}

void case_gerak(){
    // Rotasi
    if (!stick.L1 && stick.R1)        // Pivot Kanan
        Vw = 0.3;
    else if (!stick.R1 && stick.L1)   // Pivot Kiri
        Vw = -0.3;
    else 
        Vw = 0;
    
    // Linier
    Vr = 0.5;
    if ((stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri))
        a = -90/RAD_TO_DEG; // Maju
    else if ((!stick.atas)&&(stick.bawah)&&(!stick.kanan)&&(!stick.kiri))
        a = 90/RAD_TO_DEG; // Mundur
    else if ((stick.atas)&&(!stick.bawah)&&(!stick.kiri)&&(stick.kanan))
        a = -135/RAD_TO_DEG; // Serong Atas Kanan
    else if ((!stick.atas)&&(stick.bawah)&&(!stick.kiri)&&(stick.kanan))
        a = 135/RAD_TO_DEG; // Serong Bawah Kanan
    else if ((stick.atas)&&(!stick.bawah)&&(stick.kiri)&&(!stick.kanan))
        a = -45/RAD_TO_DEG; // Serong Atas Kiri
    else if ((!stick.atas)&&(stick.bawah)&&(stick.kiri)&&(!stick.kanan))
        a = 45/RAD_TO_DEG; // Serong Bawah Kiri
    else if ((!stick.atas)&&(!stick.bawah)&&(stick.kanan)&&(!stick.kiri))
        a = 180/RAD_TO_DEG; // Kanan
    else if ((!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(stick.kiri))
        a = 0/RAD_TO_DEG; // Kiri
    else {
        Vr = 0;
    }
    
    if ((stick.silang_click)&&(!stick.kotak)&&(!stick.segitiga)&&(!stick.lingkaran))
        pneumatik = !pneumatik; // Silang = Toggle pneumatik
            
}