omniPos.cpp

00001 //Includes
00002 #include "omniPos.h"
00003 #define pi 22/7
00004 #define diaEncoder 0.058
00005 #define PPR 1000
00006 #define diaRobot 0.64
00007 
00008 omniPos::omniPos(Wheel wheel)
00009 {
00010     int indeks;
00011     //int alpha[4] = {0,90,180,270};
00012     float K_enc = pi*diaEncoder;
00013     float K_robot = pi*diaRobot;
00014     
00015     switch (wheel)
00016     { 
00017         case (Dkiri) : {indeks=1; break;}
00018         case (Dkanan) : {indeks=2; break;}
00019         case (Bkanan) : {indeks=3; break;}
00020         case (Bkiri) : {indeks=4; break;}
00021     }
00022 }
00023 
00024 void omniPos::setCenter(void)
00025 {
00026     encoderDepan.reset();
00027     encoderBelakang.reset();
00028     encoderKanan.reset();
00029     encoderKiri.reset();
00030 }
00031 
00032 float getX(void)
00033 {
00034     float  jarakEncDpn, jarakEncBlk;
00035     jarakEncDpn = encoderDepan.getRevolutions()*Kel + (encoderDepan.getPulses()%(2*PPR))*Kel/(2*PPR);
00036     jarakEncBlk = encoderBelakang.getRevolutions()*Kel + (encoderBelakang.getPulses()%(2*PPR))*Kel/(2*PPR);
00037     if (jarakEncDpn>=0  && jarakEncBlk<0)
00038     {
00039         return (abs(jarakEncDpn)+abs(jarakEncBlk))/2;
00040     }
00041     else if (jarakEncDpn<0  && jarakEncBlk>0)
00042     {
00043         return -(abs(jarakEncDpn)+abs(jarakEncBlk))/2;
00044     }
00045 }
00046 
00047 float getY(void)
00048 {
00049     float  jarakEncKir, jarakEncKan;
00050     jarakEncKir = encoderKiri.getRevolutions()*Kel + (encoderKiri.getPulses()%(2*PPR))*Kel/(2*PPR);
00051     jarakEncKan = encoderKanan.getRevolutions()*Kel + (encoderKanan.getPulses()%(2*PPR))*Kel/(2*PPR);
00052     if (jarakEncKir>=0  && jarakEncKan<0)
00053     {
00054         return (abs(jarakEncKir)+abs(jarakEncKan))/2;
00055     }
00056     else if (jarakEncKir<0  && jarakEncKan>0)
00057     {
00058         return -(abs(jarakEncKir)+abs(jarakEncKan))/2;
00059     }   
00060 }
00061 
00062 float getTetha(void)
00063 {
00064     float jarakEncDpn, jarakEncBlk, jarakEncKir, jarakEncKan;
00065     float busurDpn, busurBlk, busurKir, busurKan;
00066     jarakEncDpn = encoderDepan.getRevolutions()*Kel + (encoderDepan.getPulses()%(2*PPR))*Kel/(2*PPR);
00067     jarakEncBlk = encoderBelakang.getRevolutions()*Kel + (encoderBelakang.getPulses()%(2*PPR))*Kel/(2*PPR);
00068     jarakEncKir = encoderKiri.getRevolutions()*Kel + (encoderKiri.getPulses()%(2*PPR))*Kel/(2*PPR);
00069     jarakEncKan = encoderKanan.getRevolutions()*Kel + (encoderKanan.getPulses()%(2*PPR))*Kel/(2*PPR);
00070     
00071     busurDpn = (jarakEncDpn/K_robot)*360;
00072     busurBlk = (jarakEncBlk/K_robot)*360;
00073     busurKir = (jarakEncKir/K_robot)*360;
00074     busurKan = (jarakEncKan/K_robot)*360;
00075     
00076     return -(busurDpn+busurBlk+busurKir+busurKan)/4; 
00077       
00078 }
00079 
00080 float getPos()
00081 {
00082     switch indeks
00083     {
00084         case(1) : {return getTetha()+90; break;}
00085         case(2) : {return getTetha(); break;}
00086         case(3) : {return getTetha()+270; break;}
00087         case(4) : {return getTetha()+180; break;}    
00088     }
00089 }
00090 
00091 float getVel(int delta_t)
00092 {
00093        switch indeks
00094     {
00095         case(1) : {return getPos(1)/delta_t; break;}
00096         case(2) : {return getPos(2)/delta_t; break;}
00097         case(3) : {return getPos(3)/delta_t; break;}
00098         case(4) : {return getPos(4)/delta_t; break;}    
00099     }
00100 }