File content as of revision 4:3c890389e256:

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

#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 Sudut
#define KP_W 1.0
#define KI_W 0.0065
#define KD_W 125

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

// Fungsi dan Prosedur
void gerakMotor();
void hitungPID(float theta_s);
void printPulse();
void case_gerak();
float compute_Alpha(float x_s, float y_s, float x, float y,float theta);

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

int main(){
    encoder_A.reset();
    encoder_B.reset();
    encoder_C.reset();
    pc.baud(115200);
    stick.setup();
    stick.idle();
    pneumatik = 0;
    startMillis();
    
    while(1){
        // do nothing
        if(stick.readable() ) {
            // Panggil fungsi pembacaan joystik
            stick.baca_data();
            // Panggil fungsi pengolahan data joystik
            stick.olah_data();
            // Ambil data joystick
            case_gerak();
    
            gerakMotor();
            
            if ( (millis() - last_mt_pid > TS) && !(fabs(theta_s - (theta*RAD_TO_DEG))<TOLERANCET) && modeauto ){
                hitungPID(theta_s);
                last_mt_pid = millis();
            }
            if (fabs(theta_s - (theta*RAD_TO_DEG))<TOLERANCET || !modeauto){
                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
                theta = theta_prev + (pulse_A + pulse_C + pulse_B)/(3.0*L);
                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);
                
                //Update value
                theta_prev = theta;
                x_prev = x;
                y_prev = y;
                
                encoder_A.reset();
                encoder_B.reset();
                encoder_C.reset();
                
                if(modeauto) Vw = 0;
            }
            if (millis() - last_mt_print > TS+5){
                if (print_pulse && DEBUG)
                    printPulse();
                last_mt_print = millis();
            }
        }
   }
}

void hitungPID(float theta_s){
    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();
    
    //theta_s = theta_s/RAD_TO_DEG;
    //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 == 0)){
       motor1.brake(BRAKE_HIGH);
       motor2.brake(BRAKE_HIGH);
       motor3.brake(BRAKE_HIGH);
       print_pulse = 0;
    } else {
        //a = compute_Alpha(x_s, y_s, x, y, theta);
        motor1.speed((-1*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);    
}

float compute_Alpha(float x_s, float y_s, float x, float y,float theta){
//fungsi untuk menghitung alpha sebagai arah gerak robot
    float temp = atan((y_s - y)/(x_s - x)) - theta;
    
    if (x_s < x)    return temp + PI;
    else            return temp;
}

void case_gerak(){
    // Rotasi
    if(modeauto){
        if (!stick.L1 && stick.R1)        // Pivot Kanan
            theta_s += 0.15;
        else if (!stick.R1 && stick.L1)   // Pivot Kiri
            theta_s -= 0.15;

    }
    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;
    }
    
    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;
    }
    
    // Linier
    if ((stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)){
        a = -90/RAD_TO_DEG; // Maju
        Vr = 0.8;
    //    x_s = 0;// Maju
    //    y_s = 10000;
    }
    else if ((!stick.atas)&&(stick.bawah)&&(!stick.kanan)&&(!stick.kiri)){
        a = 90/RAD_TO_DEG; // Mundur
        Vr = 0.8;
    //    x_s = 0; // Mundur
    //    y_s = -10000;
    }
    else if ((stick.atas)&&(!stick.bawah)&&(!stick.kiri)&&(stick.kanan)){
        a = -135/RAD_TO_DEG; // Serong Atas Kanan
        Vr = 0.7;
    //    x_s = 50000; // Maju+Kanan
    //    y_s = 50000;
    }
    else if ((!stick.atas)&&(stick.bawah)&&(!stick.kiri)&&(stick.kanan)){
        a = 135/RAD_TO_DEG; // Serong Bawah Kanan
        Vr = 0.7;
    //    x_s = 50000; // Mundur+Kanan
    //    y_s = -50000;
    }
    else if ((stick.atas)&&(!stick.bawah)&&(stick.kiri)&&(!stick.kanan)){
        a = -45/RAD_TO_DEG; // Serong Atas Kiri
        Vr = 0.7;
    //    x_s = 50000; // Maju+Kiri
    //    y_s = -50000;
    }
    else if ((!stick.atas)&&(stick.bawah)&&(stick.kiri)&&(!stick.kanan)){
        a = 45/RAD_TO_DEG; // Serong Bawah Kiri
        Vr = 0.7;
    //    x_s = -50000; // Mundur+Kiri
    //    y_s = -50000;
    }
    else if ((!stick.atas)&&(!stick.bawah)&&(stick.kanan)&&(!stick.kiri)){
        a = 180/RAD_TO_DEG; // Kanan
        Vr = 0.5;
    //    x_s = 50000; // Kanan
    //    y_s = 0;
    }
    else if ((!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(stick.kiri)){
        a = 0/RAD_TO_DEG; // Kiri
        Vr = 0.5;
    //    x_s = -50000; // Kiri
    //    y_s = 0;
    }
    else {
        Vr = 0;
        a = 0;
    }
    
    if ((stick.silang_click)&&(!stick.kotak)&&(!stick.segitiga)&&(!stick.lingkaran))
        pneumatik = !pneumatik; // Silang = Toggle pneumatik
}