File content as of revision 8:3cc68df2bebf:

/*********************************************************************************************/
/**     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"
#include "Ping.h"



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

//sensor ping
Ping pinger(PA_1);
//read jarak
int read_ping(){
    pinger.Send();
    wait_ms(30);
    return ((pinger.Read_cm())/2);
}

// joystick PS2
PS_PAD ps2(PB_15,PB_14,PB_13, PC_4); //(mosi, miso, sck, ss) default board lama pb_12
/*
// Motor(pwm, fwd, rev)
Motor gripper(PC_6, PC_8, PC_9); //PB_6, PB_8, PB_9
//Motor slider(PC_6, PC_9, PC_8);
Motor motor2(PB_3, PB_4, PB_5); //kanan
Motor motor1(PA_8, PC_7, PA_9); //kiri
*/
Motor gripper(PA_10, PB_3, PB_5); //PB_6, PB_8, PB_9
//Motor slider(PC_6, PC_9, PC_8);
Motor motor2(PA_11, PB_12, PA_7); //kanan
Motor motor1(PA_8, PB_4, PB_1); //kiri

DigitalOut limit0(PC_0,PullUp);
DigitalOut limit1(PC_1,PullUp);


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

// 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.70;

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

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


  
/*********************************************************************************************/
/**     DEKLARASI PROSEDUR DAN FUNGSI                                                        **/
/*********************************************************************************************/     

/*********************************************************************************************/
/*********************************************************************************************/
/**     PROGRAM UTAMA                                                                       **/
/*********************************************************************************************/
/*********************************************************************************************/
void init_servo(int i){
    if (i){
        if (sudut>60){
            sudut=60;
            }
        if (sudut<-60){
            sudut=-60;
            }
    } else {
            
        if (sudut>60){
            sudut=60;
        }
        if (sudut<-60){
            sudut=-60;
         }    
    }
}
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(PC_6,20); //pwm esc PB_8
    Servo myservo(PC_8); //pwm servo PB_9
    //set inisialisasi servo pada posisi 0 
    myservo.position(0);

    //set edf pada posisi bukan kalibrasi, yaitu set edf 0
    edf.setThrottle(pwm);
    edf.pulse();
    
    float k;
    
    pc.baud(115200);
    float speed;
    
    while(1)
    {  
        ps2.poll();
        //init_sensor();
        if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_L1)==1)){
            speed = gMax_speed;                     
            motor1.brake(0.2);
            motor2.speed(speed-0.05);            
            pc.printf("maju serong kiri\n");
            
        }
        else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_R1)==1)){
            speed = gMax_speed;                     
            motor1.speed(speed-gTuning-0.05);
            motor2.brake(0.2);
            pc.printf("maju serong kanan\n");
            
        }
        else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_L1)==1)){
            speed = gMax_speed;                     
            motor2.brake(1);
            motor1.speed(-(speed-gTuning-0.2));
            pc.printf("mundur serong kanan\n");
            
        }
        else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_R1)==1)){
            speed = gMax_speed;                     
            motor2.speed(-(speed-0.2));
            motor1.brake(1);
            pc.printf("mundur serong kiri\n");
            
        }
       else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){
            //MOTOR DEPAN
            
            speed = k;

            if (k >= gMax_speed){
                k = gMax_speed;
            }
            
            motor1.speed(speed-gTuning);
            motor2.speed(speed);
            pc.printf("maju \n");
            
            k += 0.1;
        }
        else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){
            //MOTOR BELAKANG
            speed = k;

            if (k >= gMax_speed){
                k = gMax_speed;
            }
                                
            motor1.speed(-speed);
            motor2.speed(-speed);
            pc.printf("mundur \n");
            
            k += 0.1;
        }
        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.95-gTuning));
            motor2.speed(speed*0.95);
            pc.printf("pivot 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.95-gTuning);
            motor2.speed(-speed*0.95    );
            pc.printf("pivot kanan \n");
          
        }
        else{
            motor1.brake(1);
            motor2.brake(1);
            
            k = 0.6;
        }
            
        
        if((ps2.read(PS_PAD::PAD_CIRCLE)==0) && (ps2.read(PS_PAD::PAD_TRIANGLE)==1)){
            //POWER WINDOW ATAS
            gripper.speed(1);
            
            if (limit0 == 0){
                gripper.brake(1);
            }
            pc.printf("up \n");
        }
        else if((ps2.read(PS_PAD::PAD_CIRCLE)==1) && (ps2.read(PS_PAD::PAD_TRIANGLE)==0)){
            //POWER WINDOW BAWAH 
                       
            gripper.speed(-1);
            
            if (limit1 ==0){
                gripper.brake(1);
            }
            
            pc.printf("down \n");
        }
        else{
            gripper.brake(1);

            pc.printf("diam \n");
            
        }           
        
        if(ps2.read(PS_PAD::PAD_X)==1){
            //PWM ++
            pwm += 0.01;
            pc.printf("gaspol \n");
        }
        else if(ps2.read(PS_PAD::PAD_SQUARE)==1){
            //PWM--
            pwm -= 0.01;
            pc.printf("rem ndeng \n");
        }
        
        if(ps2.read(PS_PAD::PAD_L2)==1){
            //SERVO --
            sudut += 3;
            pc.printf("servo min \n");
        }
        else if(ps2.read(PS_PAD::PAD_R2)==1){
            //SERVO ++
            sudut -= 3;
            pc.printf("servo max \n");
        }
        
        init_servo(lapangan);
        init_pwm();
        edf.setThrottle(pwm);
        edf.pulse();
        wait_ms(25);
        myservo.position(sudut);
        
    }
}