File content as of revision 12:e07c59c28c29:

/****************************************************************************/
/*                  PROGRAM UNTUK PID CLOSED LOOP                           */
/*                                                                          */
/*  - Digunakan encoder autonics                                            */
/*  - Konfigurasi Motor dan Encoder sbb :                                   */
/*        _________________                                                 */
/*        |     DEPAN      |                                                */
/*        | 1.    e     .2 |    Angka ==> Motor                             */
/*        | `            ` |    e     ==> Encoder                           */
/*        | e            e |                                                */
/*        | .            . |                                                */
/*        | 4`    e     `3 |                                                */
/*        |________________|                                                */
/*                                                                          */
/*   SETTINGS (WAJIB!) :                                                    */
/*   1. Settings Pin Encoder, Resolusi, dan Tipe encoding di omniPos.h      */
/*   2. Deklarasi penggunaan library omniPos pada bagian deklarasi encoder  */
/*                                                                          */
/****************************************************************************/
/*                                                                          */
/*    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 kotak    => Aktif servo Launcher                                 */
/*                                                                          */
/****************************************************************************/


#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 pinservo1 PC_8
//PC 9
#define pinservo2 PC_9

float K_enc = pi*diaEncoder;
float K_robot = pi*diaRobot;

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

float KpX=0.5, KpY=0.5, Kp_tetha=0.03;

Timer waktu;
//float jarakGlobalY;

// Deklarasi variabel PID
//PID PID(0.992,0.000,0.81,0.001); //(P,I,D, time sampling)

// Deklarasi variabel encoder
encoderKRAI encoderDepan( PB_14, PB_13, 2000, encoderKRAI::X2_ENCODING);   //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
encoderKRAI encoderBelakang( PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING);
encoderKRAI encoderKanan( PD_2, PC_12, 720, encoderKRAI::X2_ENCODING);
encoderKRAI encoderKiri( PC_11, PC_10, 2000, encoderKRAI::X2_ENCODING);

// Deklarasi variabel motor
Motor motor1(PA_8, PB_0 , PC_15); // pwm, fwd, rev
Motor motor2(PA_11, PA_6 ,PA_5); // pwm, fwd, rev
Motor motor3(PA_9, PC_2 , PC_3); // pwm, fwd, rev
Motor motor4(PA_10, PB_5 ,PB_4); // pwm, fwd, rev

//Motor Atas
Motor motorld(PB_8, PB_1 , PA_13); // pwm, fwd, rev
Motor motorlb(PB_9, PA_12, PC_5 ); // pwm, fwd, rev

//Servo Atas
Servo servoS(PC_9);
Servo servoB(PC_7);

// Deklarasi variabel posisi robot
//robotPos posisi();

// Inisialisasi  Pin TX-RX Joystik dan PC
//Serial pc(PA_0,PA_1);
//Serial pc(USBTX,USBRX);
// Deklarasi Variabel Global
/*
 * posX dan posY berdasarkan arah robot
 * encoder Depan & Belakang sejajar sumbu Y
 * encoder Kanan & Kiri sejajar sumbu X 
 */
float jarak, posX, posY;
//float keliling = pi*diameterRoda;

void detect_encoder(float speed);
void setCenter();
float getY();
float getX();
float getTetha();

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

// Posisi target
float XT, YT, Tetha;

//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);
    }
    
    // MOTOR Bawah
    switch (case_ger) 
    {
case (1): 
        {       
           Tetha = Tetha - 0.5;
            pivka=true;         
            maju=mundur=analog=kiri=kanan=saka=saki=sbka=sbki=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
            
            pc.printf("pivKa  Xt =%.2f x=%.2f YT=%.2f y=%.2f errx=%.2f erry=%.2f \n",XT,x,YT,y,errX,errY);
            
         
            break;
        }
    case (2):
           {
             Tetha = Tetha + 0.5; 
            
            pivki=true; 
            maju=mundur=kiri=analog=kanan=saka=saki=sbka=sbki=pivka=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
            
            pc.printf("pivKi  Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
            
         
            break;
           }
    case (3):
        {   
           YT = YT + 0.1;
            
            maju=true;             
            mundur=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;

            pc.printf("maju Xt =%.2f x=%.2f YT=%.2f y=%.2f  errx=%.2f erry=%.2f \n",XT,x,YT,y,errX,errY);

            break;
        }
    case (4):
        { 
            YT = YT - 0.1;

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

            pc.printf("mundur Xt =%.2f x=%.2f YT=%.2f y=%.2f errx=%.2f erry=%.2f \n",XT,x,YT,y,errX,errY);

    
            break;
        }
    case (5) :
        {   
            XT = XT + 0.1;
            YT = YT + 0.1;
            
            saka=true; 
            maju=mundur=kiri=kanan=sbka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
            
            pc.printf("saka Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
            
            
            break;
        }
    case (6) :
        {   
             XT = XT + 0.1;
            YT = YT - 0.1;

            
            sbka=true; 
            maju=mundur=kiri=kanan=saka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
            
            pc.printf("sbka  Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);

    
            break;
        }
    case (7) :
        {   
         XT = XT - 0.1;
         YT = YT + 0.1;

            
            saki=true; 
            maju=kiri=kanan=saka=mundur=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
            
            pc.printf("saki Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
            
    
            break;
        }
    case (8) :
        {   
           XT = XT - 0.1;
           YT = YT - 0.1;
         
            pc.printf("sbki Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
            
    
            break;
        }
    case (9) :
        {   
            XT = XT + 0.1;
            kanan=true; 
            maju=kiri=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
            
            pc.printf("Kanan Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
            
            break;
        }
    case (10) :
        {   
          XT = XT - 0.1;
            
            kiri=true; 
            maju=kanan=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
            
            pc.printf("Kiri Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
            
              
            break;
        }
    case (11): 
        {          
                     
         XT = XT + 0.1*x;
         YT = YT + 0.1*y;
                       
            analog=true;
            maju=mundur=kiri=kanan=saka=saki=sbka=sbki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
            
            pc.printf("analog  Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
            
            break;
    }
        default :
        {
          
            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 V1=%.2f V2=%.2f V3=%.2f V4=%.2f\n",V1,V2,V3,V4);
        }  
    }  
}

//Begin Encoder

void setCenter()
{
    encoderDepan.reset();
    encoderBelakang.reset();
    encoderKanan.reset();
    encoderKiri.reset();
}

float getX()
{
    float  jarakEncDpn, jarakEncBlk;
    jarakEncDpn = (encoderDepan.getPulses())/(float)(2000.0)*K_enc;
    jarakEncBlk = (encoderBelakang.getPulses())/(float)(2000.0)*K_enc;
    return (jarakEncDpn-jarakEncBlk)/2;
}

float getY()
{
    float  jarakEncKir, jarakEncKan;
    jarakEncKir = (encoderKiri.getPulses())/(float)(2000.0)*K_enc;
    jarakEncKan = (encoderKanan.getPulses())/(float)(720.0)*K_enc;
    return (jarakEncKir-jarakEncKan)/2;   
}

float getTetha()
{
    float busurDpn, busurBlk, busurKir, busurKan;
    busurDpn = ((encoderDepan.getPulses())/(float)(2000.0)*K_enc)/K_robot*360.0;
    busurBlk = ((encoderBelakang.getPulses())/(float)(2000.0)*K_enc)/K_robot*360.0;
    busurKir = ((encoderKiri.getPulses())/(float)(2000.0)*K_enc)/K_robot*360.0;
    busurKan = ((encoderKanan.getPulses())/(float)(720.0)*K_enc)/K_robot*360.0;
    
    return -(busurDpn+busurBlk+busurKir+busurKan)/4;    
}

void gotoXYT(float xa, float ya, float Ta)
{

        errX = xa-getX();
        Vx = KpX*errX;
        
        errY = ya-getY();
        Vy = KpY*errY;
        
        errT = Ta-getTetha();
        Vt = Kp_tetha*errT;
        
        V1 = Vx+Vy-Vt;
        V2 = Vx-Vy-Vt;
        V3 = -Vx-Vy-Vt;
        V4 = -Vx+Vy-Vt;
        
        if (V1>speed1)
        { V1 = speed1; }
        else if (V1<-speed1)
        { V1 = -speed1; }
        
        if (V2>speed2)
        { V2 = speed2; }
        else if (V2<-speed2)
        { V2 = -speed2; }
        
        if (V3>speed3)
        { V3 = speed3; }
        else if (V3<-speed3)
        { V3 = -speed3; }
        
        if (V4>speed4)
        { V4 = speed4; }
        else if (V4<-speed4)
        { V4 = -speed4; }
        
        if (((errX > 0.05) || (errX<-0.05)) || ((errY > 0.05) || (errY<-0.05)) || ((errT > 0.05) || (errT<-0.05)))
        {
            motor1.speed(V1);
            motor2.speed(V2);
            motor3.speed(V3);
            motor4.speed(V4);    
      //      pc.printf("V1=%.2f \ ", V1);
        }
        else
        {
            motor1.brake(1);
            motor2.brake(1);
            motor3.brake(1);
            motor4.brake(1);
            //_ms(1000);
        }
        //pc.printf("%f\t%f\t%f  ", errX*100,errY*100,errT); 
   //     wait_ms(10);

}
//End Encoder

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>0 and speedB < 0.8){
        speedB = speedB + 0.01;}
    if (joystick.L2>0 and speedB > 0.1){
        speedB = speedB - 0.01;}*/    
    pc.printf("Pwm depan = %.3f\n ", speedL);
}
   
    
    
int main (void)
{
    // Set baud rate - 115200
    joystick.setup();
    pc.baud(115200);
    wait_ms(1000);
    setCenter();
    wait_ms(500);
    
    //Posisi Awal
    XT = 0;
    YT = 0;
    Tetha = 0;
    pc.printf("Ready...\n");
    kalibrasi();
    waktu.start();
    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;
                waktu.reset();
                }  
            if (joystick.silang) {
                XT = 0;
                YT = 0;
                Tetha = 0;
                pc.printf("x..\n");
                }
            speedLauncher();
         
        } else {
            joystick.idle();
            
        }                           
            gotoXYT(XT,YT,Tetha);

    }
}