ihsan hadi / EncoderMotor

PRAWIRA

EncoderMotor.cpp@0:cd1a027f038d, 2019-09-16 (annotated)

Committer:
alienbernamaihsan
Date:
Mon Sep 16 10:06:23 2019 +0000
Revision:
0:cd1a027f038d
PRAWIRA

Who changed what in which revision?

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