File content as of revision 6:0e159860e5c6:

/*********************************************************************************************/
/**     FILE HEADER                                                                         **/
/*********************************************************************************************/
#include "mbed.h"
#include "Motor.h"
#include "NewTextLCD.h"
#include "PS_PAD.h"
#include "PID.h"
#include "millis.h"
#include "esc.h"
#include "Servo.h"


/*********************************************************************************************/
/**     DEKLARASI INPUT OUTPUT                                                              **/
/*********************************************************************************************/
// serial pc
Serial pc(USBTX,USBRX);

// LCD 20x4
//TextLCD lcd(PC_5, PB_1, PA_7, PC_4, PA_5, PA_6, TextLCD::LCD20x4); //(rs,e,d4,d5,d6,d7)

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


// tambahan power
// inisialisasi pwm awal servo
float pwm = 0.00;

// inisialisasi sudut awal
int sudut = 0;

//membatasi nilai brushless pada edf
float min=0;
float max=0.50;

// PID sensor garis 
PID PID(0.992,0.000,0.81,0.001); //(P,I,D, time sampling)

// Motor(pwm, fwd, rev)
//Motor motor2(PC_6, PC_8, PC_9);  //gripper
//Motor motor1(PA_15, PA_14, PA_13); //motor3
//Motor gripper(PB_5, PA_11, PA_12); 
//Motor motor2(PA_3, PC_15, PC_14); //kanan
//Motor motor1(PA_1, PH_0, PH_1); //kiri

Motor gripper(PA_10, PB_4, PB_5); // pwm, fwd, rev
Motor motor2(PA_0, PA_6, PA_7); // pwm, fwd, rev
//Motor motor3(PA_1, PH_1, PH_0); // pwm, fwd, rev
Motor motor1(PA_1, PC_9, PC_8); // pwm, fwd, rev

DigitalOut pnuematik_lengan(PC_10);
DigitalOut pnuematik_gripper(PC_11);
DigitalOut pnuematik_atas(PD_2);
DigitalOut pnuematik_bawah(PC_12);

Servo servo_gripper(PB_9);
//Deklarasi Input Limit Switch


// Sensor
DigitalIn S1(PC_0);
DigitalIn S2(PC_1);
DigitalIn S3(PC_2);
DigitalIn S4(PC_3);
DigitalIn S5(PA_0);
DigitalIn S6(PA_1);
DigitalIn S7(PA_4);
DigitalIn S8(PB_0);
DigitalIn S9(PB_2);
DigitalIn S10(PB_10);
DigitalIn S11(PA_10);
DigitalIn S12(PA_11);
DigitalIn S13(PA_12);
DigitalOut calibrate(PA_15);


//DigitalIn button(USER_BUTTON);


//bool sensor[13];

//DigitalIn limit_switch1(A0);
//DigitalIn limit_switch2(A1);


// Multitasker
//Ticker timer;


/*********************************************************************************************/
/**     DEKLARASI VARIABEL GLOBAL                                                           **/
/*********************************************************************************************/
float gMax_speed=0.7; //nilai maksimum kecepatan motor
float gMin_speed=-0.05;  //nilai minimum kecepatan motor
float gTuning = 0.16;

float gripper_up = 1;
float gripper_down = 0.2;

unsigned char gMode=0;  //variabel mode driving (manual = 0 otomatis = 1)
unsigned char gCase=0;  //variabel keadaan proses

unsigned char i; // variabel iterasi
int over=0;
int lapangan = 1;

bool state_atas = 0;
bool state_bawah = 0;
  
/*********************************************************************************************/
/**     DEKLARASI PROSEDUR DAN FUNGSI                                                        **/
/*********************************************************************************************/     
void init_sensor()
{
    if(ps2.read(PS_PAD::PAD_SQUARE)==1)
    {
        calibrate=0;
    }
    else
    {
            calibrate=1;
    }
     
    sensor[0]=(S1.read()==1);
    sensor[1]=(S2.read()==1);
    sensor[2]=(S3.read()==1);
    sensor[3]=(S4.read()==1);
    sensor[4]=(S5.read()==1);
    sensor[5]=(S6.read()==1);
    sensor[6]=(S7.read()==1);
    sensor[7]=(S8.read()==1);
    sensor[8]=(S9.read()==1);
    sensor[9]=(S10.read()==1);
    sensor[10]=(S11.read()==1);
    sensor[11]=(S12.read()==1);
    sensor[12]=(S13.read()==1);
}

void PIDrunning()   //menjalankan perintah untuk line follower
{  
    int pv;
    int over=0;
    float speedR,speedL;
    
    //init_sensor();
    //////////////////logic dari PV (present Value)/////////////////////////
    if(sensor[0]){
        pv = -12;
        over=1;
    }
    else if(sensor[12]){
        pv = 12;
        over=2;
    }
    else if(sensor[0] && sensor[1]){
        pv = -10;
    }
    else if(sensor[11] && sensor[12]){
        pv = 10;
    }
    else if(sensor[1]){
        pv = -9;
    }
    else if(sensor[11]){
        pv = 9;
    }
    else if(sensor[1] && sensor[2]){
        pv = -8;
    }
    else if(sensor[10] && sensor[11]){
        pv = 8;
    }
    else if(sensor[2]){
        pv = -7;
    }
    else if(sensor[10]){
        pv = 7;
    }
    else if(sensor[2] && sensor[3]){
        pv = -6;
    }
    else if(sensor[9] && sensor[10]){
        pv = 6;
    }
    else if(sensor[3]){
        pv = -5;
    }
    else if(sensor[9]){
        pv = 5;
    }
    else if(sensor[3] && sensor[4]){
        pv = -4;
    }
    else if(sensor[8] && sensor[9]){
        pv = 4;  
    }
    else if(sensor[4]){
        pv = -3;
    }
    else if(sensor[8]){
        pv = 3;
    }
    else if(sensor[4] && sensor[5]){
        pv = -2;
    }
    else if(sensor[7] && sensor[8]){
        pv = 2;
    }
    else if(sensor[5]){
        pv = -1;
    }  
     else if(sensor[7]){
        pv = 1;
    }
    else if(sensor[5] && sensor[6]){
        pv = -0.5;    
    }
    else if(sensor[6] && sensor[7]){
        pv = 0.5;
    } 
    else if (sensor[6]){
        pv = 0;
    }
    ///////////////// robot bergerak keluar dari sensor/////////////////////
        if(over==1){
            /*if(speed_ka > 0){
                wait_ms(10);
                motor2.speed(-speed_ka);
                wait_ms(10);
                }
            else{
                motor2.speed(speed_ka);
                }
            wait_ms(10);*/
            
            motor1.brake(1);
            //wait_ms(100);
            
        }
        else if(over==2){
            /*if(speed_ki > 0){
                wait_ms(10);
                motor1.speed(-speed_ki);
                wait_ms(10);
                }
            else{
                wait_ms(10);
                motor1.speed(speed_ki);
                wait_ms(10);
                }
            wait_ms(10);*/
            motor2.brake(1);
            //wait_ms(100);
        }
    
    PID.setProcessValue(pv);
    PID.setSetPoint(0);
    
    // memulai perhitungan PID

    speedR = gMax_speed - PID.compute();
    if(speedR > gMax_speed){
        speedR = gMax_speed;
        }
    else if(speedR < gMin_speed)
        speedR = gMin_speed;
    motor2.speed(speedR);

    speedL = gMax_speed + PID.compute();
    if(speedL > gMax_speed)
        speedL = gMax_speed;
    else if(speedL < gMin_speed)
        speedL = gMin_speed;
    motor1.speed(speedL);
}

/*********************************************************************************************/
/*********************************************************************************************/
/**     PROGRAM UTAMA                                                                       **/
/*********************************************************************************************/
/*********************************************************************************************/

    
void ambil(int c){
    switch (c)
    {
        case 1:    
            servo_gripper.position(170);
            pc.printf("grip lost");
            wait_ms(25);
            pnuematik_lengan=0;
            wait_ms(500);
            
        break;
        case 2:
            pnuematik_gripper=0;
            wait_ms(600);
            servo_gripper.position(50);
            pc.printf("grip get");
            wait_ms(25);
            wait_ms(1000); 
            pnuematik_gripper=1;
            wait_ms(800);
            pnuematik_lengan=1;
            wait_ms(200);
        break;
    }
}

void init_servo(int i){
    if (i){
        if (sudut>90){
            sudut=90;
            }
        if (sudut<0){
            sudut=0;
            }
    } else {
            
        if (sudut>0){
            sudut=0;
        }
        if (sudut<-90){
            sudut=-90;
        }    
    }
}

void init_pwm (){
    if (pwm>max){
        pwm = max;
    }
    
    if (pwm<min){
        pwm = min;
    }
}

int count=1;

int main(void){
    //inisialisasi joystick
    ps2.init();
    
    //inisialisasi PID
    PID.setInputLimits(-15,15);
    PID.setOutputLimits(-1.0,1.0);
    PID.setMode(1.0);
    PID.setBias(0.0);
    PID.reset();
    
    
    //tambahan power
    //ESC edf(PB_9,20); //p wm esc pin PC_7
    Servo myservo(PB_8); //pwm servo pin PA_8
    //set inisialisasi servo pada posisi 0 
    myservo.position(sudut);
    //set edf pada posisi bukan kalibrasi, yaitu set edf 0
    //edf.setThrottle(pwm);
    //edf.pulse();
    
    pc.baud(115200);
    float speed;
    
    servo_gripper.position(140);
    pnuematik_lengan=1;
    pnuematik_gripper=1;
    pnuematik_atas = state_atas;
    pnuematik_bawah = state_bawah;
    
    while(1)
    {  
    ps2.poll();
    
        if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){
            //MOTOR DEPAN
                    speed = gMax_speed;  
                    
                    motor1.speed(speed-gTuning);
                    motor2.speed(speed);
                    pc.printf("maju \n");
        }
        else if((ps2.read(PS_PAD::PAD_X)==1)&&(ps2.read(PS_PAD::PAD_TOP)==1)){
                    PIDrunning();
                    pc.printf("PID \t %f \t ",PID.compute());
                    
                    for(i=0;i<13;i++)
                    {
                        pc.printf("%i \t",sensor[i]);
                    }
                    pc.printf("\n");
        }        
        else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){
            //MOTOR BELAKANG
            speed = gMax_speed;
                        
            motor1.speed(-speed);
            motor2.speed(-speed);
            pc.printf("mundur \n");
        }
        else if((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){
            //pivot kiri
            speed = gMax_speed;
                        
            motor1.speed(-(speed*0.9-gTuning));
            motor2.speed(speed*0.9);
            pc.printf("kiri \n");
        }
        else if((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){
            //pivot kanan
            speed = gMax_speed;
                        
            motor1.speed(speed*0.9-gTuning);
            motor2.speed(-speed*0.9    );
            pc.printf("kanan \n");
        }
        else if ((ps2.read(PS_PAD::PAD_CIRCLE)==1))
        {
            //mekanisme ambil gripper
            pc.printf("mekanisme gripper");
            if (count==1){
                pc.printf("ambil 1");
                ambil(1);
                count=2;
                wait_ms(400);
            }
            else
            {
                ambil(2);
                count=1;
                pc.printf("ambil 2");
                wait_ms(400);
            }
        }
        else 
        {
            motor1.brake(1);
            motor2.brake(1);
        }
        
        if(ps2.read(PS_PAD::ANALOG_LX)<=-100){
            //SLIDER KIRI
            pc.printf("slide kiri \n");
        }
        else if(ps2.read(PS_PAD::ANALOG_LX)>=100){
            //SLIDER KANAN
            pc.printf("slide kanan \n");
        }
        else
        {
            pc.printf("slide diam \n");
        }
        
        if(ps2.read(PS_PAD::ANALOG_LY)<=-100){
            //POWER WINDOW ATAS
            
            gripper.speed(gripper_up);
            pc.printf("up \n");
        }
        else if(ps2.read(PS_PAD::ANALOG_LY)>=100){
            //POWER WINDOW BAWAH 
            if ((state_atas==1) || (state_bawah==1))
            {
                gripper_down = 1;
            }
            else
            {
                gripper_down = 0.2;
            }
                       
            gripper.speed(-gripper_down);
            pc.printf("down \n");
        }
        else
        {
            gripper.brake(1);
            pc.printf("power diam \n");
        }
        
        if(ps2.read(PS_PAD::ANALOG_RY)<=-100){
            //PWM ++
            pwm += 0.01;
            pc.printf("gaspol \n");
        }
        
        if(ps2.read(PS_PAD::ANALOG_RY)>=100){
            //PWM--
            pwm -= 0.01;
            pc.printf("rem ndeng \n");
        }
        
        if(ps2.read(PS_PAD::ANALOG_RX)<=-100){
            //SERVO --
            sudut -= 3;
            pc.printf("servo min \n");
        }
        
        if(ps2.read(PS_PAD::ANALOG_RX)>=100){
            //SERVO ++
            sudut += 3;
            pc.printf("servo max \n");
        }
        
        //gripper pole
        
        if ((ps2.read(PS_PAD::PAD_L2)==1))
        {
                
                state_atas = !state_atas;
                pnuematik_atas= state_atas;
                wait_ms(300);
        }
        else if ((ps2.read(PS_PAD::PAD_R2)==1))
        {
                state_bawah = !state_bawah;
                pnuematik_bawah = state_bawah;
                wait_ms(300);
        }
        
                        
        init_servo(lapangan);
        init_pwm();
        myservo.position(sudut);
        wait_ms(25);
        //edf.setThrottle(pwm);
        //edf.pulse();
        //wait_ms(25); 
    }
}