KRAI 2017 / Mbed 2 deprecated Test_PID2

PID Kontrol atas, tanpa library. Baru P dan D, tapi D error

Dependencies:   Motor mbed millis

Fork of Test_PID by KRAI 2017

encoderKRAI.cpp@4:8e6dfc416ecf, 2017-02-09 (annotated)

Committer:
Joshua23
Date:
Thu Feb 09 14:34:10 2017 +0000
Revision:
4:8e6dfc416ecf
coba

Who changed what in which revision?

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