Isro Syaeful Iman / odometry_v1

t

encoderKRAI.cpp@0:fbcacd686ff4, 2017-10-19 (annotated)

Committer:
gatulz
Date:
Thu Oct 19 15:37:57 2017 +0000
Revision:
0:fbcacd686ff4
coba;

Who changed what in which revision?

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