File content as of revision 2:2a1e06b525dc:

//Includes
#include "omniPos.h"
#define pi 22/7
#define diaEncoder 0.058
#define PPR 1000
#define diaRobot 0.64

omniPos::omniPos(Wheel wheel)
{
    int indeks;
    //int alpha[4] = {0,90,180,270};
    float K_enc = pi*diaEncoder;
    float K_robot = pi*diaRobot;
    
    switch (wheel)
    { 
        case (Dkiri) : {indeks=1; break;}
        case (Dkanan) : {indeks=2; break;}
        case (Bkanan) : {indeks=3; break;}
        case (Bkiri) : {indeks=4; break;}
    }
}

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

float getX(void)
{
    float  jarakEncDpn, jarakEncBlk;
    jarakEncDpn = encoderDepan.getRevolutions()*Kel + (encoderDepan.getPulses()%(2*PPR))*Kel/(2*PPR);
    jarakEncBlk = encoderBelakang.getRevolutions()*Kel + (encoderBelakang.getPulses()%(2*PPR))*Kel/(2*PPR);
    if (jarakEncDpn>=0  && jarakEncBlk<0)
    {
        return (abs(jarakEncDpn)+abs(jarakEncBlk))/2;
    }
    else if (jarakEncDpn<0  && jarakEncBlk>0)
    {
        return -(abs(jarakEncDpn)+abs(jarakEncBlk))/2;
    }
}

float getY(void)
{
    float  jarakEncKir, jarakEncKan;
    jarakEncKir = encoderKiri.getRevolutions()*Kel + (encoderKiri.getPulses()%(2*PPR))*Kel/(2*PPR);
    jarakEncKan = encoderKanan.getRevolutions()*Kel + (encoderKanan.getPulses()%(2*PPR))*Kel/(2*PPR);
    if (jarakEncKir>=0  && jarakEncKan<0)
    {
        return (abs(jarakEncKir)+abs(jarakEncKan))/2;
    }
    else if (jarakEncKir<0  && jarakEncKan>0)
    {
        return -(abs(jarakEncKir)+abs(jarakEncKan))/2;
    }   
}

float getTetha(void)
{
    float jarakEncDpn, jarakEncBlk, jarakEncKir, jarakEncKan;
    float busurDpn, busurBlk, busurKir, busurKan;
    jarakEncDpn = encoderDepan.getRevolutions()*Kel + (encoderDepan.getPulses()%(2*PPR))*Kel/(2*PPR);
    jarakEncBlk = encoderBelakang.getRevolutions()*Kel + (encoderBelakang.getPulses()%(2*PPR))*Kel/(2*PPR);
    jarakEncKir = encoderKiri.getRevolutions()*Kel + (encoderKiri.getPulses()%(2*PPR))*Kel/(2*PPR);
    jarakEncKan = encoderKanan.getRevolutions()*Kel + (encoderKanan.getPulses()%(2*PPR))*Kel/(2*PPR);
    
    busurDpn = (jarakEncDpn/K_robot)*360;
    busurBlk = (jarakEncBlk/K_robot)*360;
    busurKir = (jarakEncKir/K_robot)*360;
    busurKan = (jarakEncKan/K_robot)*360;
    
    return -(busurDpn+busurBlk+busurKir+busurKan)/4; 
      
}

float getPos()
{
    switch indeks
    {
        case(1) : {return getTetha()+90; break;}
        case(2) : {return getTetha(); break;}
        case(3) : {return getTetha()+270; break;}
        case(4) : {return getTetha()+180; break;}    
    }
}

float getVel(int delta_t)
{
       switch indeks
    {
        case(1) : {return getPos(1)/delta_t; break;}
        case(2) : {return getPos(2)/delta_t; break;}
        case(3) : {return getPos(3)/delta_t; break;}
        case(4) : {return getPos(4)/delta_t; break;}    
    }
}