KRAI 2017
/
Riset-Odometry
hadah
Fork of Riset-Odometry by
omniPos.cpp
- Committer:
- Najib_irvani
- Date:
- 2016-11-07
- Revision:
- 1:0a124b4468db
- Parent:
- 0:b455cd43929c
File content as of revision 1:0a124b4468db:
//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/(2*PPR); jarakEncBlk = encoderBelakang.getRevolutions()*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/(2*PPR); jarakEncKan = encoderKanan.getRevolutions()*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/(2*PPR); jarakEncBlk = encoderBelakang.getRevolutions()*Kel/(2*PPR); jarakEncKir = encoderKiri.getRevolutions()*Kel/(2*PPR); jarakEncKan = encoderKanan.getRevolutions()*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;} } }