Gustav Aditya Permana
/
Riset-Odometry
Coba dulu
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