File content as of revision 6:f10c0d9f228d:

#include "mbed.h"
#include "TextLCD.h"
#include "PS_PAD.h"
#include "Motor.h"
#include "encoderKRAI.h"

#include <string>
using namespace std;

#define PI  3.14159265359
#define RAD_TO_DEG  57.2957795131

#define PULSE_TO_MM 0.1177    //rev/pulse * K_lingkaran_roda
#define L 144.0           // lengan roda dari pusat robot (mm)
#define Ts 2.0    // Time Sampling sistem 2ms

#define MAX_SPEED 912.175  //Vresultan max (mm/s)
#define MAX_W_SPEED 1314.72 //Vw max (mm/s)
#define SPEED 1             //V robot

#define BOUNDARY_TOLERANCE 70.0

// konstanta PID untuk kendali Posisi (x y)
#define Kp_s 10.0
#define Ki_s 0.0
#define Kd_s 1.6

// konstanta PID untuk kendali arah (theta)
#define Kp_w 0.2
#define Ki_w 0.0
#define Kd_w 0.01

Thread thread1(osPriorityNormal, OS_STACK_SIZE, NULL);
Thread thread2(osPriorityNormal, OS_STACK_SIZE, NULL);
Thread thread3(osPriorityNormal, OS_STACK_SIZE, NULL);
Thread thread4(osPriorityNormal, OS_STACK_SIZE, NULL);
Thread thread5(osPriorityNormal, OS_STACK_SIZE, NULL);

TextLCD lcd(PA_9, PC_3, PC_2, PA_8, PB_0, PC_15, TextLCD::LCD20x4); //rs,e,d4-d7
encoderKRAI enc1 (PC_14, PC_13, 11, encoderKRAI::X4_ENCODING);
encoderKRAI enc2 (PC_0, PC_1, 11, encoderKRAI::X4_ENCODING);
encoderKRAI enc3 (PB_10, PB_3, 11, encoderKRAI::X4_ENCODING);
PS_PAD ps2(PB_15,PB_14,PB_13, PC_4); //(mosi, miso, sck, ss)

Motor motor3(PB_7, PA_14, PA_15); //motor4
Motor motor2(PA_11, PA_6, PA_5); //motor2
Motor motor1(PB_6, PA_7, PB_12); //motor 3
//Motor motor1(PA_10, PB_5, PB_4);  //motor_griper

Serial pc(USBTX,USBRX);

void dataJoystick();
void lcdPrint();
void self_localization();
void motor_out();
void PTP_movement();

int calculate_PID(float *x_set, float *y_set, float *theta_set, bool isLast);
float moving_direction( float xs, float ys, float x, float y,float theta);

/*------------buruk----------------*/
float x = 0;
float y = 0;
float theta = 0;

float x_prev = 0;
float y_prev = 0;
float theta_prev = 0;
 
float vr = 0;
float vw = 0;
float a = 0;
 
float Vx = 0;
float Vy = 0;
float W = 0;

string str;
/*---------------------------------------*/

int main()
{    
    pc.baud(115200);
    ps2.init();
    thread1.start(dataJoystick);
    thread2.start(lcdPrint);
    thread3.start(self_localization);
    thread4.start(motor_out);
    thread5.start(PTP_movement);
    
    while (1)
    {      
        //do nothing
    }
}

void dataJoystick(){
    while(true){
    ps2.poll();
    if(ps2.read(PS_PAD::PAD_X)==1)                   str =  "silang";
    else if(ps2.read(PS_PAD::PAD_CIRCLE)==1)         str = "lingkaran";
    else if(ps2.read(PS_PAD::PAD_TRIANGLE)==1)       str = "segitiga";
    else if(ps2.read(PS_PAD::PAD_SQUARE)==1)         str = "kotak";
    else                                             str = "NULL";    
    }
}

void lcdPrint(){
    while (true){
        lcd.cls();         
        lcd.locate(0,0);
        lcd.printf("input : %s", str);
        lcd.locate(0,1);
        //lcd.printf("Vr = %.2f", sqrt(Vx*Vx + Vy*Vy));
        lcd.printf("x = %.2f", x);
        lcd.locate(0,2);
        lcd.printf("y = %.2f", y);
        lcd.locate(0,3);
        lcd.printf("theta = %.2f", theta*RAD_TO_DEG);
        //lcd.printf("a = %.2f", a*RAD_TO_DEG);
        
        Thread::wait(100);
    }
}


void self_localization(){
    while(true){       
        float d1 = enc1.getPulses()*PULSE_TO_MM;
        float d2 = enc2.getPulses()*PULSE_TO_MM;
        float d3 = enc3.getPulses()*PULSE_TO_MM;
         
        x_prev = x;
        y_prev = y;
        theta_prev = theta;
        
        x = x_prev + (2*d1 - d2 - d3)/3*cos(theta_prev) - (-d2+d3)*0.5773*sin(theta_prev);
        y = y_prev + (2*d1 - d2 - d3)/3*sin(theta_prev) + (-d2+d3)*0.5773*cos(theta_prev);
        theta = theta_prev + (d1 + d2 + d3)/(3*L); //     //   0.132629 => 180 / (3. L. pi)
        
        Vx = (x - x_prev)/0.002;
        Vy = (y - y_prev)/0.002;
        W = (theta - theta_prev)/0.002;
        
        enc1.reset();
        enc2.reset();
        enc3.reset();
        
        Thread::wait(Ts); //frekuensi sampling = 500 Hz
    }
}

void PTP_movement(){
    //mapping lokasi
    float map_x[16]     = {  0,150,300,450,600,600,600,600,600,450,300,150,  0,  0,  0,  0};
    float map_y[16]     = {  0,  0,  0,  0,  0,150,300,450,600,600,600,600,600,450,300,150};
    //float map_theta[16] = {  0,  0,  0, 90, 90, 90, 90,180,180,180,180,270,270,270,270,360};
    float thet=0;
    float thet_prev = 0;
    int i=0;

    while(i < 16){
        thet = thet_prev + 90*(i/4);
        i += calculate_PID(&map_x[i],&map_y[i],&thet,false);
        
        if (i == 16){
            i = 0;
            thet_prev = thet;
        }

        Thread::wait(Ts);
    }
}

int calculate_PID(float *x_set, float *y_set, float *theta_set, bool isLast){
    // variabel tambahan
    static float S_error_prev = 0;
    static float theta_error_prev = 0;
    
    static float sum_S_error = 0;
    static float sum_theta_error = 0;
    
    //menghitung error posisi
    float S_error = sqrt((*x_set-x)*(*x_set-x) + (*y_set-y)*(*y_set-y));
    //menghitung error arah
    float theta_error = *theta_set - theta*RAD_TO_DEG;
    
    sum_S_error += S_error;
    sum_theta_error += theta_error;
     
    float vs = Kp_s*S_error + Ki_s*Ts*sum_S_error + Kd_s*(S_error - S_error_prev)/Ts;
    float w = Kp_w*theta_error + Ki_w*Ts*sum_theta_error + Kd_w*(theta_error - theta_error_prev)/Ts;
    
    vr = vs/MAX_SPEED*0.5;
    vw = w*L/MAX_W_SPEED*0.5;
    a = moving_direction(*x_set,*y_set,x,y,theta);
    
    S_error_prev = S_error;
    theta_error_prev = theta_error;
    
    if(isLast == true){
        if ((abs(*x_set - x) < 20) && (abs(*y_set - y) < 20)){
               vw = 0;
               vr = 0;
                return 1;
        }
        else return 0;
    }
    else{
        if ((abs(*x_set - x) < BOUNDARY_TOLERANCE) && (abs(*y_set - y) < BOUNDARY_TOLERANCE))   return 1;
        else return 0;
    }      
}


float moving_direction( float xs, float ys, float x, float y,float theta){
    float temp = atan((ys - y)/(xs - x)) - theta;
    
    if (xs < x)    return temp + PI;
    else            return temp;
}


void motor_out() {      
        Thread::wait(1500);   
            
        while(1){
           motor1.speed(SPEED*(vr*cos(a) + vw));
           motor2.speed(SPEED*(vr*(-0.5*cos(a) - 0.866*sin(a)) + vw));
           motor3.speed(SPEED*(vr*(-0.5*cos(a) + 0.866*sin(a)) + vw));
        }
}