File content as of revision 6:7fce519e4976:

/*********************************************************************************************/
/**     FILE HEADER                                                                         **/
/*********************************************************************************************/
#include "mbed.h"
#include "Motor.h"
#include "PS_PAD.h"
#include "esc.h"
#include "Servo.h"
#include "Ping.h"

/*********************************************************************************************/
/**     DEKLARASI INPUT OUTPUT                                                              **/
/*********************************************************************************************/
//serial
Serial pc(USBTX,USBRX);     //debug
Serial com(PA_0,PA_1);      //sensor

//joystick PS2
PS_PAD ps2(PB_15,PB_14,PB_13, PC_4); //(mosi, miso, sck, ss) 

//motor (PWM, forward, reverse)
Motor motor1(PA_8, PB_0, PC_15);
Motor motor2(PA_11, PA_6, PA_5);
Motor motor3(PA_9, PC_2, PC_3);
Motor motor4(PA_10, PB_5, PB_4);
Motor motorC2(PB_7, PA_14, PA_15);
Motor motorC1(PB_9, PA_12, PC_5);
Motor motorS(PB_8, PB_1, PA_13);
//Motor motor4(PB_6, PA_7, PB_12);

//pnuematik
DigitalOut pnuematik1(PC_11);
DigitalOut pnuematik2(PC_10);
DigitalOut pnuematik3(PD_2);
DigitalOut pnuematik4(PC_12);

//Limit Switch
DigitalIn limit1(PC_13 ,PullUp);
DigitalIn limit2(PC_14 ,PullUp);
DigitalIn limit3(PC_1 ,PullUp);
DigitalIn limit4(PC_0 ,PullUp);

DigitalIn field(PB_10 ,PullUp);

//laser pointer
DigitalOut laser(PB_3);

//servo(PWM)
Servo servoEDF(PC_8);

//EDF(PWM, timer)
ESC edf(PC_6,20);

//Timer
Ticker timer;

//Sensor Ping
Ping ping(PB_2);

/*********************************************************************************************/
/**     DEKLARASI VARIABEL, PROSEDUR, DAN FUNGSI                                                       **/
/*********************************************************************************************/ 
const float gMax_speed = 0.85;
//const float gMin_speed = 0.1;
const float v0 = 0.4;
const float ax = 0.0007;
float vcurr = v0;

const float tuning1 = 0.0;
const float tuning2 = 0.04;
const float tuning3 = 0.0;
const float tuning4 = 0.04;

const float driver0 = 1;
const float driver1 = 0.82;
const float driver2 = 0.6;
const float driver3 = 0.15;


// inisialisasi pwm awal servo
double pwm = 0.00;
 
// inisialisasi sudut awal
double sudut = -85;

// variabel kondisi pnuematik
int g = 1;

//slider auto
int c =0;  
int batas_delay = 270;

//data sensor garis dan line following
int datasensor[6];
int over;
int g_flag;


void aktuator(int f);
void edf_servo();

void init_sensor();
void linetracer(float speed);
void flag_sensor();

float read_jarak();

/*********************************************************************************************/
/*********************************************************************************************/
/**     PROGRAM UTAMA                                                                       **/
/*********************************************************************************************/
/*********************************************************************************************/ 
int main() {
    //inisiasi serial
    pc.baud(115200);
    com.baud(115200);
    
    if(field == 1)      sudut = -85;
    else        sudut = 85;
    
    //inisiasi joystick
    ps2.init();
    
    //set inisiasi servo pada posisi 0 
    servoEDF.position(sudut);

    //set edf pada posisi bukan kalibrasi, yaitu set edf 0
    edf.setThrottle(0);
    edf.pulse();
    
    //inisisasi laser
    laser = 1;
    
    //inisiasi pnuematik
    pnuematik1 = 1;
    pnuematik2 = 1;
    pnuematik3 = 1;
    pnuematik4 = 1;
    
    //inisisasi TIMER
    timer.attach(&flag_sensor,0.0005);
    timer.detach();
    
    //kondisi robot
    bool manual=true; 
    bool pool= false;
    
    //running manual
    while(manual){
        
        ps2.poll();
        
        aktuator(field);
        edf_servo();
        
        if(limit4==0)    manual = false;
        
    }
    
    //running otomatis
    timer.attach(&flag_sensor,0.0005);
    
    while(!pool){
        init_sensor();
        
        if(vcurr > 0.4)  vcurr = (float) 0.4;
        //else if (vcurr < 0.2)   vcurr = (float) 0.2;
      
        linetracer(vcurr);
        //laser = 1;
            
        if((limit3==0) || (read_jarak() <= 9.0))   pool=true;
        
        //if(read_jarak() <= 45.0)        vcurr -= ax;
        //else                        vcurr += ax;
        vcurr += ax;
        
        for(int i=0; i<6; i++){
            printf("%d  ",datasensor[i]);
        }
        printf("%d  \n",g_flag);
        
    }
    motor1.brake(0);
    motor2.brake(0);
    motor3.brake(0);
    motor4.brake(0);
    
    timer.detach();
        
    pnuematik1=0;
    wait_ms(1500);
    
    while(limit4!=0){
        motorC1.speed(0.95);
        motorC2.speed(-1);
    }
    
    motorC1.brake(1);
    motorC2.brake(1);
    
    if(field==1){
        pnuematik3 = 0;
        wait_ms(1500);
        pnuematik2 = 1;
        wait_ms(500);
        pnuematik3 = 1;
    }
    else{
        pnuematik4 = 0;
        wait_ms(1500);
        pnuematik2 = 1;
        wait_ms(500);
        pnuematik4 = 1;
    }
    
    return 0;
}

/*********************************************************************************************/
/**     ALGORITMA PROSEDUR DAN FUNGSI                                                       **/
/*********************************************************************************************/ 
void aktuator(int f){
    float speed = vcurr;
    
    if(vcurr >= gMax_speed)    vcurr = gMax_speed;
    
    if(f == 1){
        if((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){
            //pivot kiri                      
            motor2.speed((float)0.5*(speed-tuning2));
            motor4.speed((float)-0.5*(speed-tuning4));
            motor1.speed((float)0.5*(speed-tuning1));
            motor3.speed((float)-0.5*(speed-tuning3));
            pc.printf("pivot kiri \n");
            
            vcurr += ax;
        }
        else if((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){
            //pivot kanan                      
            motor2.speed((float)-0.5*(speed-tuning2));
            motor4.speed((float)0.5*(speed-tuning4));
            motor1.speed((float)-0.5*(speed-tuning1));
            motor3.speed((float)0.5*(speed-tuning3));
            pc.printf("pivot kanan \n");
            
            vcurr += ax;
        }
        else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_TOP)==1)){
            //serong atas kanan                      
            motor2.speed(speed-tuning2);
            motor4.brake(1);
            motor1.brake(1);
            motor3.speed(speed-tuning3);
            pc.printf("serong atas kanan \n");
            
            vcurr += ax;
        }
        else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
            //serong atas kiri                       
            motor2.brake(1);
            motor4.speed(-(speed-tuning4));
            motor1.speed(-(speed-tuning1));
            motor3.brake(1);
            pc.printf("serong atas kiri \n");
            
            vcurr += ax;
        }
        else if((ps2.read(PS_PAD::PAD_LEFT)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==1)){
            //serong bawah kiri                     
            motor2.speed(-(speed-tuning2));
            motor4.brake(1);
            motor1.brake(1);
            motor3.speed(-(speed-tuning3));
            pc.printf("serong bawah kiri \n");
            
            vcurr += ax;
        }
        else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_RIGHT)==1)){
            //serong bawah kanan                     
            motor2.brake(1);
            motor4.speed(speed-tuning4);
            motor1.speed(speed-tuning1);
            motor3.brake(1);
            pc.printf("serong bawah kanan \n");
            
            vcurr += ax;
        }
        else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){
            //maju
            motor1.speed(-(speed-tuning1));
            motor3.speed(speed-tuning3);
            motor2.speed(speed-tuning2);
            motor4.speed(-(speed-tuning4));
            pc.printf("maju \n");
            
            vcurr += ax;
        }
        else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){
            //mundur     
            motor1.speed(speed-tuning1);
            motor3.speed(-(speed-tuning3));
            motor2.speed(-(speed-tuning2));
            motor4.speed(speed-tuning4);
            pc.printf("mundur \n");
            
            vcurr += ax;
        } 
        else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_LEFT)==0)){
            //kanan                     
            motor2.speed(speed-tuning2);
            motor4.speed(speed-tuning4);
            motor1.speed(speed-tuning1);
            motor3.speed(speed-tuning3);
            pc.printf("kanan \n");
            
            vcurr += ax;
        }
        else if((ps2.read(PS_PAD::PAD_RIGHT)==0) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
            //kiri                       
            motor2.speed(-(speed-tuning2));
            motor4.speed(-(speed-tuning4));
            motor1.speed(-(speed-tuning1));
            motor3.speed(-(speed-tuning3));
            pc.printf("kiri \n");
            
            vcurr += ax;
        }
        else{
            motor1.brake(1);
            motor3.brake(1);
            motor2.brake(1);
            motor4.brake(1);
            pc.printf("diam \n");
            
            vcurr = v0;
        }
    }
    else{
        if((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){
            //pivot kiri                      
            motor2.speed((float)0.5*(speed-tuning2));
            motor4.speed((float)-0.5*(speed-tuning4));
            motor1.speed((float)0.5*(speed-tuning1));
            motor3.speed((float)-0.5*(speed-tuning3));
            pc.printf("pivot kiri \n");
            
            vcurr += ax;
        }
        else if((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){
            //pivot kanan                      
            motor2.speed((float)-0.5*(speed-tuning2));
            motor4.speed((float)0.5*(speed-tuning4));
            motor1.speed((float)-0.5*(speed-tuning1));
            motor3.speed((float)0.5*(speed-tuning3));
            pc.printf("pivot kanan \n");
            
            vcurr += ax;
        }
        else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_TOP)==1)){
            //serong atas kanan                                  
            motor2.speed(-(speed-tuning2));
            motor4.brake(1);
            motor1.brake(1);
            motor3.speed(-(speed-tuning3));
            pc.printf("serong atas kanan \n");
            
            vcurr += ax;
        }
        else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
            //serong atas kiri                                   
            motor2.brake(1);
            motor4.speed(speed-tuning4);
            motor1.speed(speed-tuning1);
            motor3.brake(1);
            pc.printf("serong atas kiri \n");
            
            vcurr += ax;
        }
        else if((ps2.read(PS_PAD::PAD_LEFT)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==1)){
            //serong bawah kiri 
            motor2.speed(speed-tuning2);
            motor4.brake(1);
            motor1.brake(1);
            motor3.speed(speed-tuning3);                    
            pc.printf("serong bawah kiri \n");
            
            vcurr += ax;
        }
        else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_RIGHT)==1)){
            //serong bawah kanan                     
            motor2.brake(1);
            motor4.speed(-(speed-tuning4));
            motor1.speed(-(speed-tuning1));
            motor3.brake(1);
            pc.printf("serong bawah kanan \n");
            
            vcurr += ax;
        }
        else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){
            //maju
            motor1.speed(speed-tuning1);
            motor3.speed(-(speed-tuning3));
            motor2.speed(-(speed-tuning2));
            motor4.speed(speed-tuning4);
            pc.printf("maju \n");
            
            vcurr += ax;
        }
        else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){
            //mundur     
            motor1.speed(-(speed-tuning1));
            motor3.speed(speed-tuning3);
            motor2.speed(speed-tuning2);
            motor4.speed(-(speed-tuning4));
            pc.printf("mundur \n");
            
            vcurr += ax;
        } 
        else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_LEFT)==0)){
            //kanan                     
            motor2.speed(-(speed-tuning2));
            motor4.speed(-(speed-tuning4));
            motor1.speed(-(speed-tuning1));
            motor3.speed(-(speed-tuning3));
            pc.printf("kanan \n");
            
            vcurr += ax;
        }
        else if((ps2.read(PS_PAD::PAD_RIGHT)==0) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
            //kiri                                 
            motor2.speed(speed-tuning2);
            motor4.speed(speed-tuning4);
            motor1.speed(speed-tuning1);
            motor3.speed(speed-tuning3);
            pc.printf("kiri \n");
            
            vcurr += ax;
        }
        else{
            motor1.brake(1);
            motor3.brake(1);
            motor2.brake(1);
            motor4.brake(1);
            pc.printf("diam \n");
            
            vcurr = v0;
        }
    
    }    
        
    if((ps2.read(PS_PAD::PAD_CIRCLE)==0) && (ps2.read(PS_PAD::PAD_TRIANGLE)==1)){
            //POWER WINDOW ATAS
        motorS.speed(1);
        if (limit1 == 0){
            motorS.brake(1);
        }
            
            
        pc.printf("up \n");
        c++;
    }
    else if((ps2.read(PS_PAD::PAD_CIRCLE)==1) && (ps2.read(PS_PAD::PAD_TRIANGLE)==0)){
            //POWER WINDOW BAWAH        
        motorS.speed(-0.5);
            
        if (limit2 ==0){
            motorS.brake(1);
        }
            
        pc.printf("down \n");
        c--;        
    }
    else{
        motorS.brake(1);
        if ((c <= batas_delay) && (c>=-batas_delay)){
            c=0;
        }
            
        pc.printf("diam \n");
    }           
        
    if((c > batas_delay) && (limit1 == 0)){
        c = 0;
        motorS.brake(1);
    }
    else if((c < -batas_delay) && (limit2 == 0)){
        c = 0;
        motorS.brake(1);
    }
    else if( (c > batas_delay) && (limit1 != 0)){
        motorS.speed(1);
    }
    else if ((c<-batas_delay) && (limit2 != 0)){
        motorS.speed(-0.7);
    }
        
        
    if ((ps2.read(PS_PAD::PAD_SELECT)==1))
    {
            //mekanisme ambil gripper
            pc.printf("mekanisme gripper");
            if (g==1){
                pc.printf("ambil 1");
                pnuematik2 = 0;
                g=2;
                wait_ms(400);
            }
            else
            {
                pnuematik2 = 1;
                wait_ms(400);
                g=1;
            }
    }
}

void edf_servo(){
        if(ps2.read(PS_PAD::PAD_X)==1){
            //PWM ++
            pwm += 0.0007;
            if( pwm > 0.8)  pwm = 0.8;
            pc.printf("gaspol \n");
        }
        else if(ps2.read(PS_PAD::PAD_SQUARE)==1){
            //PWM--
            pwm -= 0.0007;
            
            if(pwm < 0)    pwm = 0.0;
            pc.printf("rem ndeng \n");
        }
        
        if(ps2.read(PS_PAD::PAD_R2)==1){
            //SERVO --
            sudut += 0.5;
            
            if(sudut > 90)  sudut = 90;
            pc.printf("servo max \n");
        }
        else if(ps2.read(PS_PAD::PAD_L2)==1){
            //SERVO ++
            sudut -= 0.5;
            
            if(sudut < -90) sudut = -90;
            pc.printf("servo min \n");
        }
        
        if(ps2.read(PS_PAD::PAD_START)==1){
            
            sudut = 0;
            pwm = 0.22;
        }
        
        
        servoEDF.position((float)sudut);
        edf.setThrottle((float)pwm);
        edf.pulse();
}

/////////////////////////////////////////LINE FOLLOWER/////////////////////////

void init_sensor(){
    char data;
    if(com.readable()){  
        data = com.getc();
        
        for(int i=0; i<6; i++){
           datasensor[i] = (data >> i) & 1;
       }
    }
}

void linetracer(float speed){    
    float speed1,speed2,speed3,speed4;
    
        //////////////////logic dari PV (present Value)/////////////////////////
        if(datasensor[2] && datasensor[3]){
            speed1 = speed*driver0;
            speed2 = speed*driver0;
            speed3 = speed*driver0;
            speed4 = speed*driver0;
        }
        else if(datasensor[2]){
            speed1 = speed*driver1;
            speed2 = speed*driver0;
            speed3 = speed*driver0;
            speed4 = speed*driver1;
        }
        else if(datasensor[3]){
            speed1 = speed*driver0;
            speed2 = speed*driver1;
            speed3 = speed*driver1;
            speed4 = speed*driver0;
        }
        else if(datasensor[1]){
            speed1 = speed*driver2;
            speed2 = speed*driver0;
            speed3 = speed*driver0;
            speed4 = speed*driver2;
        }
        else if(datasensor[4]){
            speed1 = speed*driver0;
            speed2 = speed*driver2;
            speed3 = speed*driver2;
            speed4 = speed*driver0;
        }
        else if(datasensor[0]){
            speed1 = speed*driver3;
            speed2 = speed*driver0;
            speed3 = speed*driver0;
            speed4 = speed*driver3;
        }
        else if(datasensor[5]){
            speed1 = speed*driver0;
            speed2 = speed*driver3;
            speed3 = speed*driver3;
            speed4 = speed*driver0;
        }
        
        else
        {
            if(g_flag == 0){
                speed1 = speed*driver0;
                speed2 = speed*driver0;
                speed3 = speed*driver0;
                speed4 = speed*driver0;
            }
            else if(g_flag == 3){
                speed1 = speed*driver1;
                speed2 = speed*driver0;
                speed3 = speed*driver0;
                speed4 = speed*driver1;
            }
            else if(g_flag == 4){
                speed1 = speed*driver0;
                speed2 = speed*driver1;
                speed3 = speed*driver1;
                speed4 = speed*driver0;
            }
            else if(g_flag == 2){
                speed1 = speed*driver2;
                speed2 = speed*driver0;
                speed3 = speed*driver0;
                speed4 = speed*driver2;
            }
            else if(g_flag == 5){
                speed1 = speed*driver0;
                speed2 = speed*driver2;
                speed3 = speed*driver2;
                speed4 = speed*driver0;
            }
            else if(g_flag == 1){
                speed1 = -speed*driver2;
                speed2 = speed*driver2;
                speed3 = speed*driver2;
                speed4 = -speed*driver2;
            }
            else if(g_flag == 6){
                speed1 = speed*driver2;
                speed2 = -speed*driver2;
                speed3 = -speed*driver2;
                speed4 = speed*driver2;    
            }
        } 
        
        motor1.speed(-(float)(speed1-0.04));
        motor2.speed(-(float)(speed2-0.04));
        motor3.speed(-(float)(speed3-0.0));
        motor4.speed(-(float)(speed4-0.0));

}

void flag_sensor(){
    if((datasensor[2] == 1) && (datasensor[3] == 1))    g_flag = 0;
    else if(datasensor[2] == 1)                         g_flag = 3;
    else if(datasensor[3] == 1)                         g_flag = 4;
    else if(datasensor[1] == 1)                         g_flag = 2;
    else if(datasensor[4] == 1)                         g_flag = 5;
    else if(datasensor[0] == 1)                         g_flag = 1;
    else if(datasensor[5] == 1)                         g_flag = 6;
}


////////////////////////SENSOR PING///////////////////////////////////////
float read_jarak() {
    float jarak;
    
    ping.Send();
    wait_ms(45);
    jarak = ping.Read_cm()/2;
    return jarak;
}