TaPeng / Mbed 2 deprecated BacaMotor

BacaMotor

Dependencies:   mbed Motornew millis

encoderKRAI.cpp@0:b6f3a4e90aff, 2019-03-18 (annotated)

Committer:
tamamfirdaus
Date:
Mon Mar 18 15:15:43 2019 +0000
Revision:
0:b6f3a4e90aff
BacaMotor;

Who changed what in which revision?

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