File content as of revision 3:5f9fbb53c8d5:

/*********************************************************************************************/
/**     FILE HEADER                                                                         **/
/*********************************************************************************************/
#include "mbed.h"
#include "Motor.h"
#include "PS_PAD.h"
#include "PID.h"
#include "esc.h"
#include "Servo.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 motor4(PB_6, PA_7, PB_12);
Motor motor3(PA_9, PC_2, PC_3);
Motor motorS(PB_7, PA_14, PA_15);
Motor motorC1(PB_9, PA_12, PC_5);
Motor motorC2(PB_8, PB_1, PA_13);
Motor motor4(PA_10, PB_5, PB_4);

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

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

//PID line follower(P, I, D, Time Sampling)
PID PID(0.32,0.000,0.1,0.001);

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

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

//Timer Pnuematik
//Timer timer;



/*********************************************************************************************/
/**     DEKLARASI VARIABEL, PROSEDUR, DAN FUNGSI                                                       **/
/*********************************************************************************************/ 
const float gMax_speed = 0.8;
const float gMin_speed = 0.1;
/*const float tuning1 = 0.0;
const float tuning2 = 0.2;
const float tuning3 = 0.06;
const float tuning4 = 0.24;*/
const float v0 = 0.35;
const float ax = 0.0006;

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


float vcurr = v0;

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

///////
void aktuator();
void edf_servo();
void init_sensor();
void line(float speed);

void majulf(float i, float j, float k, float l);
void kirilf(float e, float f, float g, float h);
void kananlf(float a, float b, float c, float d);

/*********************************************************************************************/
/*********************************************************************************************/
/**     PROGRAM UTAMA                                                                       **/
/*********************************************************************************************/
/*********************************************************************************************/ 
int main() {
    //inisiasi serial
    pc.baud(115200);
    com.baud(115200);
    
    //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();
    
    //inisiasi pnuematik
    pnuematik1 = 1;
    pnuematik2 = 1;
    pnuematik3 = 1;
    pnuematik4 = 1;
    
    //inisiasi PID sensor    
    PID.setInputLimits(-15,15);
    PID.setOutputLimits(-1.0,1.0);
    PID.setMode(1.0);
    PID.setBias(0.0);
    PID.reset();
    
    //inisisasi TIMER
    //timer.start();
    
    //kondisi robot
    bool manual=true; 
    
    while(1)
    {
        ps2.poll();
        init_sensor();
        
        if(ps2.read(PS_PAD::PAD_X)==1){    
            line(0.35);
        }
        else{
            motor1.brake(1);
            motor2.brake(1);
            motor3.brake(1);
            motor4.brake(1);
            
            //v =0.1;
        }
            for(int i=0; i<6; i++){
                pc.printf("%d  ",datasensor[i]);
            }
            pc.printf("\n");

    }
    
    while(manual){
        
        ps2.poll();
        
        aktuator();
        edf_servo();
        
        if(limit3==0)    manual = false;
        
    }
    /*
    while(1){
        kananlf(0.1,0.1,0.1,0.1);
        wait_ms(3000);
        motor1.brake(1);
        motor2.brake(1);
        motor3.brake(1);
        motor4.brake(1);
        wait_ms(3000);
        kirilf(0.1,0.1,0.1,0.1);
        wait_ms(3000);
        motor1.brake(1);
        motor2.brake(1);
        motor3.brake(1);
        motor4.brake(1);
        wait_ms(3000);
        majulf(0.1,0.1,0.1,0.1);
        wait_ms(3000);   
        motor1.brake(1);
        motor2.brake(1);
        motor3.brake(1);
        motor4.brake(1);
        wait_ms(3000);
    }*/
    
    //bool lf = true;
    //int v_sensor;
    
    
    motor1.brake(1);
    motor2.brake(1);
    motor3.brake(1);
    motor4.brake(1);
    
    pnuematik1=0;
    wait_ms(1500);
    
    while(limit4!=0){
        motorC1.speed(1);
        motorC2.speed(-1);
    }
    
    motorC1.brake(1);
    motorC2.brake(1);
    
    pnuematik3 = 0;
    wait_ms(1500);
    pnuematik2 = 1;
    wait_ms(500);
    pnuematik3 = 1;
    
    return 0;
}

/*********************************************************************************************/
/**     ALGORITMA PROSEDUR DAN FUNGSI                                                       **/
/*********************************************************************************************/ 

void aktuator(){
    float speed = vcurr;
    
    if(vcurr >= gMax_speed)    vcurr = gMax_speed;
    
        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.7)  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.25;
        }
    
        servoEDF.position((float)sudut);
        edf.setThrottle((float)pwm);
        edf.pulse();
}

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

void line(float speed){    
    int pv;  
    float speed1,speed2,speed3,speed4;
         
        //////////////////logic dari PV (present Value)/////////////////////////
        if(datasensor[0]){
            pv = -5;
            over=1;
        }
        else if(datasensor[5]){
            pv = 5;
            over=2;
        }
        else if(datasensor[0] && datasensor[1]){
            pv = -3.5;
        }
        else if(datasensor[4] && datasensor[5]){
            pv = 3.5;
        }
        else if(datasensor[1]){
            pv = -2;
        }
        else if(datasensor[4]){
            pv = 2;
        }
        else if(datasensor[1] && datasensor[2]){
            pv = -1;
        }
        else if(datasensor[3] && datasensor[4]){
            pv = 1;
        }
        else if(datasensor[2]){
            pv = -0.5;
        }
        else if(datasensor[3]){
            pv = 0.5;
        }
        else if(datasensor[2] && datasensor[3]){
            pv = 0;
        }
        else
        {
            ///////////////// robot bergerak keluar dari sensor/////////////////////
            if(over==1){
                
                //motor1.brake(1);
                //motor4.brake(1);
                //wait_ms(100);
                
            }
            else if(over==2){
                
                //motor3.brake(1);
                //motor2.brake(1);
                //wait_ms(100);
            }
        } 
        /*
        if(pv > 1){
            //kanan
            speed1 = -(0.4-tuning1);
            speed2 = 0;
            speed3 = 0;
            speed4 = -(0.5-tuning4);   
            motor3.brake(1);
            motor2.brake(1);  
            motor1.speed(speed1);
            motor4.speed(speed4);  
        }
        else if(pv < -1){
            //kiri
            speed1 = 0;
            speed2 = -(0.4-tuning2);
            speed3 = -(0.4-tuning3);
            speed4 = 0;
            
            motor3.brake(1);
            motor2.speed(speed2);  
            motor1.speed(speed1);
            motor4.brake(1);
        }
        else{
            //gaspoll
            speed1 = -(0.7-tuning1);
            speed2 = -(0.7-tuning2);
            speed3 = -(0.7-tuning3);
            speed4 = -(0.7-tuning4);
            
            motor3.speed(speed3);
            motor2.speed(speed2);  
            motor1.speed(speed1);
            motor4.speed(speed4);
        } */
        PID.setProcessValue(pv);
        PID.setSetPoint(0);
        
        // memulai perhitungan PID
        //speed1 = speed - tuning1 + PID.compute();
        //speed2 = speed - tuning2 - PID.compute();
        //speed3 = speed - tuning3 - PID.compute();
        //speed4 = speed - tuning4 + PID.compute();
        
        speed1 = -0.74;
        speed2 = -0.74;
        speed3 = -0.8;
        speed4 = -0.8;

        /*if(speed1 > speed)      speed1 = speed;
        else if(speed1 < gMin_speed)    speed1 = gMin_speed;
        
        if(speed2 > speed)     speed2 = speed;
        else if(speed2 < gMin_speed)    speed2 = gMin_speed;
        
        if(speed3 > speed)      speed3 = speed;
        else if(speed3 < gMin_speed)    speed3 = gMin_speed;
        
        if(speed4 > speed)      speed4 = speed;
        else if(speed4 < gMin_speed)    speed4 = gMin_speed;
        */
        
        //motor3.speed(-speed3);
        //motor2.speed(-speed2);  
        //motor1.speed(-speed1);
        //motor4.speed(-speed4);
        
        motor3.speed(speed3);
        motor2.speed(speed2);  
        motor1.speed(speed1);
        motor4.speed(speed4);
}