terbaru

Dependencies:   DigitDisplay Motor PID Ping mbed millis

Fork of MainProgram_BaseBaru_20Feb_malam by KRAI 2017

main.cpp

Committer:
rahmadirizki18
Date:
2016-10-16
Revision:
3:1287fccc11be
Parent:
1:56bd3e8f38c5
Child:
4:483c07cc22e1

File content as of revision 3:1287fccc11be:

/**
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 ax 0.005
//#define koefperlambatan 0.8

// Deklarasi variabel motor
Motor motor1(PB_6, PA_7 , PB_12); // pwm, fwd, rev
Motor motor2(PB_9, PA_12, PC_5); // pwm, fwd, rev
Motor motor3(PB_7, PA_15, PA_14); // pwm, fwd, rev
Motor motor4(PB_8, PB_1 , PA_13); // 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;
int jLX,jLY;
double vcurr=0;
float x,y,xk,yk; // untuk analog, x sebelum kalibrasi xk sesudah kalibrasi
float errorx=0,errory=0;

// Fungsi mapping 0-255 ke -1 sampai 1
float mapping(int x)
{   
    float hasil;
    hasil = ((x+1)/128)-1;
    
    return (hasil);
}

// simpan data analog
void baca_analog()
{
        jLX = joystick.LX;
        jLY = joystick.LY;
        
        // Pembacaan nilai Y terbalik
        x = mapping(jLX);
        y = -mapping(jLY); 
            
        // Setelah di kalibrasi
        xk = x + errorx;
        yk = y + 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)) {  
        // analog switch mode
        if (joystick.SELECT_click) {analog=!analog;}
        // 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;
    
    // 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 (analog) {
                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)(0.5*koef*vcurr*((-xk)+(yk)));
            s2 = (float)(0.5*koef*vcurr*((-xk)+(-yk)));
            s3 = (float)(0.5*koef*vcurr*((xk)+(-yk)));
            s4 = (float)(0.5*koef*vcurr*((xk)+(yk)));    
            
                                  
                       
            analog=true;
            maju=mundur=kiri=kanan=saka=saki=sbka=sbki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
            
            pc.printf("analog x= %d  y= %d  /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");
        }  
    }
}

// Kalibrasi tombol analog kiri
void kalibrasi()
{
    errorx = 0-x;
    errory = 0-y;    

}


int main (void)
{
    // Set baud rate - 115200
    joystick.setup();
    pc.baud(115200);
    pc.printf("Ready...\n");

    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_click){
                kalibrasi();
            }  

        } else {
            joystick.idle();
            
        }                           
    }
}