File content as of revision 0:edddd373a163:

/*********************************************************************************************/
/**     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 motor3(PB_6, PA_7, PB_12);
Motor motor3(PA_9, PC_2, PC_3);
Motor motor4(PB_7, PA_14, PA_15);
Motor motorC1(PB_9, PA_12, PC_5);
Motor motorC2(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.992,0.000,0.81,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                                                       **/
/*********************************************************************************************/ 
float gMax_speed = 1;
float v0 = 0.6;
float ax = 0.0007;

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

// variabel kondisi pnuematik
int g = 1;

///////
void aktuator();
void edf_servo();
/*********************************************************************************************/
/*********************************************************************************************/
/**     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(manual){
        
        ps2.poll();
        
        aktuator();
        edf_servo();
        
        if(limit3==0)    manual = false;
        
    }
    
    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 = v0;
    float tuning = 0.01;
        if((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){
            //pivot kiri                      
            motor2.speed(speed*0.7);
            motor4.speed(-speed*0.7);
            motor1.speed(speed*0.7-tuning);
            motor3.speed(-(speed*0.7-tuning));
            pc.printf("pivot kiri \n");
            
            v0 += ax;
        }
        else if((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){
            //pivot kanan                      
            motor2.speed(-speed*0.7);
            motor4.speed(speed*0.7);
            motor1.speed(-(speed*0.7-tuning));
            motor3.speed(speed*0.7-tuning);
            pc.printf("pivot kanan \n");
            
            v0 += ax;
        }
        else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_TOP)==1)){
            //serong atas kanan                      
            motor2.speed(speed);
            motor4.brake(1);
            motor1.brake(1);
            motor3.speed(speed-tuning);
            pc.printf("serong atas kanan \n");
            
            v0 += 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);
            motor1.speed(-(speed-tuning));
            motor3.brake(1);
            pc.printf("serong atas kiri \n");
            
            v0 += ax;
        }
        else if((ps2.read(PS_PAD::PAD_LEFT)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==1)){
            //serong bawah kiri                     
            motor2.speed(-speed);
            motor4.brake(1);
            motor1.brake(1);
            motor3.speed(-(speed-tuning));
            pc.printf("serong bawah kiri \n");
            
            v0 += 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);
            motor1.speed(speed-tuning);
            motor3.brake(1);
            pc.printf("serong bawah kanan \n");
            
            v0 += ax;
        }
        else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){
            //maju
            motor1.speed(-(speed-tuning));
            motor3.speed(speed-tuning);
            motor2.speed(speed);
            motor4.speed(-speed);
            pc.printf("maju \n");
            
            v0 += ax;
        }
        else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){
            //mundur     
            motor1.speed(speed-tuning);
            motor3.speed(-(speed-tuning));
            motor2.speed(-speed);
            motor4.speed(speed);
            pc.printf("mundur \n");
            
            v0 += ax;
        } 
        else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_LEFT)==0)){
            //kanan                     
            motor2.speed(speed);
            motor4.speed(speed);
            motor1.speed(speed);
            motor3.speed(speed);
            pc.printf("kanan \n");
            
            v0 += ax;
        }
        else if((ps2.read(PS_PAD::PAD_RIGHT)==0) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
            //kiri                       
            motor2.speed(-speed);
            motor4.speed(-speed);
            motor1.speed(-speed);
            motor3.speed(-speed);
            pc.printf("kiri \n");
            
            v0 += ax;
        }
        else{
            motor1.brake(1);
            motor3.brake(1);
            motor2.brake(1);
            motor4.brake(1);
            pc.printf("diam \n");
            
            v0 = 0.6;
        }
        
        if((ps2.read(PS_PAD::PAD_CIRCLE)==0) && (ps2.read(PS_PAD::PAD_TRIANGLE)==1)){
            //POWER WINDOW ATAS
            motorS.speed(1);
            pc.printf("up \n");
        }
        else if((ps2.read(PS_PAD::PAD_CIRCLE)==1) && (ps2.read(PS_PAD::PAD_TRIANGLE)==0)){
            //POWER WINDOW BAWAH        
            motorS.speed(-1);
            pc.printf("down \n");
        }
        else{
            motorS.brake(1);
            pc.printf("diam \n");
            
        }        
        
        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.002;
            if( pwm > 0.7)  pwm = 0.7;
            pc.printf("gaspol \n");
        }
        else if(ps2.read(PS_PAD::PAD_SQUARE)==1){
            //PWM--
            pwm -= 0.002;
            
            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();
}