M Tamamul Firdaus / encoderKRAI

16 Mei 2019

encoderKRAI.cpp@0:88b904c4ffbb, 2019-05-16 (annotated)

Committer:
tamamfirdaus
Date:
Thu May 16 08:42:16 2019 +0000
Revision:
0:88b904c4ffbb
16 Mei 2019;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tamamfirdaus 0:88b904c4ffbb 1 /********************************************************/
tamamfirdaus 0:88b904c4ffbb 2 /* Library untuk pembacaan Encoder */
tamamfirdaus 0:88b904c4ffbb 3 /* Adopsi dari QEI */
tamamfirdaus 0:88b904c4ffbb 4 /* Bismillahirahmanirrahim */
tamamfirdaus 0:88b904c4ffbb 5 /* Encoder yang sudah dicoba : */
tamamfirdaus 0:88b904c4ffbb 6 /* 1. Autonics */
tamamfirdaus 0:88b904c4ffbb 7 /* 2. Encoder bawaan Motor */
tamamfirdaus 0:88b904c4ffbb 8 /* */
tamamfirdaus 0:88b904c4ffbb 9 /* ______________________ */
tamamfirdaus 0:88b904c4ffbb 10 /* |______Autonics______| */
tamamfirdaus 0:88b904c4ffbb 11 /* | Out A = Input 1 | */
tamamfirdaus 0:88b904c4ffbb 12 /* | Out B = Input 2 | */
tamamfirdaus 0:88b904c4ffbb 13 /* | 5V | */
tamamfirdaus 0:88b904c4ffbb 14 /* |_Gnd________________| */
tamamfirdaus 0:88b904c4ffbb 15 /* */
tamamfirdaus 0:88b904c4ffbb 16 /********************************************************/
tamamfirdaus 0:88b904c4ffbb 17
tamamfirdaus 0:88b904c4ffbb 18 #include "mbed.h"
tamamfirdaus 0:88b904c4ffbb 19 #include "encoderKRAI.h"
tamamfirdaus 0:88b904c4ffbb 20
tamamfirdaus 0:88b904c4ffbb 21 encoderKRAI::encoderKRAI(PinName channelA,
tamamfirdaus 0:88b904c4ffbb 22 PinName channelB,
tamamfirdaus 0:88b904c4ffbb 23 int pulsesPerRev,
tamamfirdaus 0:88b904c4ffbb 24 Encoding encoding) : channelA_(channelA), channelB_(channelB)
tamamfirdaus 0:88b904c4ffbb 25 {
tamamfirdaus 0:88b904c4ffbb 26 pulses_ = 0;
tamamfirdaus 0:88b904c4ffbb 27 revolutions_ = 0;
tamamfirdaus 0:88b904c4ffbb 28 pulsesPerRev_ = pulsesPerRev;
tamamfirdaus 0:88b904c4ffbb 29 encoding_ = encoding;
tamamfirdaus 0:88b904c4ffbb 30
tamamfirdaus 0:88b904c4ffbb 31 //Workout what the current state is.
tamamfirdaus 0:88b904c4ffbb 32 int chanA = channelA_.read();
tamamfirdaus 0:88b904c4ffbb 33 int chanB = channelB_.read();
tamamfirdaus 0:88b904c4ffbb 34
tamamfirdaus 0:88b904c4ffbb 35 //2-bit state.
tamamfirdaus 0:88b904c4ffbb 36 currState_ = (chanA << 1) | (chanB);
tamamfirdaus 0:88b904c4ffbb 37 prevState_ = currState_;
tamamfirdaus 0:88b904c4ffbb 38
tamamfirdaus 0:88b904c4ffbb 39 //X2 encoding uses interrupts on only channel A.
tamamfirdaus 0:88b904c4ffbb 40 //X4 encoding uses interrupts on channel A,
tamamfirdaus 0:88b904c4ffbb 41 //and on channel B.
tamamfirdaus 0:88b904c4ffbb 42 channelA_.rise(this, &encoderKRAI::encode);
tamamfirdaus 0:88b904c4ffbb 43 channelA_.fall(this, &encoderKRAI::encode);
tamamfirdaus 0:88b904c4ffbb 44
tamamfirdaus 0:88b904c4ffbb 45 //If we're using X4 encoding, then attach interrupts to channel B too.
tamamfirdaus 0:88b904c4ffbb 46 if (encoding == X4_ENCODING) {
tamamfirdaus 0:88b904c4ffbb 47 channelB_.rise(this, &encoderKRAI::encode);
tamamfirdaus 0:88b904c4ffbb 48 channelB_.fall(this, &encoderKRAI::encode);
tamamfirdaus 0:88b904c4ffbb 49 }
tamamfirdaus 0:88b904c4ffbb 50 }
tamamfirdaus 0:88b904c4ffbb 51
tamamfirdaus 0:88b904c4ffbb 52 void encoderKRAI::reset(void) {
tamamfirdaus 0:88b904c4ffbb 53
tamamfirdaus 0:88b904c4ffbb 54 pulses_ = 0;
tamamfirdaus 0:88b904c4ffbb 55 revolutions_ = 0;
tamamfirdaus 0:88b904c4ffbb 56
tamamfirdaus 0:88b904c4ffbb 57 }
tamamfirdaus 0:88b904c4ffbb 58
tamamfirdaus 0:88b904c4ffbb 59 int encoderKRAI::getPulses(void) {
tamamfirdaus 0:88b904c4ffbb 60
tamamfirdaus 0:88b904c4ffbb 61 return pulses_;
tamamfirdaus 0:88b904c4ffbb 62
tamamfirdaus 0:88b904c4ffbb 63 }
tamamfirdaus 0:88b904c4ffbb 64
tamamfirdaus 0:88b904c4ffbb 65 int encoderKRAI::getRevolutions(void) {
tamamfirdaus 0:88b904c4ffbb 66
tamamfirdaus 0:88b904c4ffbb 67 revolutions_ = pulses_ / pulsesPerRev_;
tamamfirdaus 0:88b904c4ffbb 68 return revolutions_;
tamamfirdaus 0:88b904c4ffbb 69
tamamfirdaus 0:88b904c4ffbb 70 }
tamamfirdaus 0:88b904c4ffbb 71
tamamfirdaus 0:88b904c4ffbb 72 /***************************************
tamamfirdaus 0:88b904c4ffbb 73 * Perhitungan Pulse Encoder
tamamfirdaus 0:88b904c4ffbb 74 ***************************************/
tamamfirdaus 0:88b904c4ffbb 75
tamamfirdaus 0:88b904c4ffbb 76 void encoderKRAI::encode(void) {
tamamfirdaus 0:88b904c4ffbb 77
tamamfirdaus 0:88b904c4ffbb 78 int change = 0;
tamamfirdaus 0:88b904c4ffbb 79 int chanA = channelA_.read();
tamamfirdaus 0:88b904c4ffbb 80 int chanB = channelB_.read();
tamamfirdaus 0:88b904c4ffbb 81
tamamfirdaus 0:88b904c4ffbb 82 //2-bit state.
tamamfirdaus 0:88b904c4ffbb 83 currState_ = (chanA << 1) | (chanB);
tamamfirdaus 0:88b904c4ffbb 84
tamamfirdaus 0:88b904c4ffbb 85 if (encoding_ == X2_ENCODING) {
tamamfirdaus 0:88b904c4ffbb 86
tamamfirdaus 0:88b904c4ffbb 87 //11->00->11->00 is counter clockwise rotation or "forward".
tamamfirdaus 0:88b904c4ffbb 88 if ((prevState_ == 0x3 && currState_ == 0x0) ||
tamamfirdaus 0:88b904c4ffbb 89 (prevState_ == 0x0 && currState_ == 0x3)) {
tamamfirdaus 0:88b904c4ffbb 90
tamamfirdaus 0:88b904c4ffbb 91 pulses_++;
tamamfirdaus 0:88b904c4ffbb 92
tamamfirdaus 0:88b904c4ffbb 93 }
tamamfirdaus 0:88b904c4ffbb 94 //10->01->10->01 is clockwise rotation or "backward".
tamamfirdaus 0:88b904c4ffbb 95 else if ((prevState_ == 0x2 && currState_ == 0x1) ||
tamamfirdaus 0:88b904c4ffbb 96 (prevState_ == 0x1 && currState_ == 0x2)) {
tamamfirdaus 0:88b904c4ffbb 97
tamamfirdaus 0:88b904c4ffbb 98 pulses_--;
tamamfirdaus 0:88b904c4ffbb 99
tamamfirdaus 0:88b904c4ffbb 100 }
tamamfirdaus 0:88b904c4ffbb 101
tamamfirdaus 0:88b904c4ffbb 102 } else if (encoding_ == X4_ENCODING) {
tamamfirdaus 0:88b904c4ffbb 103
tamamfirdaus 0:88b904c4ffbb 104 //Entered a new valid state.
tamamfirdaus 0:88b904c4ffbb 105 if (((currState_ ^ prevState_) != INVALID) && (currState_ != prevState_)) {
tamamfirdaus 0:88b904c4ffbb 106 //2 bit state. Right hand bit of prev XOR left hand bit of current
tamamfirdaus 0:88b904c4ffbb 107 //gives 0 if clockwise rotation and 1 if counter clockwise rotation.
tamamfirdaus 0:88b904c4ffbb 108 change = (prevState_ & PREV_MASK) ^ ((currState_ & CURR_MASK) >> 1);
tamamfirdaus 0:88b904c4ffbb 109
tamamfirdaus 0:88b904c4ffbb 110 if (change == 0) {
tamamfirdaus 0:88b904c4ffbb 111 change = -1;
tamamfirdaus 0:88b904c4ffbb 112 }
tamamfirdaus 0:88b904c4ffbb 113
tamamfirdaus 0:88b904c4ffbb 114 pulses_ -= change;
tamamfirdaus 0:88b904c4ffbb 115 }
tamamfirdaus 0:88b904c4ffbb 116
tamamfirdaus 0:88b904c4ffbb 117 }
tamamfirdaus 0:88b904c4ffbb 118
tamamfirdaus 0:88b904c4ffbb 119 prevState_ = currState_;
tamamfirdaus 0:88b904c4ffbb 120
tamamfirdaus 0:88b904c4ffbb 121 }