File content as of revision 2:2a1e06b525dc:

/****************************************************************************/
/*                  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  */
/*                                                                          */
/****************************************************************************/

#include "mbed.h"
#include "Motor.h"
#include "omniPos.h"

#define diameterRoda 10
#define delta_t 0.001

// 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

// Deklarasi variabel encoder
//~ Dkiri untuk Motor 1
//~ Dkanan untuk Motor 2        
//~ Bkanan untuk Motor 3        
//~ Bkiri untuk Motor 4
omniPos omni1(omniPos::Dkiri);
omniPos omni2(omniPos::Dkanan);
omniPos omni3(omniPos::Bkiri);
omniPos omni4(omniPos::Bkiri);

// Inisialisasi  Pin TX-RX Joystik dan PC
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()
{
    int pv;
    // Motor1
    
    PID.setProcessValue(omni1.getVel(delta_t));
    PID.setSetPoint();
}

int main()
{
    pc.baud(115200);
    
}

void gerakKanan()
{
    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;
        }    
}

void gerakKiri()
{
    float revDepan, revBelakang, revKanan, revKiri;
    float tempX, tempY;
    
    revDepan    = encoderDepan.getRevolutions();
    revBelakang = encoderBelakang.getRevolutions();
    revKanan    = encoderKanan.getRevolutions();
    revKiri     = encoderKiri.getRevolutions();    
}