KRAI 2017 / Mbed 2 deprecated Riset-PID-Encoder

On Progress. Di baca pelan-pelan :'

Dependencies:   Motor PID QEI mbed

Fork of Riset-Odometry by Gustav Aditya Permana

omniPos.cpp@2:2a1e06b525dc, 2016-11-07 (annotated)

Committer:
gustavaditya
Date:
Mon Nov 07 12:42:58 2016 +0000
Revision:
2:2a1e06b525dc
Parent:
0:b455cd43929c 
On Progress

Who changed what in which revision?

UserRevisionLine numberNew 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 }