hidaka sato / QEI_simple

simple QEI library. no return speed, only return angle.

Dependents:   WRS_mechanamu_test

QEI.cpp@1:6fa863d09d45, 2018-11-14 (annotated)

Committer:
sgrsn
Date:
Wed Nov 14 02:18:58 2018 +0000
Revision:
1:6fa863d09d45
Parent:
0:bffc97496048 
Add getDisAngle for Odmetry

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sgrsn 0:bffc97496048 1 #include "QEI.h"
sgrsn 0:bffc97496048 2
sgrsn 0:bffc97496048 3 const int8_t encodeTable[] = {0, -1, 1, 0, 1, 0, 0, -1, -1, 0, 0, 1, 0, 1, -1, 0 };
sgrsn 0:bffc97496048 4
sgrsn 1:6fa863d09d45 5 QEI::QEI(PinName A, PinName B, int ppr, Timer *t) : channelA(A), channelB(B)
sgrsn 0:bffc97496048 6 {
sgrsn 1:6fa863d09d45 7 timer = t;
sgrsn 0:bffc97496048 8 channelA.rise(this, &QEI::encode);
sgrsn 0:bffc97496048 9 channelB.rise(this, &QEI::encode);
sgrsn 0:bffc97496048 10 channelA.fall(this, &QEI::encode);
sgrsn 0:bffc97496048 11 channelB.fall(this, &QEI::encode);
sgrsn 0:bffc97496048 12 _ppr = ppr;
sgrsn 0:bffc97496048 13 currState = 0;
sgrsn 0:bffc97496048 14 prevState = 0;
sgrsn 0:bffc97496048 15 position = 0;
sgrsn 1:6fa863d09d45 16 last_position = 0;
sgrsn 1:6fa863d09d45 17 time = timer->read();
sgrsn 0:bffc97496048 18 }
sgrsn 0:bffc97496048 19
sgrsn 0:bffc97496048 20 float QEI::getAngle()
sgrsn 0:bffc97496048 21 {
sgrsn 0:bffc97496048 22 return float(position) * 360.0/(_ppr*4.0);
sgrsn 0:bffc97496048 23 }
sgrsn 0:bffc97496048 24
sgrsn 1:6fa863d09d45 25 float QEI::getSpeed()
sgrsn 1:6fa863d09d45 26 {
sgrsn 1:6fa863d09d45 27 float dt = timer->read() - time;
sgrsn 1:6fa863d09d45 28 float dangle = float(position - last_position) * 360.0/(_ppr*4.0);
sgrsn 1:6fa863d09d45 29 float speed = dangle / dt;
sgrsn 1:6fa863d09d45 30 time = timer->read();
sgrsn 1:6fa863d09d45 31 last_position = position;
sgrsn 1:6fa863d09d45 32 return speed;
sgrsn 1:6fa863d09d45 33 }
sgrsn 1:6fa863d09d45 34
sgrsn 1:6fa863d09d45 35 float QEI::getDisAngle()
sgrsn 1:6fa863d09d45 36 {
sgrsn 1:6fa863d09d45 37 float dangle = float(position - last_position) * 360.0/(_ppr*4.0);
sgrsn 1:6fa863d09d45 38 last_position = position;
sgrsn 1:6fa863d09d45 39 return dangle;
sgrsn 1:6fa863d09d45 40 }
sgrsn 1:6fa863d09d45 41
sgrsn 1:6fa863d09d45 42 void QEI::reset()
sgrsn 1:6fa863d09d45 43 {
sgrsn 1:6fa863d09d45 44 position = 0;
sgrsn 1:6fa863d09d45 45 last_position = 0;
sgrsn 1:6fa863d09d45 46 }
sgrsn 1:6fa863d09d45 47
sgrsn 0:bffc97496048 48 void QEI::encode(void)
sgrsn 0:bffc97496048 49 {
sgrsn 0:bffc97496048 50 int8_t chanA = channelA.read();
sgrsn 0:bffc97496048 51 int8_t chanB = channelB.read();
sgrsn 0:bffc97496048 52 currState = chanA | (chanB << 1);
sgrsn 0:bffc97496048 53
sgrsn 0:bffc97496048 54 if (prevState != currState) {
sgrsn 0:bffc97496048 55 position += encodeTable[currState | (prevState<<2)];
sgrsn 0:bffc97496048 56 prevState = currState;
sgrsn 0:bffc97496048 57 }
sgrsn 0:bffc97496048 58 }