Diff: omniPos.cpp

Revision:

0:b455cd43929c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/omniPos.cpp	Mon Nov 07 12:24:01 2016 +0000
@@ -0,0 +1,100 @@
+//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;}    
+    }
+}
\ No newline at end of file