File content as of revision 4:483c07cc22e1:

/**
Case Gerak
1.  cw
2.  ccw
3.  Maju
4.  Mundur
5.  Serong Atas Kanan
6.  Serong Bawah Kanan
7.  Serong Atas Kiri
8.  Serong Bawah Kiri
9.  Kanan
10. Kiri
11. Analog kiri base
12. stop

Urutan motor 1234 searah jarum jam

Source Code dari
Bima Sahbani            EL'12
Fanny Achmad Hindrarta  EL'12
**/
#include "mbed.h"
#include "JoystickPS3.h"
#include "Motor.h"

#define vmax 1
#define vmaxserong 0.9
#define vmaxpivot 0.7
#define vmaxanalog 0.9
#define ax 0.005
//#define koefperlambatan 0.8

// Deklarasi variabel motor
Motor motor1(PB_6, PB_4 , PB_5); // pwm, fwd, rev
Motor motor2(PB_7, PA_4, PC_1); // pwm, fwd, rev
Motor motor3(PB_8, PC_12, PD_2); // pwm, fwd, rev
Motor motor4(PB_9, PC_10 , PC_11); // pwm, fwd, rev

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

//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;
float jLX,jLY;
double vcurr=0;
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()
{
    double koef;
    double s1=0,s2=0,s3=0,s4=0,s1t=0,s2t=0,s3t=0,s4t=0;
    
    // MOTOR
    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; 
            }
            
            if(joystick.R2==255 && joystick.L2==0) {
                koef=2;
            } else if (joystick.L2==255 && joystick.R2==0) {
                koef=0.5;
            }
            else { 
                koef=1;
            }
            
            s1 = (float)(-0.5*koef*vcurr);
            s2 = (float)(-0.5*koef*vcurr);
            s3 = (float)(-0.5*koef*vcurr);
            s4 = (float)(-0.5*koef*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; 
            }
            
            if(joystick.R2==255 && joystick.L2==0) {
                koef=2;
            } else if (joystick.L2==255 && joystick.R2==0) {
                koef=0.5;
            } else {
                koef=1;
            }
            
            s1 = (float)(0.5*koef*vcurr);
            s2 = (float)(0.5*koef*vcurr);
            s3 = (float)(0.5*koef*vcurr); 
            s4 = (float)(0.5*koef*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; 
            }
            
            if(joystick.R2==255 && joystick.L2==0) {
                koef=2;
            }  else if (joystick.L2==255 && joystick.R2==0)  { koef=0.5;}
            else {
                koef=1;  
            }
            
            //Case s1 untuk mode L2 lebih lambat
            s1 = (float)(-1*koef*(vcurr+0.005));
            
            s2 = (float)(1.0*koef*vcurr); 
            s3 = (float)(1.0*koef*vcurr); 
            s4 = (float)(-1*koef*vcurr);  
            
            //s1 =-0.8*koef*vcurr;
            //s2 =koef*vcurr; 
            //s3 =-koef*vcurr; 
            //s4 =koef*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; 
            }
            
            if(joystick.R2==255 && joystick.L2==0) { 
                koef=2;
            } else if (joystick.L2==255 && joystick.R2==0) { 
                koef=0.5;
            } else { 
                koef=1;  
            }
            //Motor 4  telat mulai
            s1 = (float)(1*koef*(vcurr-0.008));
            s2 = (float)(-1*koef*(vcurr-0.005)); 
            s3 = (float)(-1*koef*(vcurr-0.005)); 
            s4 = (float)(1*koef*(vcurr+0.005));

            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; 
            } if(joystick.R2==255 && joystick.L2==0)  { 
                koef=2;  
            } else if (joystick.L2==255 && joystick.R2==0) { 
                koef=0.5;
            } else { 
                koef=1;  
            }
            
            s1 = (float)(-koef*vcurr);
            s2 = (float)(0);                 //koef*0.1*vcurr;
            s3 = (float)(koef*vcurr); 
            s4 = (float)(0);                 //-koef*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; 
            }
            
            if(joystick.R2==255 && joystick.L2==0) { 
                koef=2;  
            } else if (joystick.L2==255 && joystick.R2==0) { 
                koef=0.5;
            } else { 
                koef=1;  
            }
            
            s1 = (float)(0);                 //koef*0.1*vcurr;
            s2 = (float)(-koef*vcurr); 
            s3 = (float)(0);                 //-koef*0.1*vcurr;
            s4 = (float)(koef*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; 
            }
            
            if(joystick.R2==255 && joystick.L2==0) { 
                koef=2;  
            } else if (joystick.L2==255 && joystick.R2==0)  { 
                koef=0.5;
            } else { 
                koef=1;  
            }
            
            s1 = (float)(0);                     //-koef*0.1*vcurr;
            s2 = (float)(koef*vcurr); 
            s3 = (float)(0);                     //koef*0.1*vcurr; 
            s4 = (float)(-koef*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; 
            }
            
            if(joystick.R2==255 && joystick.L2==0) { 
                koef=2;  
            } else if (joystick.L2==255 && joystick.R2==0)  { 
                koef=0.5;
            } else { 
                koef=1;  
            }
            
            s1 = (float)(koef*vcurr);
            s2 = (float)(0);                 //-koef*0.1*vcurr;
            s3 = (float)(-koef*vcurr); 
            s4 = (float)(0);                 //koef*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; 
            }
            
            if(joystick.R2==255 && joystick.L2==0) { 
                koef=2;  
            } else if (joystick.L2==255 && joystick.R2==0)  { 
                koef=0.5;
            } else { 
                koef=1;  
            }
            
            s1 =(float)(-1*koef*vcurr);
            s2 =(float)(-1.0*koef*vcurr); 
            s3 =(float)(1*koef*vcurr); 
            s4 =(float)(1.0*koef*vcurr);           
            
            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; 
            }

            if(joystick.R2==255 && joystick.L2==0)  { 
                koef=2;  
            } else if (joystick.L2==255 && joystick.R2==0)  { 
                koef=0.5;
            } else { 
                koef=1;  
            }
            
            s1 =(float)(1*koef*vcurr);
            s2 =(float)(1*koef*vcurr); 
            s3 =(float)(-1*koef*vcurr); 
            s4 =(float)(-1.0*koef*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): 
        {          
                     
            if(joystick.R2==255 && joystick.L2==0) {
                koef=2;
            } else if (joystick.L2==255 && joystick.R2==0) {
                koef=0.5;
            }
            else { 
                koef=1;
            }
            
            s1t = (vmaxanalog*(-x+y));
            s2t = (vmaxanalog*(-x-y));
            s3t = (vmaxanalog*(x-y));
            s4t = (vmaxanalog*(x+y));
            
            s1 = (float)(0.5*koef*s1t);
            s2 = (float)(0.5*koef*s2t);
            s3 = (float)(0.5*koef*s3t);
            s4 = (float)(0.5*koef*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 :
        {
            //if (mundur||kiri||kanan||saka||saki||sbka||sbki||pivki||pivka||cw1||ccw1||cw2||ccw2||cw3||ccw3) wait_ms(100);
            //if (maju && (vcurr>=0.5)) wait_ms(100); 
            //else if (maju && (vcurr<0.5)) wait_ms(50);  
            /*      
            if(s1>0.2 || s1<-0.2 || s2>0.2 || s2<-0.2) {
                s1 = koefperlambatan * s1;
                s2 = koefperlambatan * s2;
                s3 = koefperlambatan * s3;
                s4 = koefperlambatan * s4;

                motor1.speed(s1);
                motor2.speed(s2);
                motor3.speed(s3);
                motor4.speed(s4);   
                
                
            } else {
            */
                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;

            //s1 = 0;s2 =0; s3 =0; s4 =0;

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


int main (void)
{
    // Set baud rate - 115200
    joystick.setup();
    pc.baud(115200);
    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("%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();
            //kalibrasi
            if (joystick.START){
                kalibrasi();}
             // analog switch mode
            if (joystick.SELECT_click) {analog=!analog;}  
            //pc.printf(" X =%.2f   Y =%.2f \n  ",x,y);
        } else {
            joystick.idle();
            
        }                           
    }
}