File content as of revision 3:6d469c371f49:

#include "JoystickPS3.h"
#include "Motor.h"
#include "mbed.h"
/* Inisialisasi  Pin TX-RX Joystik dan PC */
joysticknucleo joystick(PA_0,PA_1);
Serial pc(USBTX,USBRX);
/* Deklarasi Motor Base */
Motor motorKanan(PA_15, PB_0, PA_4);
Motor motorKiri  (PB_7, PC_2, PC_3);
int case_joy;
/****************************************************/
/*         Deklarasi Fungsi dan Procedure           */
/****************************************************/
int case_joystick()
{
//---------------------------------------------------//
//  Gerak Motor Base                                 //
//   Case 1  : Pivot kanan                           //
//   Case 2  : Pivot Kiri                            //
//   Case 3  : Maju                                  //
//   Case 4  : Kiri                                  //
//   Case 5  : Bawah                                 //
//   Case 6  : Kanan                                 //
//---------------------------------------------------//
    
    int caseJoystick;
    if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
        // Pivot Kanan
        caseJoystick = 1;
    } 
    else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
        // Pivot Kiri
        caseJoystick = 2;
    }
    else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
        // Maju
        caseJoystick = 3;
    }
    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri))  {  
        // Pivot Kiri
        caseJoystick = 4;
    }
    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
        // Pivot Kiri
        caseJoystick = 5;
    }
    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri))  {  
        // Pivot Kiri
        caseJoystick = 6;
    }
        
    return(caseJoystick);
}
 
void aktuator()
{
    switch (case_joy) {
        case (1): 
        {       
            // Pivot Kanan
            motorKanan.speed(0.3);
            motorKiri.speed(0.4);
            break;
        }
        case (2): 
        {
            // Pivot Kiri
            motorKanan.speed(-0.3);
            motorKiri.speed(-0.4);
            break;
        }
        case (3): 
        {
            // Maju
            motorKanan.speed(-0.5);
            motorKiri.speed(0.5);
            break;
        }
        case (4): 
        {
            // Kiri
            motorKanan.speed(0.5);
            motorKiri.speed(0.6);
            break;
        }
        case (5): 
        {
            //Belakang
            motorKanan.speed(0.3);
            motorKiri.speed(-0.4);
            break;
        }
        case (6): 
        {
            // Kanan
            motorKanan.speed(-0.3);
            motorKiri.speed(-0.4);
            break;
        }
        default : 
        {
            motorKanan.brake(1);
            motorKiri.brake(1);
        }
    }
}
/*********************************************************/
/*                  Main Function                        */
/*********************************************************/
 
int main (void)
{
    // Set baud rate - 115200
    joystick.setup();
    pc.baud(115200);
    wait_ms(1000);
//    startMillis();
    while(1)
    {   
        // Interrupt Serial
        joystick.idle();        
        if(joystick.readable()) 
        {
            // Panggil fungsi pembacaan joystik
            joystick.baca_data();
            // Panggil fungsi pengolahan data joystik
            joystick.olah_data();
            // Masuk ke case joystick
            case_joy = case_joystick();
            //pc.printf("%d\n",case_joy);
            aktuator();
        }
        else
        {
            joystick.idle();
        }
        
//        if(millis() - previousMillis5 >= 400)
//        {    
//            display.write(0,((int) rpm2) / 10);
//            display.write(1,((int)rpm2) % 10);
//            display.write(2, (int)target_rpm2 / 10);
//            display.write(3, (int)target_rpm2 % 10);
//            display.setColon(true);
//            
//            previousMillis5 = millis();
//        }
        
    }
}