File content as of revision 7:f139bb3b2401:

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

#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

//STATE dari Sistem
#define state_Idle 1
#define state_ManualControl 2
#define state_AutoControl 3

#define BUTTON_offAwal 6
#define BUTTON_onAwal 7
#define BUTTON_onEnd 8

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

/*------------Variabel Global----------------*/
float x = 0;
float y = 0;
float theta = 0;
 
float Vr = 0;
float Vw = 0;
float a = 0;
 
float Vx = 0;
float Vy = 0;
float W = 0;

int state = state_Idle;
int stateSelect = BUTTON_offAwal;
int stateStart = BUTTON_offAwal; //state awal

typedef struct map {
    int n;
    float x_pos[16];
    float y_pos[16];
    float theta_pos[16];
} mapping;

const mapping map_square =  {16,
                            {  0,150,300,450,600,600,600,600,600,450,300,150,  0,  0,  0,  0},
                            {  0,  0,  0,  0,  0,150,300,450,600,600,600,600,600,450,300,150},
                            {  0,  0,  0,  0, 90, 90, 90, 90,180,180,180,180,270,270,270,270}};
const mapping map_triangle= {13,
                            {  0,150,300,450,600,525,450,375,300,225,150, 75,  0},
                            {  0,  0,  0,  0,  0,130,260,390,519,390,260,130,  0},
                            {  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}};
const mapping map_circle =  {16,
                            {0,114.88,212.17,277.17,300,277,211.83,114.44,0,-115.1,-212.34,-277.26,-300,-277,-212,-114.66},
                            {0,22.68,87.57,184.78,299.52,414.77,512,577.13,600,577.22,512.25,415,300.23,185.44,88.07,22.96},
                            {0,22.5,45,67.5, 90,112.5,135,157.5,180,202.5,225,247.5,270,292.5, 315, 337.5}};
const mapping map_NULL =    {0,
                            {  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0},
                            {  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0},
                            {  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}};
mapping map_data;

/*------------- DEKLARASI FUNGSI ----------------*/
void dataJoystick();
void lcd_out();
void self_localization();
void motor_out();
void PTP_movement();

void FSM(int input1, int input2, int *state);
int fsmButton(int input,int *stateButton);
int calculate_PID(float x_s, float y_s, float theta_s, bool isLast);
float moving_direction( float x_s, float y_s, float x, float y,float theta);
/*-----------------------------------------------*/

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

void dataJoystick(){
    while(true){
        ps2.poll();
        
        FSM(fsmButton(ps2.read(PS_PAD::PAD_SELECT),&stateSelect),fsmButton(ps2.read(PS_PAD::PAD_START),&stateStart), &state);
        
        if(state == state_ManualControl){
            Vr = 0.5;
            if(ps2.read(PS_PAD::PAD_LEFT))          a = 180/RAD_TO_DEG;
            else if(ps2.read(PS_PAD::PAD_BOTTOM))   a = -90/RAD_TO_DEG;
            else if(ps2.read(PS_PAD::PAD_RIGHT))    a = 0/RAD_TO_DEG;
            else if(ps2.read(PS_PAD::PAD_TOP))      a = 90/RAD_TO_DEG;
            else                                    Vr = 0;
            
            if(ps2.read(PS_PAD::PAD_L1))          Vw = 0.2;
            else if(ps2.read(PS_PAD::PAD_R1))     Vw = -0.2;
            else                                  Vw = 0.0;
        }
        else if(state == state_AutoControl){       
            if(ps2.read(PS_PAD::PAD_X)){
                map_data = map_NULL;
                Vr = 0;
                Vw = 0;
            }
            else if(ps2.read(PS_PAD::PAD_SQUARE)){
                map_data = map_square;
            }
            else if(ps2.read(PS_PAD::PAD_TRIANGLE)){
                map_data = map_triangle;
            }
            else if(ps2.read(PS_PAD::PAD_CIRCLE)){
                map_data = map_circle;
            }
                
        }
        else if(state == state_Idle){
        
        }
        
        Thread::wait(1);
    }
}

void FSM(int input1, int input2, int *state){
    switch(*state){
        case state_Idle :
            if (input1 == 1)   *state = state_ManualControl;
            else if (input2 == 1)   *state = state_AutoControl; 
        break;
        case state_ManualControl :
            if (input1 == 1)    *state = state_Idle;
        break;
        case state_AutoControl :
            if (input2 == 1)    *state = state_Idle;
        break; 
    }    
}

int fsmButton(int input,int *stateButton)
{
  switch (*stateButton)
  {
    case BUTTON_offAwal :
      if (input ==  1)
        *stateButton = BUTTON_onAwal;
      else
        *stateButton = BUTTON_offAwal;
       return 0;
    //break;
    case BUTTON_onAwal :
      if(input == 0)
        *stateButton = BUTTON_onEnd;
      else
        *stateButton = BUTTON_onAwal;
      return 0;
    //break;
    case BUTTON_onEnd :
       *stateButton = BUTTON_offAwal;
       return 1;
    //break;
  }
}

void lcd_out(){
    lcd.locate(0,0);
    lcd.printf("Sistem Pergerakan");
    lcd.locate(0,1);
    lcd.printf("Objek dengan 3 Roda");
    lcd.locate(0,2);
    lcd.printf("Berbasis Odometry"); 
    Thread::wait(1500);
    
    lcd.cls();         
    lcd.locate(0,0);
    lcd.printf("Bryan Christy P");
    lcd.locate(0,1);
    lcd.printf("   13214073");
    lcd.locate(0,2);
    lcd.printf("Rizqi Cahyo Y"); 
    lcd.locate(0,3);
    lcd.printf("   13214090");
    Thread::wait(1500);  
    while (true){
        lcd.cls();
        if (state == state_ManualControl){
            lcd.locate(0,0);
            lcd.printf("Mode : Manual");
            lcd.locate(0,1);
            lcd.printf("Vx = %.2f", Vx);
            lcd.locate(0,2);
            lcd.printf("Vy = %.2f", Vy);
            lcd.locate(0,3);
            lcd.printf("W = %.2f", W*RAD_TO_DEG);
        }
        else if (state == state_AutoControl){
            lcd.locate(0,0);
            lcd.printf("Mode : Autonomous");
            lcd.locate(0,1);
            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);
        } 
        else if(state == state_Idle){
            lcd.locate(0,0);
            lcd.printf("PAD_START");
            lcd.locate(0,1);
            lcd.printf(" => Mode Auto");
            lcd.locate(0,2);
            lcd.printf("PAD_SELECT");
            lcd.locate(0,3);
            lcd.printf(" => Mode Manual");
        }  
        Thread::wait(50);
    }
}

void self_localization(){
    static float x_prev = 0;
    static float y_prev = 0;
    static float theta_prev = 0;
    
    while(true){       
        float d1 = enc1.getPulses()*PULSE_TO_MM;
        float d2 = enc2.getPulses()*PULSE_TO_MM;
        float d3 = enc3.getPulses()*PULSE_TO_MM;
                
        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;
        
        x_prev = x;
        y_prev = y;
        theta_prev = theta;
        
        enc1.reset();
        enc2.reset();
        enc3.reset();
        
        if((state == state_AutoControl) && (map_data.n == 0)){
            x_prev = 0;
            y_prev = 0;
            theta_prev = 0;
        }        
        Thread::wait(Ts); //frekuensi sampling = 500 Hz
    }
}

void PTP_movement(){
    int i = 0;
    while(true){
        if(state == state_AutoControl){
            while(i < map_data.n){
                i += calculate_PID(map_data.x_pos[i],map_data.y_pos[i],map_data.theta_pos[i],(i==(map_data.n-1)));
                //if (i == map.n)    i = 0;
                Thread::wait(Ts);
            }
            i = 0;
        }
        else    i = 0;
        Thread::wait(Ts);
    }
}

int calculate_PID(float x_s, float y_s, float theta_s, bool isLast){
    //error_prev & sum_error
    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 jarak x,y terhaadap xs,ys
    float S_error = sqrt((x_s-x)*(x_s-x) + (y_s-y)*(y_s-y));
    //menghitung error arah
    float theta_error = theta_s - 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_s,y_s,x,y,theta);
    
    S_error_prev = S_error;
    theta_error_prev = theta_error;
    
    if(isLast == true){
        if ((abs(x_s - x) < 20) && (abs(y_s - y) < 20)){
            Vw = 0;
            Vr = 0;
            return 1;
        }
        else return 0;
    }
    else{
        if ((abs(x_s - x) < BOUNDARY_TOLERANCE) && (abs(y_s - y) < BOUNDARY_TOLERANCE))   return 1;
        else return 0;
    }      
}


float moving_direction( float x_s, float y_s, float x, float y,float theta){
    float temp = atan((y_s - y)/(x_s - x)) - theta;
    
    if (x_s < 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));
        
        if((Vr == 0) && (Vw == 0)){
           motor1.brake(BRAKE_HIGH);
           motor2.brake(BRAKE_HIGH);
           motor3.brake(BRAKE_HIGH);
        }
    }
}