KRAI 2017 / Mbed 2 deprecated Riset-PID-Encoder

On Progress. Di baca pelan-pelan :'

Dependencies:   Motor PID QEI mbed

Fork of Riset-Odometry by Gustav Aditya Permana

encoderKRAI.cpp@2:2a1e06b525dc, 2016-11-07 (annotated)

Committer:
gustavaditya
Date:
Mon Nov 07 12:42:58 2016 +0000
Revision:
2:2a1e06b525dc
Parent:
0:b455cd43929c 
On Progress

Who changed what in which revision?

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