KRAI 2017
/
Riset-Odometry
hadah
Fork of Riset-Odometry by
omniPos.cpp@0:b455cd43929c, 2016-11-07 (annotated)
- Committer:
- gustavaditya
- Date:
- Mon Nov 07 12:24:01 2016 +0000
- Revision:
- 0:b455cd43929c
- Child:
- 1:0a124b4468db
Coba-riset-PID
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
gustavaditya | 0:b455cd43929c | 1 | //Includes |
gustavaditya | 0:b455cd43929c | 2 | #include "omniPos.h" |
gustavaditya | 0:b455cd43929c | 3 | #define pi 22/7 |
gustavaditya | 0:b455cd43929c | 4 | #define diaEncoder 0.058 |
gustavaditya | 0:b455cd43929c | 5 | #define PPR 1000 |
gustavaditya | 0:b455cd43929c | 6 | #define diaRobot 0.64 |
gustavaditya | 0:b455cd43929c | 7 | |
gustavaditya | 0:b455cd43929c | 8 | omniPos::omniPos(Wheel wheel) |
gustavaditya | 0:b455cd43929c | 9 | { |
gustavaditya | 0:b455cd43929c | 10 | int indeks; |
gustavaditya | 0:b455cd43929c | 11 | //int alpha[4] = {0,90,180,270}; |
gustavaditya | 0:b455cd43929c | 12 | float K_enc = pi*diaEncoder; |
gustavaditya | 0:b455cd43929c | 13 | float K_robot = pi*diaRobot; |
gustavaditya | 0:b455cd43929c | 14 | |
gustavaditya | 0:b455cd43929c | 15 | switch (wheel) |
gustavaditya | 0:b455cd43929c | 16 | { |
gustavaditya | 0:b455cd43929c | 17 | case (Dkiri) : {indeks=1; break;} |
gustavaditya | 0:b455cd43929c | 18 | case (Dkanan) : {indeks=2; break;} |
gustavaditya | 0:b455cd43929c | 19 | case (Bkanan) : {indeks=3; break;} |
gustavaditya | 0:b455cd43929c | 20 | case (Bkiri) : {indeks=4; break;} |
gustavaditya | 0:b455cd43929c | 21 | } |
gustavaditya | 0:b455cd43929c | 22 | } |
gustavaditya | 0:b455cd43929c | 23 | |
gustavaditya | 0:b455cd43929c | 24 | void omniPos::setCenter(void) |
gustavaditya | 0:b455cd43929c | 25 | { |
gustavaditya | 0:b455cd43929c | 26 | encoderDepan.reset(); |
gustavaditya | 0:b455cd43929c | 27 | encoderBelakang.reset(); |
gustavaditya | 0:b455cd43929c | 28 | encoderKanan.reset(); |
gustavaditya | 0:b455cd43929c | 29 | encoderKiri.reset(); |
gustavaditya | 0:b455cd43929c | 30 | } |
gustavaditya | 0:b455cd43929c | 31 | |
gustavaditya | 0:b455cd43929c | 32 | float getX(void) |
gustavaditya | 0:b455cd43929c | 33 | { |
gustavaditya | 0:b455cd43929c | 34 | float jarakEncDpn, jarakEncBlk; |
gustavaditya | 0:b455cd43929c | 35 | jarakEncDpn = encoderDepan.getRevolutions()*Kel + (encoderDepan.getPulses()%(2*PPR))*Kel/(2*PPR); |
gustavaditya | 0:b455cd43929c | 36 | jarakEncBlk = encoderBelakang.getRevolutions()*Kel + (encoderBelakang.getPulses()%(2*PPR))*Kel/(2*PPR); |
gustavaditya | 0:b455cd43929c | 37 | if (jarakEncDpn>=0 && jarakEncBlk<0) |
gustavaditya | 0:b455cd43929c | 38 | { |
gustavaditya | 0:b455cd43929c | 39 | return (abs(jarakEncDpn)+abs(jarakEncBlk))/2; |
gustavaditya | 0:b455cd43929c | 40 | } |
gustavaditya | 0:b455cd43929c | 41 | else if (jarakEncDpn<0 && jarakEncBlk>0) |
gustavaditya | 0:b455cd43929c | 42 | { |
gustavaditya | 0:b455cd43929c | 43 | return -(abs(jarakEncDpn)+abs(jarakEncBlk))/2; |
gustavaditya | 0:b455cd43929c | 44 | } |
gustavaditya | 0:b455cd43929c | 45 | } |
gustavaditya | 0:b455cd43929c | 46 | |
gustavaditya | 0:b455cd43929c | 47 | float getY(void) |
gustavaditya | 0:b455cd43929c | 48 | { |
gustavaditya | 0:b455cd43929c | 49 | float jarakEncKir, jarakEncKan; |
gustavaditya | 0:b455cd43929c | 50 | jarakEncKir = encoderKiri.getRevolutions()*Kel + (encoderKiri.getPulses()%(2*PPR))*Kel/(2*PPR); |
gustavaditya | 0:b455cd43929c | 51 | jarakEncKan = encoderKanan.getRevolutions()*Kel + (encoderKanan.getPulses()%(2*PPR))*Kel/(2*PPR); |
gustavaditya | 0:b455cd43929c | 52 | if (jarakEncKir>=0 && jarakEncKan<0) |
gustavaditya | 0:b455cd43929c | 53 | { |
gustavaditya | 0:b455cd43929c | 54 | return (abs(jarakEncKir)+abs(jarakEncKan))/2; |
gustavaditya | 0:b455cd43929c | 55 | } |
gustavaditya | 0:b455cd43929c | 56 | else if (jarakEncKir<0 && jarakEncKan>0) |
gustavaditya | 0:b455cd43929c | 57 | { |
gustavaditya | 0:b455cd43929c | 58 | return -(abs(jarakEncKir)+abs(jarakEncKan))/2; |
gustavaditya | 0:b455cd43929c | 59 | } |
gustavaditya | 0:b455cd43929c | 60 | } |
gustavaditya | 0:b455cd43929c | 61 | |
gustavaditya | 0:b455cd43929c | 62 | float getTetha(void) |
gustavaditya | 0:b455cd43929c | 63 | { |
gustavaditya | 0:b455cd43929c | 64 | float jarakEncDpn, jarakEncBlk, jarakEncKir, jarakEncKan; |
gustavaditya | 0:b455cd43929c | 65 | float busurDpn, busurBlk, busurKir, busurKan; |
gustavaditya | 0:b455cd43929c | 66 | jarakEncDpn = encoderDepan.getRevolutions()*Kel + (encoderDepan.getPulses()%(2*PPR))*Kel/(2*PPR); |
gustavaditya | 0:b455cd43929c | 67 | jarakEncBlk = encoderBelakang.getRevolutions()*Kel + (encoderBelakang.getPulses()%(2*PPR))*Kel/(2*PPR); |
gustavaditya | 0:b455cd43929c | 68 | jarakEncKir = encoderKiri.getRevolutions()*Kel + (encoderKiri.getPulses()%(2*PPR))*Kel/(2*PPR); |
gustavaditya | 0:b455cd43929c | 69 | jarakEncKan = encoderKanan.getRevolutions()*Kel + (encoderKanan.getPulses()%(2*PPR))*Kel/(2*PPR); |
gustavaditya | 0:b455cd43929c | 70 | |
gustavaditya | 0:b455cd43929c | 71 | busurDpn = (jarakEncDpn/K_robot)*360; |
gustavaditya | 0:b455cd43929c | 72 | busurBlk = (jarakEncBlk/K_robot)*360; |
gustavaditya | 0:b455cd43929c | 73 | busurKir = (jarakEncKir/K_robot)*360; |
gustavaditya | 0:b455cd43929c | 74 | busurKan = (jarakEncKan/K_robot)*360; |
gustavaditya | 0:b455cd43929c | 75 | |
gustavaditya | 0:b455cd43929c | 76 | return -(busurDpn+busurBlk+busurKir+busurKan)/4; |
gustavaditya | 0:b455cd43929c | 77 | |
gustavaditya | 0:b455cd43929c | 78 | } |
gustavaditya | 0:b455cd43929c | 79 | |
gustavaditya | 0:b455cd43929c | 80 | float getPos() |
gustavaditya | 0:b455cd43929c | 81 | { |
gustavaditya | 0:b455cd43929c | 82 | switch indeks |
gustavaditya | 0:b455cd43929c | 83 | { |
gustavaditya | 0:b455cd43929c | 84 | case(1) : {return getTetha()+90; break;} |
gustavaditya | 0:b455cd43929c | 85 | case(2) : {return getTetha(); break;} |
gustavaditya | 0:b455cd43929c | 86 | case(3) : {return getTetha()+270; break;} |
gustavaditya | 0:b455cd43929c | 87 | case(4) : {return getTetha()+180; break;} |
gustavaditya | 0:b455cd43929c | 88 | } |
gustavaditya | 0:b455cd43929c | 89 | } |
gustavaditya | 0:b455cd43929c | 90 | |
gustavaditya | 0:b455cd43929c | 91 | float getVel(int delta_t) |
gustavaditya | 0:b455cd43929c | 92 | { |
gustavaditya | 0:b455cd43929c | 93 | switch indeks |
gustavaditya | 0:b455cd43929c | 94 | { |
gustavaditya | 0:b455cd43929c | 95 | case(1) : {return getPos(1)/delta_t; break;} |
gustavaditya | 0:b455cd43929c | 96 | case(2) : {return getPos(2)/delta_t; break;} |
gustavaditya | 0:b455cd43929c | 97 | case(3) : {return getPos(3)/delta_t; break;} |
gustavaditya | 0:b455cd43929c | 98 | case(4) : {return getPos(4)/delta_t; break;} |
gustavaditya | 0:b455cd43929c | 99 | } |
gustavaditya | 0:b455cd43929c | 100 | } |