File content as of revision 10:0fadb197749f:

/*********************************************************************************************/
/**     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);
Serial sensor(PA_0,PA_1);

// joystick PS2
PS_PAD ps2(PB_15,PB_14,PB_13, PC_4); //(mosi, miso, sck, ss) default board lama pb_12

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

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

DigitalOut pnuematik_lengan(PC_12);
DigitalOut pnuematik_gripper(PC_11);

ESC edf(PC_6,20); //pwm esc PB_8
Servo myservo(PC_8); //pwm servo PB_9

Timer timer;

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

int c=0;    //motor gripper auto
int g=2;    //pnuematik kondisi gripper
int count=1;

float k;
float speed;

int datasensor[6];

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

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


void motor_base(void){
    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;
        }
}

void motor_gripper(){
        if((ps2.read(PS_PAD::PAD_CIRCLE)==0) && (ps2.read(PS_PAD::PAD_TRIANGLE)==1)){
            //POWER WINDOW ATAS
        /*  do{
                motor_base();
                gripper.speed(1);
            }while((limit0 != 0) && (c > 7));
          */
            gripper.speed(0.9);
            
            if (limit0 == 0){
                gripper.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 
           /*do{
                motor_base();
                gripper.speed(-0.8);
            }while((limit1 != 0) && (c > 7));
            */
                       
            gripper.speed(-0.7);
            
            if (limit1 ==0){
                gripper.brake(1);
            }
            
            pc.printf("down \n");
            c--;        
        }
        else{
            gripper.brake(1);
            if ((c <= 7) && (c>=-7)){
                c=0;
            }
            
            pc.printf("diam \n");
        }           
        if((c > 7) && (limit0 == 0)){
            c = 0;
            gripper.brake(1);
        }
        else if((c < -7) && (limit1 == 0)){
            c = 0;
            gripper.brake(1);
        }
        else if( (c > 7) && (limit0 != 0)){
            gripper.speed(1);
        }
        else if ((c<-7) && (limit1 != 0)){
            gripper.speed(-0.9);
        }
}

void servo_edf(){
    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");
        }
        
        if(ps2.read(PS_PAD::PAD_START)==1){
            
            sudut = -60;
            pwm = 0.18;
        }
        
        init_servo(lapangan);
        init_pwm();
        edf.setThrottle(pwm);
        edf.pulse();
        wait_ms(25);
        myservo.position(sudut);
}

void pnuematik_running(){
    if ((ps2.read(PS_PAD::PAD_SELECT)==1))
        {
            //mekanisme ambil gripper
            pc.printf("mekanisme gripper");
            if (g==1){
                pc.printf("ambil 1");
                pnuematik_lengan = 1;
                pnuematik_gripper = 1;
                g=2;
                wait_ms(400);
            }
            else
            {
                pnuematik_gripper = 0;
                wait_ms(400);
                pnuematik_lengan = 0;
                
                g=1;
                pc.printf("ambil 2");
                wait_ms(400);
            }
        }
}

void line_follower(float speed){    
    int pv;  
    float speedR,speedL;
         
        //////////////////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 = -4;
        }
        else if(datasensor[4] && datasensor[5]){
            pv = 4;
        }
        else if(datasensor[1]){
            pv = -3;
        }
        else if(datasensor[4]){
            pv = 3;
        }
        else if(datasensor[1] && datasensor[2]){
            pv = -2;
        }
        else if(datasensor[3] && datasensor[4]){
            pv = 2;
        }
        else if(datasensor[2]){
            pv = -1;
        }
        else if(datasensor[3]){
            pv = 1;
        }
        else if(datasensor[2] && datasensor[3]){
            pv = 0;
        }
        else
        {
            ///////////////// 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 = speed - PID.compute();
        if(speedR > speed){
            speedR = speed;
            }
        else if(speedR < gMin_speed)
            speedR = gMin_speed;
        motor2.speed(speedR);
    
        speedL = speed + PID.compute();
        if(speedL > speed)
            speedL = speed;
        else if(speedL < gMin_speed)
            speedL = gMin_speed;
        motor1.speed(speedL);
}

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

/*********************************************************************************************/
/**     PROGRAM UTAMA                                                                       **/
/*********************************************************************************************/
/*********************************************************************************************/ 
int main(void){
    //inisialisasi serial
    pc.baud(115200);
    sensor.baud(115200);
    
    //inisialisasi joystick
    ps2.init();   

    //set inisialisasi servo pada posisi 0 
    myservo.position(0);

    //set edf pada posisi bukan kalibrasi, yaitu set edf 0
    edf.setThrottle(pwm);
    edf.pulse();
    
    //inisialisasi gripper propeller
    pnuematik_lengan = 0;
    pnuematik_gripper = 0;
    
    //inisialisasi PID sensor    
    PID.setInputLimits(-15,15);
    PID.setOutputLimits(-1.0,1.0);
    PID.setMode(1.0);
    PID.setBias(0.0);
    PID.reset();
    
    bool manual=true;
   
   while(manual)
    {
        ps2.poll();          
        
        motor_base();
        
        motor_gripper();
             
        pnuematik_running();
                
        servo_edf();
 
        if(ps2.read(PS_PAD::ANALOG_RIGHT)==1){
            manual = false;
        }
    }
    
    timer.start();
    while(1)
    {
        init_sensor();
    
        //line_follower(0.4);
            
        for(int i=0; i<6; i++){
            pc.printf("%d  ",datasensor[i]);
        }
        pc.printf("\n");
    }
        
}