File content as of revision 5:34be90fa6d27:

/*********************************************************************************************/
/**     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                                                        **/
/*********************************************************************************************/     


/*********************************************************************************************/
/*********************************************************************************************/
/**     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();
    
    //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_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); 
    }
}