KRAI 2017 / Mbed 2 deprecated MainProgram_BaseBaru_fix_omni_20April

paling baru

Dependencies:   DigitDisplay Motor PID Ping mbed millis

Fork of MainProgram_BaseBaru_fix_omni_11April by KRAI 2017

encoderKRAI.cpp@5:3aa203218306, 2016-11-22 (annotated)

Committer:
rahmadirizki18
Date:
Tue Nov 22 15:30:31 2016 +0000
Revision:
5:3aa203218306
lintasan masih curve, balik ke posisi awal masih error besar

Who changed what in which revision?

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