File content as of revision 2:35c396166e13:

#include "mbed.h"
#include "PS_PAD.h"
#include "Motor.h"

PS_PAD ps2(PB_15,PB_14,PB_13, PB_12); //mosi,miso,clk,ss
Serial pc(USBTX,USBRX);
Serial com(A0,A1);

#define kec_max 0.5        // konstanta kecepatan maksimum
#define kec_pivot 0.8   // konstanta  kecepatan pivot maksimum
#define ax 0.1       // konstanta perceptan
#define kec_gripper_naik 0.85
#define kec_gripper_turun 0.15
//Deklarasi pin motor
//Motor NAME (PinName pwm, PinName fwd, PinName rev):

    Motor gripper(PB_6, PB_8, PB_9);
    Motor motor3(PC_6, PC_8, PC_9);
    Motor motor2(PB_3, PB_5, PB_4);
    Motor motor1(PA_8, PA_9, PC_7);

bool maju=false;
bool mundur=false;
bool pivki=false;
bool pivka=false;
bool naik = false;
bool turun = false;
bool stop = true;
char case_jalan, prev_case_jalan;


// variabel kecepatan motor
double kec_motor;

void prosedurmotor()
{
    double koef;
    double s1=0,s2=0;
    //Kondisi Motor 
    switch (case_jalan) 
    {
        case (1): // pivot kanan
            {       
                if (pivka)
                {
                    
                    if(kec_motor<0.1) 
                    {
                        kec_motor=0.1;
                    } 
                    else 
                    {
                        kec_motor+=ax;
                    }
                    
                //perlambatan=0;
                }
                else 
                { 
                    //perlambatan=1;
                } 
            if (kec_motor>=kec_pivot) 
            {
                kec_motor=kec_pivot; 
            }
            //mode lambat dan cepat pada tombol R2 dan L2
            if((ps2.read(PS_PAD::PAD_R2)==1) && (ps2.read(PS_PAD::PAD_L2)==0)) 
            {
                koef=2;
            } 
            else if ((ps2.read(PS_PAD::PAD_R2)==0) && (ps2.read(PS_PAD::PAD_L2)==1)) 
            {
                koef=0.5;
            }
            else 
            { 
                koef=1;
            }
            
            s1 = (float)(-0.5*koef*kec_motor);
            s2 = (float)(0.5*koef*kec_motor);
              
            pivka=true;         
            maju=mundur=pivki=naik=turun=false;
            
            pc.printf("\rPivot Kanan Cuy\n");
            
            motor1.speed(s1);
            motor2.speed(s2);
            gripper.brake(1);
             
            break;
        }
    case (2): // pivot kiri
           {
            if (pivki)
            {
                if(kec_motor<0.1) 
                {
                    kec_motor=0.1;
                } 
                else 
                {
                    kec_motor+=ax;
                }
                //perlambatan=0;
            } 
            else 
            { 
                //perlambatan=1;
            } 
            
            if (kec_motor>=kec_pivot) 
            {
                kec_motor=kec_pivot; 
            }
            //mode lambat dan cepat pada tombol R2 dan L2
            if((ps2.read(PS_PAD::PAD_R2)==1) && (ps2.read(PS_PAD::PAD_L2)==0)) 
            {
                koef=2;
            } else if ((ps2.read(PS_PAD::PAD_R2)==0) && (ps2.read(PS_PAD::PAD_L2)==1)) 
            {
                koef=0.5;
            } 
            else 
            {
                koef=1;
            }
            
            s1 = (float)( 0.5*koef*kec_motor);
            s2 = (float)(-0.5*koef*kec_motor);
  
            
            pivki=true; 
            maju=mundur=pivka=naik=turun=false;
            
            pc.printf("\rPivot Kiri Cuy\n");
            
            motor1.speed(s1);
            motor2.speed(s2);
            gripper.brake(1);  
         
    
            break;
           }
    case (3): // maju
        {   
            if (maju) 
            {
                if(kec_motor<0.1) 
                {
                    kec_motor=0.1;
                } 
                else 
                {
                    kec_motor+=ax;
                }
                //perlambatan=0;
            } 
            else 
            {
                //perlambatan=1;
            } 
            
            if (kec_motor>=kec_max) 
            { 
                kec_motor=kec_max; 
            }
            //mode lambat dan cepat pada tombol R2 dan L2
            if((ps2.read(PS_PAD::PAD_R2)==1) && (ps2.read(PS_PAD::PAD_L2)==0)) 
            {
                koef=2;
            }  
            else if ((ps2.read(PS_PAD::PAD_R2)==0) && (ps2.read(PS_PAD::PAD_L2)==1))  
            { 
                koef=0.5;
            }
            else 
            {
                koef=1;  
            }
            
            //Case s1 untuk mode L2 lebih lambat
            s1 = (float)(1*koef*(kec_motor));            
            s2 = (float)(1*koef*(kec_motor)); 

            
            maju=true;             
            mundur=pivka=pivki=naik=turun=false;
 
            pc.printf("\rMaju Cuy\n");
 
            motor1.speed(s1);
            motor2.speed(s2);
            gripper.brake(1);
        
    
            break;
        }
    case (4): // mundur
        { 
            if (mundur) 
            {
                if(kec_motor<0.1) 
                {
                    kec_motor=0.1;
                } 
                else 
                {
                    kec_motor+=ax;
                }
                //perlambatan=0;
            } 
            else 
            {
                //perlambatan=1;
            } 
            
            if (kec_motor>=kec_max) 
            {
                kec_motor=kec_max; 
            }
            //mode lambat dan cepat pada tombol R2 dan L2
            if((ps2.read(PS_PAD::PAD_R2)==1) && (ps2.read(PS_PAD::PAD_L2)==0)) 
            { 
                koef=2;
            } 
            else if ((ps2.read(PS_PAD::PAD_R2)==0) && (ps2.read(PS_PAD::PAD_L2)==1)) 
            { 
                koef=0.5;
            } 
            else 
            { 
                koef=1;  
            }
            //Motor 4  telat mulai
            s1 = (float)(-1*koef*(kec_motor));
            s2 = (float)(-1*koef*(kec_motor)); 
 
 
            mundur=true; 
            maju=pivka=pivki=naik=turun=false;
 
            pc.printf("\rMundur Cuy\n");
 
            motor1.speed(s1);
            motor2.speed(s2);
            gripper.brake(1);
           
    
            break;
        }
    case (5) : //naik
        {
            //motor1.brake(1);
            //motor2.brake(1);
            gripper.speed(-kec_gripper_naik);
            break;
        }
     case (6) : //turun
        {
            //motor1.brake(1);
            //motor2.brake(1);
            gripper.speed(kec_gripper_turun);
            break;
        }  
        
        //TAMBAHAN POWER
        case (7) : //turun
        {
            motor1.speed(0.01);
            motor2.brake(0.01);
            //com.putc('1');
            //break;
        }
        case (8) : //turun
        {
            motor1.speed(0.01);
            motor2.speed(0.01);
            //com.putc('2');
            break;
        } 
         
         
    default :
        {
            
                motor1.brake(1);
                motor2.brake(1);
                gripper.brake(1);
               
            //}
 
            maju=mundur=pivki=pivka=false;
            stop = true;
 
            //s1 = 0;s2 =0; 
 
            pc.printf("\rStop\n");
        }
    }
}

int mode_jalan ()
{
    int jalan_ke;
    if ((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)) 
    {
        // Pivot Kanan
        jalan_ke = 1;
    } 
    else if ((ps2.read(PS_PAD::PAD_R1)==0) && (ps2.read(PS_PAD::PAD_L1)==1)) 
    {
        // Pivot Kiri
        jalan_ke = 2;
    } 
    else if ((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)) 
    {  
        // Maju
        jalan_ke = 3; 
    } 
    else if ((ps2.read(PS_PAD::PAD_TOP)==0) && (ps2.read(PS_PAD::PAD_BOTTOM)==1))  
    {  
        // Mundur
        jalan_ke = 4;
    }
    else if((ps2.read(PS_PAD::PAD_X)==1) && (ps2.read(PS_PAD::PAD_TRIANGLE)==0))
    {
        jalan_ke = 5;
    }
    else if((ps2.read(PS_PAD::PAD_X)==0) && (ps2.read(PS_PAD::PAD_TRIANGLE)==1))
    {
        jalan_ke = 6;
    }
    
    //tambahan power
    else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_LEFT)==0))
    {
        jalan_ke = 7;
    }
    else if((ps2.read(PS_PAD::PAD_LEFT)==1) && (ps2.read(PS_PAD::PAD_RIGHT)==0))
    {
        jalan_ke = 8;
    }
    else
    {
        jalan_ke = 0;
    }
    return(jalan_ke);
}

int main ()
{
    // Set baud rate - 115200
    pc.baud(115200);
    com.baud(9600);
    pc.printf("Ready...\n");
    ps2.init();
    
    while (1)
    {
            ps2.poll();
            case_jalan= mode_jalan(); 
            if (case_jalan != prev_case_jalan)
            {
                kec_motor=0;
            }
            
            prosedurmotor();         
            prev_case_jalan = mode_jalan(); 
    }    
}