Kei Ohta / enc

enc

Dependents:   ToyBox ohta

Fork of QEI by Aaron Berk

QEI.cpp@1:7e60713aefc2, 2013-12-09 (annotated)

Committer:
ohtake_i
Date:
Mon Dec 09 23:57:01 2013 +0000
Revision:
1:7e60713aefc2
Parent:
0:5c2ad81551aa 
for izumi and ayami

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aberk 0:5c2ad81551aa 1
aberk 0:5c2ad81551aa 2 #include "QEI.h"
aberk 0:5c2ad81551aa 3
aberk 0:5c2ad81551aa 4 QEI::QEI(PinName channelA,
aberk 0:5c2ad81551aa 5 PinName channelB,
aberk 0:5c2ad81551aa 6 PinName index,
aberk 0:5c2ad81551aa 7 int pulsesPerRev,
aberk 0:5c2ad81551aa 8 Encoding encoding) : channelA_(channelA), channelB_(channelB),
aberk 0:5c2ad81551aa 9 index_(index) {
aberk 0:5c2ad81551aa 10
aberk 0:5c2ad81551aa 11 pulses_ = 0;
aberk 0:5c2ad81551aa 12 revolutions_ = 0;
aberk 0:5c2ad81551aa 13 pulsesPerRev_ = pulsesPerRev;
aberk 0:5c2ad81551aa 14 encoding_ = encoding;
aberk 0:5c2ad81551aa 15
aberk 0:5c2ad81551aa 16 //Workout what the current state is.
aberk 0:5c2ad81551aa 17 int chanA = channelA_.read();
aberk 0:5c2ad81551aa 18 int chanB = channelB_.read();
aberk 0:5c2ad81551aa 19
aberk 0:5c2ad81551aa 20 //2-bit state.
aberk 0:5c2ad81551aa 21 currState_ = (chanA << 1) | (chanB);
aberk 0:5c2ad81551aa 22 prevState_ = currState_;
aberk 0:5c2ad81551aa 23
aberk 0:5c2ad81551aa 24 //X2 encoding uses interrupts on only channel A.
aberk 0:5c2ad81551aa 25 //X4 encoding uses interrupts on channel A,
aberk 0:5c2ad81551aa 26 //and on channel B.
aberk 0:5c2ad81551aa 27 channelA_.rise(this, &QEI::encode);
aberk 0:5c2ad81551aa 28 channelA_.fall(this, &QEI::encode);
aberk 0:5c2ad81551aa 29
aberk 0:5c2ad81551aa 30 //If we're using X4 encoding, then attach interrupts to channel B too.
aberk 0:5c2ad81551aa 31 if (encoding == X4_ENCODING) {
aberk 0:5c2ad81551aa 32 channelB_.rise(this, &QEI::encode);
aberk 0:5c2ad81551aa 33 channelB_.fall(this, &QEI::encode);
aberk 0:5c2ad81551aa 34 }
aberk 0:5c2ad81551aa 35 //Index is optional.
aberk 0:5c2ad81551aa 36 if (index != NC) {
aberk 0:5c2ad81551aa 37 index_.rise(this, &QEI::index);
aberk 0:5c2ad81551aa 38 }
aberk 0:5c2ad81551aa 39 }
aberk 0:5c2ad81551aa 40
aberk 0:5c2ad81551aa 41 void QEI::reset(void) {
aberk 0:5c2ad81551aa 42 pulses_ = 0;
aberk 0:5c2ad81551aa 43 revolutions_ = 0;
aberk 0:5c2ad81551aa 44 }
aberk 0:5c2ad81551aa 45
aberk 0:5c2ad81551aa 46 int QEI::getCurrentState(void) {
aberk 0:5c2ad81551aa 47 return currState_;
aberk 0:5c2ad81551aa 48 }
aberk 0:5c2ad81551aa 49
aberk 0:5c2ad81551aa 50 int QEI::getPulses(void) {
aberk 0:5c2ad81551aa 51 return pulses_;
aberk 0:5c2ad81551aa 52 }
aberk 0:5c2ad81551aa 53
aberk 0:5c2ad81551aa 54 int QEI::getRevolutions(void) {
aberk 0:5c2ad81551aa 55 return revolutions_;
aberk 0:5c2ad81551aa 56 }
aberk 0:5c2ad81551aa 57
aberk 0:5c2ad81551aa 58 void QEI::encode(void) {
aberk 0:5c2ad81551aa 59 int change = 0;
aberk 0:5c2ad81551aa 60 int chanA = channelA_.read();
aberk 0:5c2ad81551aa 61 int chanB = channelB_.read();
aberk 0:5c2ad81551aa 62 //2-bit state.
aberk 0:5c2ad81551aa 63 currState_ = (chanA << 1) | (chanB);
aberk 0:5c2ad81551aa 64
aberk 0:5c2ad81551aa 65 if (encoding_ == X2_ENCODING) {
aberk 0:5c2ad81551aa 66 //11->00->11->00 is counter clockwise rotation or "forward".
aberk 0:5c2ad81551aa 67 if ((prevState_ == 0x3 && currState_ == 0x0) ||
aberk 0:5c2ad81551aa 68 (prevState_ == 0x0 && currState_ == 0x3)) {
aberk 0:5c2ad81551aa 69 pulses_++;
aberk 0:5c2ad81551aa 70 }
aberk 0:5c2ad81551aa 71 //10->01->10->01 is clockwise rotation or "backward".
aberk 0:5c2ad81551aa 72 else if ((prevState_ == 0x2 && currState_ == 0x1) ||
aberk 0:5c2ad81551aa 73 (prevState_ == 0x1 && currState_ == 0x2)) {
aberk 0:5c2ad81551aa 74 pulses_--;
aberk 0:5c2ad81551aa 75 }
aberk 0:5c2ad81551aa 76 } else if (encoding_ == X4_ENCODING) {
aberk 0:5c2ad81551aa 77 //Entered a new valid state.
aberk 0:5c2ad81551aa 78 if (((currState_ ^ prevState_) != INVALID) && (currState_ != prevState_)) {
aberk 0:5c2ad81551aa 79 //2 bit state. Right hand bit of prev XOR left hand bit of current
aberk 0:5c2ad81551aa 80 //gives 0 if clockwise rotation and 1 if counter clockwise rotation.
aberk 0:5c2ad81551aa 81 change = (prevState_ & PREV_MASK) ^ ((currState_ & CURR_MASK) >> 1);
aberk 0:5c2ad81551aa 82 if (change == 0) {
aberk 0:5c2ad81551aa 83 change = -1;
aberk 0:5c2ad81551aa 84 }
aberk 0:5c2ad81551aa 85 pulses_ -= change;
aberk 0:5c2ad81551aa 86 }
aberk 0:5c2ad81551aa 87 }
aberk 0:5c2ad81551aa 88 prevState_ = currState_;
aberk 0:5c2ad81551aa 89 }
aberk 0:5c2ad81551aa 90
aberk 0:5c2ad81551aa 91 void QEI::index(void) {
aberk 0:5c2ad81551aa 92 revolutions_++;
aberk 0:5c2ad81551aa 93 }
ohtake_i 1:7e60713aefc2 94 /*
ohtake_i 1:7e60713aefc2 95 void QEI::move(float theta_z) {
ohtake_i 1:7e60713aefc2 96 float tmp = theta_z.getPulses();
ohtake_i 1:7e60713aefc2 97 return tmp;
ohtake_i 1:7e60713aefc2 98 }
ohtake_i 1:7e60713aefc2 99 */