File content as of revision 0:8c6f29487664:

////////////////////////////////////////////////////////////////////////////////
// Robo Taker (Semi-Otomatis) 2018                                            //
// -------------------------------------------------------------------------- //
// Gerakan Robot:                                                             //
// - Arah   -> Gerak                                                          //
// - Silang -> Buka atau Tutup Gripper A                                      //
// - Lingkaran -> Buka atau Tutup Gripper B                                   //
// - Kotak -> Extend atau Shrink Slider A                                     //
// - Segitigas -> Extend atau Shrink Slider B                                 //
// - Start  -> Ubah Mode                                                      //
////////////////////////////////////////////////////////////////////////////////
#include "mbed.h"
#include "Motor.h"
#include "encoderKRAI.h"
#include "JoystickPS3.h"
#include "PS_PAD.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.7416027  //0.7656027 //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.8
#define KI_W 0
#define KD_W 140

#define MOTOR_LIMIT_MAX 1
#define MOTOR_LIMIT_MIN -1

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

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

//joystick PS2
PS_PAD ps2(PB_15,PB_14, PB_10, PB_12); //(mosi, miso, sck, ss) 
//PS_PAD ps2(PB_15,PB_14,PB_13, PC_4); //PB_13 udah dipake pin forward motor-_-

// Pneumatik
DigitalOut pneumatik[5]{PIN_PNEUMATIK_1, PIN_PNEUMATIK_2, PIN_PNEUMATIK_5, PIN_PNEUMATIK_6, PIN_PNEUMATIK_9};

// 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();
void motor_Stop();
void motor_ForceStop();
void gerak_Pneumatic();
void case_pneumatic();

////////////////////////////////////////////////////////////////////////////////
// Variable-variable                                                          //
////////////////////////////////////////////////////////////////////////////////
int joystick;
int pn = 0;
int pn2 = 0;
int pn3 = 0;
int pn_state = 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_print2, last_mt_pid, last_mt_rotasi;
unsigned long last_mt_aksel, last_mt_desel,last1;
bool modeauto = 1;
bool print_pulse = 0;
int brake_state=0;
double Vmax = 0.4;
double Vw_max = 0.3;
int force=0;
int count = 0;
int countX = 0;
int countKotak = 0;
int countSegitiga = 0;
int countCircle = 0;
int dudu;

////////////////////////////////////////////////////////////////////////////////
// Main Program                                                               //
////////////////////////////////////////////////////////////////////////////////
int main(){
    encoder_A.reset();
    encoder_B.reset();
    encoder_C.reset();
    
    pneumatik[0]=0; //gripA
    pneumatik[1]=0; //gripB
    pneumatik[2]=1; //sliderA
    pneumatik[3]=1; //sliderB
    
    startMillis();
    //stick.setup();
    //stick.idle();

    ps2.init();

    
    while(1){
        ps2.poll();
        if (millis() - last_mt_pid > TS){
                last_mt_pid = millis();
                hitungParameter();
                if (!(fabs(theta_s - (theta*RAD_TO_DEG))<TOLERANCET)  ){
                    hitungPID(theta_s);
                }
                //if (fabs(theta_s - (theta*RAD_TO_DEG))<TOLERANCET ){
//                    Vw = 0;
//                }
                if (DEBUG==1)
                    printPulse();
        }
        //gerakMotor();

        //if (millis() - last_mt_print2 > TS){
//               hitungParameter();
//               // printPulse();
//               pc.printf("%.2f\t%.2f\t%.2f\t%d\n",theta*RAD_TO_DEG, theta_s, a,modeauto);
//                last_mt_print2 = millis();
//            }
        //if(stick.readable() ) {
            // Panggil fungsi pembacaan joystik
            //stick.baca_data();
            // Panggil fungsi pengolahan data joystik
            //stick.olah_data();
            // Ambil data joystick
            case_gerak();
            case_pneumatic();

            // Pengatur Gerak Motor
            gerakMotor();

            // Program PID
            if (fabs(theta_s - (theta*RAD_TO_DEG))<TOLERANCET ){
                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);
    //theta2 = theta_prev2 + (pulse_A + pulse_C + pulse_B)/(3.0*L);

    //Update value
    x_prev = x;
    y_prev = y;
    theta_prev = theta;
    //theta_prev2 = theta2;

    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;

    //Limit vw
    if (Vw > Vw_max){
        Vw = Vw_max;
    }
    else if ( Vw < -1*Vw_max){
        Vw = -1*Vw_max;
    }
}

void gerakMotor(){
    // Berhent jika tidak maju, mundur, kiri, kanan, atau pivot
    if (brake_state == 1){
        motor_ForceStop();
        //motor_Stop();
    } else {  
        if (count>50){
            if ((millis() - last_mt_aksel > 60) && Vr < Vr_max){
                if (Vr < 0.35){
                    Vr = 0.35;
                }
                else if ( (Vr >= 0.35)&& (Vr<=0.45)){
                    Vr+= 0.025;
                } else if ((Vr>0.45) && (Vr<0.7) ){
                    Vr+=0.1;
                } else if ((Vr>=0.7) && (Vr<1.00)) {
                    Vr+=0.15;
                } else 
                    Vr = 1.00;
                last_mt_aksel = millis();
            }
        } else {
            Vr=0.4;
        }
        
        // 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("Up=%d \t Down=%d \t Left=%d \t Right=%d \t\n", (ps2.read(PS_PAD::PAD_TOP)), (ps2.read(PS_PAD::PAD_BOTTOM)), (ps2.read(PS_PAD::PAD_LEFT)), (ps2.read(PS_PAD::PAD_RIGHT)));
    //pc.printf("%d \n", pn);
    //pc.printf("%.2f\t%.2f\t%.2f\t%.2f\t%.2f\n", pulse_A, pulse_B, pulse_C, x, y);
    //pc.printf("%.2f\t%.2f\t%.2f\t%.2f\t%d\n",theta*RAD_TO_DEG, x, y, Vmax, modeauto);
}

void motor_Stop(){
    motor1.speed(0);
    motor2.speed(0);
    motor2.speed(0);
}

void motor_ForceStop(){
    motor1.brake(BRAKE_HIGH);
    motor2.brake(BRAKE_HIGH);
    motor3.brake(BRAKE_HIGH);
}

void gerak_Pneumatic(){
    if (pn_state==0){
        pneumatik[0] = 1; 
        pneumatik[1] = 1;
        pneumatik[2] = 1;
        pneumatik[3] = 1;
    } else if (pn_state==1){
        pneumatik[0] = 1; 
        pneumatik[1] = 1;
        pneumatik[2] = 0;
        pneumatik[3] = 1;
    } else if (pn_state==2){
        pneumatik[0] = 0; 
        pneumatik[1] = 1;
        wait_ms(100);
        pneumatik[2] = 1;
        pneumatik[3] = 1;
    } else if (pn_state==3){
        pneumatik[0] = 0; 
        pneumatik[1] = 1;
        pneumatik[2] = 1;
        pneumatik[3] = 0;
    } else if (pn_state==4){
        pneumatik[0] = 0; 
        pneumatik[1] = 0;
        wait_ms(100);
        pneumatik[2] = 1;
        pneumatik[3] = 1;
    } 
}
void case_gerak(){
    if(ps2.read(PS_PAD::PAD_SELECT)==1){
        pneumatik[0]=1; //gripA
        pneumatik[1]=1; //gripB
        pneumatik[2]=1; //sliderA
        pneumatik[3]=1; //sliderB
        pn=0;
        pn2=0;
        modeauto = 0;
        theta = 0.0;
        theta_prev = 0.0;
        theta_s = 0;
        theta_error_prev = 0;
        sum_theta_error = 0;
        theta_error = 0;
    }

    // Rotasi
    if ((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){        // Pivot Kanan
        //theta = 0.0;
        //theta_prev = 0.0;
        theta_s = theta*RAD_TO_DEG;
        theta_error_prev = 0;
        sum_theta_error = 0;
        theta_error = 0;
        if (Vr_max==0)
            Vw = 0.2;
        else
            Vw = 0.2;
    }
    else if ((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){   // Pivot Kiri
        //theta = 0.0;
        //theta_prev = 0.0;
        theta_s = theta*RAD_TO_DEG;
        theta_error_prev = 0;
        sum_theta_error = 0;
        theta_error = 0;
        if (Vr_max==0)
            Vw = -0.2;
        else
            Vw = -0.2;
    }

    if (ps2.read(PS_PAD::PAD_R2)==1){
        // Mode Sprint
        Vmax=1.0;
    } else {
        Vmax=0.8;
    }
    
    if ( (ps2.read(PS_PAD::PAD_TOP)==0)&&(ps2.read(PS_PAD::PAD_BOTTOM)==0)&&(ps2.read(PS_PAD::PAD_RIGHT)==0)&&(ps2.read(PS_PAD::PAD_LEFT)==0)&&(ps2.read(PS_PAD::PAD_L1)==0)&&(ps2.read(PS_PAD::PAD_R1)==0) && (ps2.read(PS_PAD::PAD_R2)==0)){
        count=0;
        brake_state=1;
    }else{
        if (count<200)
            count++;
        else 
            count=500;
        brake_state=0;
    }
    // Linier
    if ((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){
        a = (-90/RAD_TO_DEG); // Maju
        Vr_max = Vmax;
    }
    else if ((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){
        a = (90/RAD_TO_DEG); // Mundur
        Vr_max = Vmax;
    }
    else if ((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_TOP)==1)){
        a = (-45/RAD_TO_DEG); // Serong Atas Kanan
        Vr_max = Vmax;
    }
    else if ((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_RIGHT)==1)){
        a = (45/RAD_TO_DEG); // Serong Bawah Kanan
        Vr_max = Vmax;
    }
    else if ((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
        a = (-135/RAD_TO_DEG); // Serong Atas Kiri
        Vr_max = Vmax;
    }
    else if ((ps2.read(PS_PAD::PAD_LEFT)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==1)){
        a = (135/RAD_TO_DEG); // Serong Bawah Kiri
        Vr_max = Vmax;
    }
    else if ((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_LEFT)==0)){
        a = (0/RAD_TO_DEG); // Kanan
        Vr_max = Vmax;
    }
    else if ((ps2.read(PS_PAD::PAD_RIGHT)==0) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
        a = (180/RAD_TO_DEG); // Kiri
        Vr_max = Vmax;
    }
    else {
        Vr_max = 0;
    }
    

}
void case_pneumatic(){
 // Control Pneumatic
 
    if((ps2.read(PS_PAD::PAD_X)==0) && (ps2.read(PS_PAD::PAD_SQUARE)==0) && (ps2.read(PS_PAD::PAD_CIRCLE)==0) && (ps2.read(PS_PAD::PAD_TRIANGLE)==0)) {
        countX=0;
        countKotak=0;
        countSegitiga=0;
        countCircle=0;
    } else if ((ps2.read(PS_PAD::PAD_X)==1)) {
    //SILANG : sekuensial tiap tangan gripper
        countX++;
    }
    
    if (countX>5 && countX<=300) {
        pn2=0;
        countX=325;
        if (pn<5 && pn>=0)
            pn++;
        else    
            pn=0;

        if(pn==0 || pn==22){
                pneumatik[0]=1;//gripA
                pneumatik[1]=1;//gripB
                pneumatik[2]=1;//sliderA
                pneumatik[3]=1;//sliderB
        } else if (pn==1){
            pneumatik[2] = 0;
        } else  if (pn==2){
            pneumatik[0] = 0; 
            wait_ms(70);
            pneumatik[2] = 1;  
        } else if (pn==3){
            pneumatik[0]=1;
            pneumatik[2]=1;
        } else if(pn==4){
            pneumatik[3]=0;
        } else if (pn==5){
            pneumatik[1] = 0; 
            wait_ms(20);
            pneumatik[3] = 1;
        }

    }
    
    if (ps2.read(PS_PAD::PAD_SQUARE)==1){
    //KOTAK : maju 2 glider , 2 kali kotak 1 kali lingkaran
        countKotak++;
    }
    
    if (countKotak>5 && countKotak<300){
        pn=0;
        countKotak=325;
        if (pn2<5 && pn2>=0)
            pn2++;
        else
            pn2=0;
        
            if(pn2==0){
                pneumatik[0]=1;
                pneumatik[1]=1;
                pneumatik[2]=1;
                pneumatik[3]=1;
            } else if(pn2==1){
                pneumatik[2]=0;
                pneumatik[3]=0;
            } else if(pn2==2){
                pneumatik[0]=0;
                pneumatik[1]=0;
                wait_ms(50);
                pneumatik[0]=0;
                pneumatik[1]=0;
                pneumatik[2]=1;
                pneumatik[3]=1;
             } else if (pn2==3){
                 pneumatik[0]=0;
                 pneumatik[1]=0;
            }  
        
    } 

    if (ps2.read(PS_PAD::PAD_TRIANGLE)==1 && countSegitiga<400)
        countSegitiga++;
    if (countSegitiga>=7 && countSegitiga<300){
        //SEGITIGA :  buka tutup kedua gripper
        countSegitiga=500;
        pneumatik[0]=!pneumatik[0];
        pneumatik[1]=!pneumatik[1];
        pneumatik[2]=1;
        pneumatik[3]=1;
        pn=0;
        pn2=0;
    } 
        
        
    if (ps2.read(PS_PAD::PAD_CIRCLE)==1  && countCircle<400)
        countCircle++;
    if (countCircle>=5 && countCircle<200){
        //LINGKARAN : glider AB mundur, gripper AB buka, (kombinasikan dg kotak utk passing golde sc)
        countCircle=500;
        pneumatik[0]=0; //gripA
        pneumatik[1]=0; //gripB
        pneumatik[2]=1; //sliderA
        pneumatik[3]=1; //sliderB
        pn=0;
        pn2=0;
    }
     
}