KRAI 2017 / Mbed 2 deprecated prototype_encoder_launcher_V1

tuning pid

Dependencies:   Motor kalman mbed millis

Fork of prototype_encoder_launcher by KRAI 2017

encoderKRAI.cpp@1:4200f28040d3, 2017-02-09 (annotated)

Committer:
Najib_irvani
Date:
Thu Feb 09 13:46:48 2017 +0000
Revision:
1:4200f28040d3
Parent:
0:bc9c90c7b357 
tuning pid

Who changed what in which revision?

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