Magang KRAI 2019 / encoderKRAI

library encoder

Dependents:   Project_Plum OnMotor DataMotor bacaencoder

encoderKRAI.cpp@0:8a7fb008561c, 2019-02-03 (annotated)

Committer:
yafieabdillah
Date:
Sun Feb 03 15:51:54 2019 +0000
Revision:
0:8a7fb008561c
ghgy

Who changed what in which revision?

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