enc
Fork of QEI by
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?
User | Revision | Line number | New 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 | */ |