File content as of revision 2:ab055f0284fc:

#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 motorDepan(PB_7, PC_3, PC_0); //(PB_9, PA_12, PC_5); 
Motor motorKanan(PB_3, PA_10, PC_4);
Motor motorKiri  (PB_9, PA_6, PA_12);
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))  {  
        // Pivot Kiri
        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
            motorDepan.speed(0.23);
            motorKanan.speed(0.17);
            motorKiri.speed(0.23);
            break;
        }
        case (2): 
        {
            // Pivot Kiri
            motorDepan.speed(-0.23);
            motorKanan.speed(-0.17);
            motorKiri.speed(-0.23);
            break;
        }
        case (3): 
        {
            // Maju
            motorDepan.speed(0);
            motorKanan.speed(-0.17);
            motorKiri.speed(0.3);
            break;
        }
        case (4): 
        {
            // Kiri
            motorDepan.speed(-0.23);
            motorKanan.speed(0.17);
            motorKiri.speed(0.3);
            break;
        }
        case (5): 
        {
            //Belakang
            motorDepan.speed(0);
            motorKanan.speed(0.17);
            motorKiri.speed(-0.3);
            break;
        }
        case (6): 
        {
            // Kanan
            motorDepan.speed(0.23);
            motorKanan.speed(-0.17);
            motorKiri.speed(-0.3);
            break;
        }
        default : 
        {
            motorDepan.brake(1);
            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();
//        }
        
    }