File content as of revision 19:bb304933a9f8:

/****************************************************************************/
/*                                                                          */
/*    Joystick                                                              */
/*  kanan => posisi target x ditambah 0.01                                  */
/*  kiri  => posisi target x dikurang 0.01                                  */
/*  atas  => posisi target y ditambah 0.01                                  */
/*  bawah => posisi target y dikurang 0.01                                  */
/*                                                                          */
/*  Tombol silang   => Kembali keposisi Awal                                */
/*  Tombol segitiga => Aktif motor Launcher                                 */
/*  Tombol lingkaran=> Aktif servo Launcher                                 */
/*  Tombol L3       => PWM Launcher dikurangin                              */
/*  Tombol R3       => PWM Launcher ditambahin                                                                        */
/*                                                                          */
/*                                                                          */
/*                                                                          */
/****************************************************************************/


#include "mbed.h"
#include "JoystickPS3.h"
#include "Motor.h"
#include "Servo.h"

//#define koefperlambatan 0.8
//#include "encoderKRAI.h"
/*
#define pi 22/7
#define diaEncoder 0.058
#define PPR 1000
#define diaRobot 0.64
*/
#define vmax 0.3    //0.4
#define vmaxserong 0.3 //0.3
#define vmaxpivot 0.3   //0.3
#define vmaxanalog 0.3  //0.3
#define ax 0.005
#define pinservo1 PC_8
#define pinservo2 PC_9

float speed1=0.6;
float speed2=0.6;
float speed3=0.6;
float speed4=0.6;
float speedB=0.23 ;
float speedL=0.4;



// Deklarasi variabel motor
Motor motor1(PB_7, PA_15 , PA_14); // pwm, fwd, rev
Motor motor2(PB_8, PB_0 ,PA_13); // pwm, fwd, rev
Motor motor3(PB_9, PC_5 ,PA_12); // pwm, fwd, rev
Motor motor4(PB_6, PB_12 ,PB_1); // pwm, fwd, rev

//Motor Atas
Motor motorld(PA_8, PC_1 , PC_2); // pwm, fwd, rev
Motor motorlb(PA_9, PA_4, PC_15 ); // pwm, fwd, rev

//Servo Atas
Servo servoS(PB_2);
Servo servoB(PA_5);

float jarak, posX, posY;

// Inisialisasi  Pin TX-RX Joystik dan PC
joysticknucleo joystick(PA_0,PA_1);
Serial pc(PA_2,PA_3);

//encoder variable
float errX, errY, errT, Vt, Vx, Vy;
float V1, V2, V3, V4;

//bool perlambatan=0;
char case_ger;
bool maju=false,mundur=false,pivka=false,pivki=false,kiri=false,kanan=false,saka=false,saki=false,sbka=false,sbki=false,cw1=false,ccw1=false,cw2=false,ccw2=false,cw3=false,ccw3=false,analog=false;
bool stop = true;
bool Launcher = false,ServoGo = false;
float jLX,jLY;
double vcurr;
float x,y; // untuk analoghat kiri
float errorx=0,errory=0;

// Fungsi mapping 0-255 ke -128 sampai 127
float mapping(float a,float error)
{   
     float hasil,b;
    b = (float)((a-128)/128);
    if (b>(error - 0.2) && b<(error + 0.2))
    {
        hasil = 0;
    } else {
        hasil = b;
        }
    return (hasil);
}

// Kalibrasi tombol analog kiri
void kalibrasi()
{
    errorx = (jLX - 128)/128;
    errory = (jLY - 128)/128;    

}

// simpan data analog
void baca_analog()
{
        jLX = joystick.LX;
        jLY = joystick.LY;
        
        // Pembacaan nilai Y terbalik
        x = mapping(jLX,errorx);
        y = -mapping(jLY,errory); 
}

// Gerak dari Motor base
int case_gerak()
{
    int casegerak;
    baca_analog();
     if (!joystick.L1 && joystick.R1) {
        // Pivot Kanan
        casegerak = 1;
    } else if (!joystick.R1 && joystick.L1) {
        // Pivot Kiri
        casegerak = 2;
    } else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {  
        // Maju
        casegerak = 3;
    } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
        // Mundur
        casegerak = 4;
    } else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan))   {   
        // Serong Atas Kanan
        casegerak = 5;
    } else if((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan)) {   
        // Serong Bawah Kanan
        casegerak = 6;
    } else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)) {   
        // Serong Atas Kiri
        casegerak = 7;
    } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)) {   
        // Serong Bawah Kiri
        casegerak = 8;
    } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri))  {   
        // Kanan
        casegerak = 9;
    } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {   
        // Kiri
        casegerak = 10;        
    } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {  
        // case gerak analog on off
        if (analog){
            casegerak = 11;
        } else {
            casegerak = 12;
        }    
}
    return(casegerak);
}



/**

**  Case 1  : Pivot kanan
**  Case 2  : Pivot Kiri
**  Case 3  : Maju
**  Case 4  : Mundur
**  Case 5  : Serong Atas Kanan
**  Case 6  : Serong Bawah Kanan
**  Case 7  : Serong Atas Kiri
**  Case 8  : Serong Bawah Kiri
**  Case 9  : Kanan
**  Case 10 : Kiri
**  Case 11 : Analog
**  Case 11 : break

**/


// Pergerakan dari base
void aktuator()
{
    //Servo
    if (ServoGo){
        servoS.position(20);
        wait_ms(500);
        servoS.position(-28);
        wait_ms(500);
        servoS.position(20);
        wait_ms(500);
        for (int i = -0; i<=70; i++){
        servoB.position(i);
        wait_ms(10);
        }
        wait_ms(500);
        servoB.position(0);
        ServoGo = false;
        
    }else{
        servoS.position(20);
        servoB.position(0);
    
    }
    
    // Motor Atas
    if (Launcher) {
            motorld.speed(speedL);
            motorlb.speed(speedB);
    }else{
            motorld.speed(0);
            motorlb.speed(0);
    }
    
    double s1=0,s2=0,s3=0,s4=0,s1t=0,s2t=0,s3t=0,s4t=0;
    
    // MOTOR BAWAH
    switch (case_ger) 
    {
case (1): 
        {       
            if (pivka) {
                if(vcurr<0.1) {
                    vcurr=0.1;
                } else {
                    vcurr+=ax;
                }
                //perlambatan=0;
            } else { 
                //perlambatan=1;
            } 
            
            if (vcurr>=vmaxpivot) {
                vcurr=vmaxpivot; 
            }
            
            
            s1 = (float)(-0.5* vcurr);
            s2 = (float)(-0.5* vcurr);
            s3 = (float)(-0.5* vcurr);
            s4 = (float)(-0.5* vcurr);    
            
            pivka=true;         
            maju=mundur=analog=kiri=kanan=saka=saki=sbka=sbki=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
            
            //pc.printf("pivKa\n");
            
            motor1.speed(s1);
            motor2.speed(s2);
            motor3.speed(s3);
            motor4.speed(s4);

            break;
        }
    case (2):
           {
            if (pivki){
                if(vcurr<0.1) {
                    vcurr=0.1;
                } else {
                    vcurr+=ax;
                }
                //perlambatan=0;
            } else { 
                //perlambatan=1;
            } 
            
            if (vcurr>=vmaxpivot) {
                vcurr=vmaxpivot; 
            }
            
               
            s1 = (float)(0.5* vcurr);
            s2 = (float)(0.5* vcurr);
            s3 = (float)(0.5* vcurr); 
            s4 = (float)(0.5* vcurr);    
            
            pivki=true; 
            maju=mundur=kiri=analog=kanan=saka=saki=sbka=sbki=pivka=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
            
            //pc.printf("pivKi\n");
            
            motor1.speed(s1);
            motor2.speed(s2);
            motor3.speed(s3);
            motor4.speed(s4);
    
            break;
           }
    case (3):
        {   
            if (maju) { 
                if(vcurr<0.1) {
                    vcurr=0.1;
                } else {
                    vcurr+=ax;
                }
                //perlambatan=0;
            } else {
                //perlambatan=1;
            } 
            
            if (vcurr>=vmax) { 
                vcurr=vmax; 
            }
            
            
            //Case s1 untuk mode L2 lebih lambat
            s1 = (float)(-1* (vcurr));
            s2 = (float)(1.0* vcurr); 
            s3 = (float)(1.0* vcurr); 
            s4 = (float)(-1* vcurr);  
            
            //s1 =-0.8* vcurr;
            //s2 = vcurr; 
            //s3 =- vcurr; 
            //s4 = vcurr;    
            
            maju=true;             
            mundur=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;

            //pc.printf("maju\n");

            motor1.speed(s1);
            motor2.speed(s2);
            motor3.speed(s3);
            motor4.speed(s4);
    
            break;
        }
    case (4):
        { 
            if (mundur) {
                if(vcurr<0.1) {
                    vcurr=0.1;
                } else {
                    vcurr+=ax;
                }
                //perlambatan=0;
            } else {
                //perlambatan=1;
            } 
            
            if (vcurr>=vmax) {
                vcurr=vmax; 
            }
            
            
            s1 = (float)(1* (vcurr));
            s2 = (float)(-1* (vcurr)); 
            s3 = (float)(-1* (vcurr)); 
            s4 = (float)(1* (vcurr));

            mundur=true; 
            maju=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;

            //pc.printf("mundur\n");

            motor1.speed(s1);
            motor2.speed(s2);
            motor3.speed(s3);
            motor4.speed(s4);
    
            break;
        }
    case (5) :
        {   
            if (saka) { 
                if(vcurr<0.1) {
                    vcurr=0.1;
                } else {
                    vcurr+=ax;
                }
                //perlambatan=0;
            } else { 
                //perlambatan=1;
            } 
            
            if (vcurr>=vmax) {
                vcurr=vmax; 
            } 
            
            
            s1 = (float)(- vcurr);
            s2 = (float)(0);                 // 0.1*vcurr;
            s3 = (float)( vcurr); 
            s4 = (float)(0);                 //- 0.1*vcurr;                
            
            saka=true; 
            maju=mundur=kiri=kanan=sbka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
            
            //pc.printf("saka\n");
            
            motor1.speed(s1);
            motor2.brake(1);
            //motor2.speed(s2);
            motor3.speed(s3);
            motor4.brake(1);
            //motor4.speed(s4);
            
            break;
        }
    case (6) :
        {   
            if (sbka){
                if(vcurr<0.1) {
                    vcurr=0.1;
                } else {
                    vcurr+=ax;
                }
                //perlambatan=0;
            } else {
                //perlambatan=1;
            } 
            
            if (vcurr>=vmaxserong) {
                vcurr=vmaxserong; 
            }
            
            
            s1 = (float)(0);                 // 0.1*vcurr;
            s2 = (float)(- vcurr); 
            s3 = (float)(0);                 //- 0.1*vcurr;
            s4 = (float)( vcurr);
            
            sbka=true; 
            maju=mundur=kiri=kanan=saka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
            
            //pc.printf("sbka\n");

            //motor1.speed(s1);
            motor1.brake(1);
            motor2.speed(s2);
            //motor3.speed(s3);
            motor3.brake(1);
            motor4.speed(s4);
    
            break;
        }
    case (7) :
        {   
            if (saki) {
                if(vcurr<0.1) {
                    vcurr=0.1;
                } else {
                    vcurr+=ax;
                }
                //perlambatan=0;
            } else {
                //perlambatan=1;
            } 
            
            if (vcurr>=vmaxserong) { 
                vcurr=vmaxserong; 
            }
            
            
            s1 = (float)(0);                     //- 0.1*vcurr;
            s2 = (float)( vcurr); 
            s3 = (float)(0);                     // 0.1*vcurr; 
            s4 = (float)(- vcurr);  
            
            saki=true; 
            maju=kiri=kanan=saka=mundur=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
            
            //pc.printf("saki\n");
            
            //motor1.speed(s1);
            motor1.brake(1);
            motor2.speed(s2);
            //motor3.speed(s3);
            motor3.brake(1);
            motor4.speed(s4);
    
            break;
        }
    case (8) :
        {   
            if (sbki) {
                if(vcurr<0.1) {
                    vcurr=0.1;
                } else {
                    vcurr+=ax;
                }
                //perlambatan=0;
            } else {
                //perlambatan=1;
            } 
            
            if (vcurr>=vmaxserong) { 
                vcurr=vmaxserong; 
            }
            
            
            s1 = (float)( vcurr);
            s2 = (float)(0);                 //- 0.1*vcurr;
            s3 = (float)(- vcurr); 
            s4 = (float)(0);                 // 0.1*vcurr;
            
            sbki=true; 
            maju=kiri=kanan=saka=saki=sbka=mundur=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
            
            //pc.printf("sbki\n");
            
            motor1.speed(s1);
            //motor2.speed(s2);
            motor2.brake(1);
            motor3.speed(s3);
            //motor4.speed(s4);
            motor4.brake(1);
    
            break;
        }
    case (9) :
        {   
            if (kanan) {
                if(vcurr<0.1) {
                    vcurr=0.1;
                } else {
                    vcurr+=ax;
                }
                //perlambatan=0;
            } else {
                //perlambatan=1;
            } 
            
            if (vcurr>=vmax) { 
                vcurr=vmax; 
            }
            
            
            s1 =(float)(-1* vcurr);
            s2 =(float)(-1.0* vcurr); 
            s3 =(float)(1* vcurr); 
            s4 =(float)(1.0* (vcurr+0.005));           
            
            kanan=true; 
            maju=kiri=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
            
            //pc.printf("Kanan\n");
            
            motor1.speed(s1);
            motor2.speed(s2);
            motor3.speed(s3);
            motor4.speed(s4);
            break;
        }
    case (10) :
        {   
            if (kiri) { 
                if(vcurr<0.1) {
                    vcurr=0.1;
                } else {
                    vcurr+=ax;
                }
                //perlambatan=1;
            } else {
                //perlambatan=1;
            } 
            
            if (vcurr>=vmax) {
                vcurr=vmax; 
            }

            
            s1 =(float)(1* vcurr);
            s2 =(float)(1* (vcurr+0.003)); 
            s3 =(float)(-1* vcurr); 
            s4 =(float)(-1.0* vcurr);
            
            kiri=true; 
            maju=kanan=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
            
            //pc.printf("Kiri\n");
            
            motor1.speed(s1);
            motor2.speed(s2);
            motor3.speed(s3);
            motor4.speed(s4);
              
            break;
        }
    case (11): 
        {          
                     
            
            s1t = (vmaxanalog*(-x+y));
            s2t = (vmaxanalog*(-x-y));
            s3t = (vmaxanalog*(x-y));
            s4t = (vmaxanalog*(x+y));
            
            s1 = (float)(0.5* s1t);
            s2 = (float)(0.5* s2t);
            s3 = (float)(0.5* s3t);
            s4 = (float)(0.5* s4t);  
            
                                  
                       
            analog=true;
            maju=mundur=kiri=kanan=saka=saki=sbka=sbki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
            
            //pc.printf("analog X =%.2f   Y =%.2f \n  ",x,y);
            
       
                motor1.speed(s1);
                motor2.speed(s2);
                motor3.speed(s3);
                motor4.speed(s4);
            break;
    }
        default :
        {
                motor1.brake(1);
                motor2.brake(1);
                motor3.brake(1);
                motor4.brake(1);
         
            maju=mundur=kiri=kanan=saka=saki=sbka=sbki=analog=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
            stop = true;


            //pc.printf("Stop\n");
        }   
    }  
}



void speedLauncher()
{
    if (joystick.R3_click and speedL < 0.8){
        speedL = speedL + 0.01;}
    if (joystick.L3_click and speedL > 0.1){
        speedL = speedL - 0.01;} 
    if (joystick.R2_click and speedB < 0.8 ){
            speedB = speedB + 0.01;}  
    if (joystick.L2_click and speedB > 0.1 ){
        speedB = speedB - 0.01;}    
    //pc.printf("Pwm depan = %.3f\t  Pwm belakang = %.3f\n", speedL, speedB);
}
   
    
    
int main (void)
{
    // Set baud rate - 115200
    joystick.setup();
    pc.baud(57600);
    wait_ms(500);
    
    pc.printf("Ready...\n");
    kalibrasi();
    while(1)
    {
        // Interrupt Serial
        joystick.idle();
       if(joystick.readable() ) {
            // Panggil fungsi pembacaan joystik
            joystick.baca_data();
            // Panggil fungsi pengolahan data joystik
            joystick.olah_data();
            //pc.printf("%3d %3d\n\r",joystick.R2, joystick.L2);
            //pc.printf("%2x %2x %2x %2x %3d %3d %3d %3d %3d %3d\n\r",joystick.button, joystick.RL, joystick.button_click, joystick.RL_click, joystick.R2, joystick.L2, joystick.RX, joystick.RY, joystick.LX, joystick.LY);
            case_ger = case_gerak();
            aktuator();
            
            //pc.printf("bacaS = %.2f\tbacaB = %.2f\n",servoS.read(), servoB.read());
            
            //kalibrasi
            if (joystick.START){
                kalibrasi();}
             // analog switch mode
            if (joystick.SELECT_click)      {analog = !analog;}  
            if (joystick.segitiga_click)    {Launcher = !Launcher;}
            if (joystick.lingkaran_click){
                ServoGo = true;
                }  
            if (joystick.silang) {
                pc.printf("Depan  = %.3f \t Belakang = %.3f \n",speedB,speedL);
                }
            speedLauncher();
         
        } else {
            joystick.idle();
            
        }                           
//            gotoXYT(XT,YT,Tetha);

    }
}