EL4121 Embedded System / Mbed OS odometry_omni_3roda_v2

PID sudah ternormalisasi

Dependencies:   Motor PS_PAD TextLCD mbed-os

Fork of cobaLCDJoyMotor_Thread by EL4121 Embedded System

encoderKRAI.cpp@5:d08502bd4abe, 2017-12-16 (annotated)

Committer:
rizqicahyo
Date:
Sat Dec 16 13:40:56 2017 +0000
Revision:
5:d08502bd4abe
Parent:
4:cd5de3b14797 
perbaikan pada algoritma PID dengan dilakukan normalisasi nilai output PID

Who changed what in which revision?

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