File content as of revision 0:dddc43348c25:

////////////////////////////////////////////////////////////////////////////////
// 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 "encoderKRTMI.h"
#include "JoystickPS3.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);

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

// Encoder
encoderKRTMI encoder_A(PIN_A_CHANNEL_A, PIN_A_CHANNEL_B, 540, encoderKRTMI::X4_ENCODING);
encoderKRTMI encoder_B(PIN_B_CHANNEL_A, PIN_B_CHANNEL_B, 540, encoderKRTMI::X4_ENCODING);
encoderKRTMI encoder_C(PIN_C_CHANNEL_A, PIN_C_CHANNEL_B, 540, encoderKRTMI::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 countCircle = 0;
int countSegitiga=0;

////////////////////////////////////////////////////////////////////////////////
// Main Program                                                               //
////////////////////////////////////////////////////////////////////////////////
int main(){
    stick.setup();
    stick.idle();

    while(1){
        if(stick.readable() ) {
            // Panggil fungsi pembacaan joystik
            stick.baca_data();
            // Panggil fungsi pengolahan data joystik
            stick.olah_data();
            if ( (!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)&&(!stick.L1)&&(!stick.R1) && (!stick.R2) && (!stick.L2) ){

                pc.printf("tidak ada input\n");
            } else {
               pc.printf("ada input\n");
            }
            
            
        }
        
   }/*
    while(1){
        //doing nothing
        
        pulse_A = encoder_C.getPulses();
        pc.printf("%.2f \n", pulse_A);
        if (pulse_A>10000)
            motor3.speed(0.0);
        else
            motor3.speed(-0.9);
        Thread::wait(2);
    }*/
}

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

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

    encoder_A.reset();
    encoder_B.reset();
    encoder_C.reset();
}

void hitungPID(double theta_s){
    //menghitung error theta
    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 Kecepatan Vw (kec rotasi)
    if (Vw > Vw_max){
        Vw = Vw_max;
    }
    else if ( Vw < -1*Vw_max){
        Vw = -1*Vw_max;
    }
}

void gerakMotor(){
    // Berhenti 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 > 100) && Vr < Vr_max){
                if (Vr < 0.39){
                    Vr = 0.39;
                }
                else if ( (Vr >= 0.39)&& (Vr<=0.45)){
                    Vr+= 0.05;
                } else if ((Vr>0.45) && (Vr<1.00) ){
                    Vr+=0.12;
                } else { 
                        Vr = 1.2;
                }
                last_mt_aksel = millis();
            }
        } else {
            Vr=0.42;
        }
        
        // 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("%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(){
//brake biasa
    motor1.speed(0);
    motor2.speed(0);
    motor2.speed(0);
}

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

void case_gerak(){
    if(stick.SELECT_click){
    //reset semua variable
        pneumatik[0]=1; //gripA
        pneumatik[1]=1; //gripB
        pneumatik[2]=1; //sliderA
        pneumatik[3]=1; //sliderB
        pn=0;
        pn2=0;
        pn3=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 (!stick.L1 && stick.R1){        // 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.25;
        else
            Vw = 0.17;
    }
    else if (!stick.R1 && stick.L1){   // 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.25;
        else
            Vw = -0.17;
    }
    

    if (stick.R2){
        // Mode Sprint
        Vmax=1.25;
    } else 
        Vmax=0.83;
        
    
    if ( (!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)&&(!stick.L1)&&(!stick.R1) && (!stick.R2) && (!stick.L2) ){
        count=0;
        brake_state=1;
        theta = 0.0;
        theta_prev = 0.0;
        theta_s = 0.0;
        theta_error_prev = 0;
        sum_theta_error = 0;
        theta_error = 0; 
    }else{
        if (count<200)
            count++;
        else 
            count=500;
        brake_state=0;
    }
    // Linier
    
        if ((!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)&&(stick.L2)){
            //L2 : serong kiri bawah + pivot kiri (mundur saat kasih SC
            a = (-22/RAD_TO_DEG); // mundur saat setelah pengambilan
            Vr_max = 0.84;
            Vw=-0.19;
        } 
        else if ((stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)&&(stick.L2)){
            a = (5/RAD_TO_DEG); // L2+atas : mundur saat setelah pengambilan
            Vr_max = 0.87;
            Vw=-0.05;
        } 
        else if ((stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)){
            a = (90/RAD_TO_DEG); // Maju
            Vr_max = Vmax;
        }
        else if ((!stick.atas)&&(stick.bawah)&&(!stick.kanan)&&(!stick.kiri)){
            a = (-96/RAD_TO_DEG); // Mundur
            Vr_max = Vmax;
        }
        else if ((stick.atas)&&(!stick.bawah)&&(!stick.kiri)&&(stick.kanan)){
            a = (135/RAD_TO_DEG); // Serong Atas Kanan
            Vr_max = Vmax;
        }
        else if ((!stick.atas)&&(stick.bawah)&&(!stick.kiri)&&(stick.kanan)){
            a = (-135/RAD_TO_DEG); // Serong Bawah Kanan
            Vr_max = Vmax;
        }
        else if ((stick.atas)&&(!stick.bawah)&&(stick.kiri)&&(!stick.kanan)){
            a = (45/RAD_TO_DEG); // Serong Atas Kiri
            Vr_max = Vmax;
        }
        else if ((!stick.atas)&&(stick.bawah)&&(stick.kiri)&&(!stick.kanan)){
            a = (-45/RAD_TO_DEG); // Serong Bawah Kiri
            Vr_max = Vmax;
        }
        else if ((!stick.atas)&&(!stick.bawah)&&(stick.kanan)&&(!stick.kiri)){
            a = (180/RAD_TO_DEG); // Kanan
            Vr_max = 0.8;
        }
        else if ((!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(stick.kiri)){
            a = (0/RAD_TO_DEG); // Kiri
            Vr_max = 0.8;
        }
        else {
            Vr_max = 0;
        }
    //}
}
void case_pneumatic(){
 // Control Pneumatic
 /**
 *  pneumatik[0] = Gripper A
 *  pneumatik[1] = Gripper B
 *  pneumatik[3] = Slider A
 *  pneumatik[4] = Slider B
 **/
    if((!stick.silang) && (!stick.kotak) && (!stick.lingkaran) && (!stick.segitiga)) {
    // reset state pneumatik
        countX=0;
        countCircle=0;
        countSegitiga=0;
        countKotak=0;
    } 
    if ((stick.silang && countX<50)) {
    //SILANG : sekuensial tiap tangan gripper
        countX++;
    }
    
    if (countX>0 && countX<100) {
    //state button X, lengan Kiri - lengakn Kanan
        countX=150;
        if (pn<4 && 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(50);
            pneumatik[2] = 1;
        } else if (pn==3){
            pneumatik[3]=0;
        } else if (pn==4){
            pneumatik[1] = 0; 
            wait_ms(50);
            pneumatik[3] = 1;
        }
        pn2=0;
        pn3=0;
    }
    
    if ((stick.kotak) && countKotak<50){
    //KOTAK : maju 2 glider 
        countKotak++;
    }
    
    if (countKotak>5 && countKotak<100){
    //state button KOTAK
        countKotak=250;
        pn=0;
        pn3=0;
        if (pn2<2 && 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[2]=1;
                pneumatik[3]=1;
            } 
        
    } 

    if ((stick.segitiga) && countSegitiga<50)
    //SEGITIGA :  buka tutup kedua gripper
        countSegitiga++;
    if (countSegitiga>4 && countSegitiga<100) {
    //state button SEGITIGA
        countSegitiga=250;
        if ( (pneumatik[0]==0 && pneumatik[1]==1) || (pneumatik[0]==1 && pneumatik[1]==0) ){
            pneumatik[0]=1;
            pneumatik[1]=1;
            pneumatik[2]=1;
            pneumatik[3]=1;
        } else { 
            pneumatik[0]=!pneumatik[0];
            pneumatik[1]=!pneumatik[1];
            pneumatik[2]=1;
            pneumatik[3]=1;
        }
        pn=0;
        pn2=0;
        pn3=0;
    }
        
    if ((stick.lingkaran) && countCircle<50){
    //KOTAK : maju 2 glider , 2 kali kotak 1 kali lingkaran
        countCircle++;
    }
    
    if (countCircle>4 && countCircle<100){
    //LINGKARAN : lengan kanan - lengan kiri 
        countCircle=250;
        pn=0;
        pn2=0;
        if (pn3<4 && pn3>=0)
            pn3++;
        else
            pn3=0;
        if(pn3==0 || pn3==22){
                pneumatik[0]=1;//gripA
                pneumatik[1]=1;//gripB
                pneumatik[2]=1;//sliderA
                pneumatik[3]=1;//sliderB
        } else if (pn3==1){
            pneumatik[3] = 0;
        } else  if (pn3==2){
            pneumatik[1] = 0; 
            wait_ms(50);
            pneumatik[3] = 1;
        } else if (pn3==3){
            pneumatik[2]=0;
        } else if (pn3==4){
            pneumatik[0] = 0; 
            wait_ms(50);
            pneumatik[2] = 1;
        }
        pn2=0;
    }  
        
}