Simple PID example for LabVIEW

Dependencies:   mbed

Committer:
simon
Date:
Mon Aug 02 18:53:02 2010 +0000
Revision:
0:e3b759ab4b5c

        

Who changed what in which revision?

UserRevisionLine numberNew contents of line
simon 0:e3b759ab4b5c 1 //****************************************************************************/
simon 0:e3b759ab4b5c 2 //@section LICENSE
simon 0:e3b759ab4b5c 3 //
simon 0:e3b759ab4b5c 4 //Copyright (c) 2010 ARM Limited
simon 0:e3b759ab4b5c 5 //
simon 0:e3b759ab4b5c 6 //Permission is hereby granted, free of charge, to any person obtaining a copy
simon 0:e3b759ab4b5c 7 //of this software and associated documentation files (the "Software"), to deal
simon 0:e3b759ab4b5c 8 //in the Software without restriction, including without limitation the rights
simon 0:e3b759ab4b5c 9 //to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
simon 0:e3b759ab4b5c 10 //copies of the Software, and to permit persons to whom the Software is
simon 0:e3b759ab4b5c 11 //furnished to do so, subject to the following conditions:
simon 0:e3b759ab4b5c 12 //
simon 0:e3b759ab4b5c 13 //The above copyright notice and this permission notice shall be included in
simon 0:e3b759ab4b5c 14 //all copies or substantial portions of the Software.
simon 0:e3b759ab4b5c 15 //
simon 0:e3b759ab4b5c 16 //THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
simon 0:e3b759ab4b5c 17 //IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
simon 0:e3b759ab4b5c 18 //FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
simon 0:e3b759ab4b5c 19 //AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
simon 0:e3b759ab4b5c 20 //LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
simon 0:e3b759ab4b5c 21 //OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
simon 0:e3b759ab4b5c 22 //THE SOFTWARE.
simon 0:e3b759ab4b5c 23 //****************************************************************************/
simon 0:e3b759ab4b5c 24 //@section DESCRIPTION
simon 0:e3b759ab4b5c 25 //
simon 0:e3b759ab4b5c 26 // Quadrature Encoder Interface.
simon 0:e3b759ab4b5c 27 //
simon 0:e3b759ab4b5c 28 // A quadrature encoder consists of two code tracks on a disc which are 90
simon 0:e3b759ab4b5c 29 // degrees out of phase. It can be used to determine how far a wheel has
simon 0:e3b759ab4b5c 30 // rotated, relative to a known starting position.
simon 0:e3b759ab4b5c 31 //
simon 0:e3b759ab4b5c 32 // Only one code track changes at a time leading to a more robust system than
simon 0:e3b759ab4b5c 33 // a single track, because any jitter around any edge won't cause a state
simon 0:e3b759ab4b5c 34 // change as the other track will remain constant.
simon 0:e3b759ab4b5c 35 //
simon 0:e3b759ab4b5c 36 // Encoders can be a homebrew affair, consisting of infrared emitters/receivers
simon 0:e3b759ab4b5c 37 // and paper code tracks consisting of alternating black and white sections;
simon 0:e3b759ab4b5c 38 // alternatively, complete disk and PCB emitter/receiver encoder systems can
simon 0:e3b759ab4b5c 39 // be bought, but the interface, regardless of implementation is the same.
simon 0:e3b759ab4b5c 40 //
simon 0:e3b759ab4b5c 41 // +-----+ +-----+ +-----+
simon 0:e3b759ab4b5c 42 // Channel A | ^ | | | | |
simon 0:e3b759ab4b5c 43 // ---+ ^ +-----+ +-----+ +-----
simon 0:e3b759ab4b5c 44 // ^ ^
simon 0:e3b759ab4b5c 45 // ^ +-----+ +-----+ +-----+
simon 0:e3b759ab4b5c 46 // Channel B ^ | | | | | |
simon 0:e3b759ab4b5c 47 // ------+ +-----+ +-----+ +-----
simon 0:e3b759ab4b5c 48 // ^ ^
simon 0:e3b759ab4b5c 49 // ^ ^
simon 0:e3b759ab4b5c 50 // 90deg
simon 0:e3b759ab4b5c 51 //
simon 0:e3b759ab4b5c 52 // This interface uses X4 encoding which calculates the pulse count based on
simon 0:e3b759ab4b5c 53 // reading the current state after each rising and falling edge of either
simon 0:e3b759ab4b5c 54 // channel.
simon 0:e3b759ab4b5c 55 //
simon 0:e3b759ab4b5c 56 // +-----+ +-----+ +-----+
simon 0:e3b759ab4b5c 57 // Channel A | | | | | |
simon 0:e3b759ab4b5c 58 // ---+ +-----+ +-----+ +-----
simon 0:e3b759ab4b5c 59 // ^ ^ ^ ^ ^
simon 0:e3b759ab4b5c 60 // ^ +-----+ ^ +-----+ ^ +-----+
simon 0:e3b759ab4b5c 61 // Channel B ^ | ^ | ^ | ^ | ^ | |
simon 0:e3b759ab4b5c 62 // ------+ ^ +-----+ ^ +-----+ +--
simon 0:e3b759ab4b5c 63 // ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
simon 0:e3b759ab4b5c 64 // ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
simon 0:e3b759ab4b5c 65 // Pulse count 0 1 2 3 4 5 6 7 8 9 ...
simon 0:e3b759ab4b5c 66 //
simon 0:e3b759ab4b5c 67 // An optional index channel can be used which determines when a full
simon 0:e3b759ab4b5c 68 // revolution has occured.
simon 0:e3b759ab4b5c 69 //
simon 0:e3b759ab4b5c 70 // If a 4 pules per revolution encoder was used, the following would be
simon 0:e3b759ab4b5c 71 // observed.
simon 0:e3b759ab4b5c 72 //
simon 0:e3b759ab4b5c 73 // +-----+ +-----+ +-----+
simon 0:e3b759ab4b5c 74 // Channel A | | | | | |
simon 0:e3b759ab4b5c 75 // ---+ +-----+ +-----+ +-----
simon 0:e3b759ab4b5c 76 // ^ ^ ^ ^ ^
simon 0:e3b759ab4b5c 77 // ^ +-----+ ^ +-----+ ^ +-----+
simon 0:e3b759ab4b5c 78 // Channel B ^ | ^ | ^ | ^ | ^ | |
simon 0:e3b759ab4b5c 79 // ------+ ^ +-----+ ^ +-----+ +--
simon 0:e3b759ab4b5c 80 // ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
simon 0:e3b759ab4b5c 81 // ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
simon 0:e3b759ab4b5c 82 // ^ ^ ^ +--+ ^ ^ +--+ ^
simon 0:e3b759ab4b5c 83 // ^ ^ ^ | | ^ ^ | | ^
simon 0:e3b759ab4b5c 84 // Index ------------+ +--------+ +-----------
simon 0:e3b759ab4b5c 85 // ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
simon 0:e3b759ab4b5c 86 // Pulse count 0 1 2 3 4 5 6 7 8 9 ...
simon 0:e3b759ab4b5c 87 // Rev. count 0 1 2
simon 0:e3b759ab4b5c 88 //
simon 0:e3b759ab4b5c 89 // Rotational position in degrees can be calculated by:
simon 0:e3b759ab4b5c 90 //
simon 0:e3b759ab4b5c 91 // (pulse count / X * N) * 360
simon 0:e3b759ab4b5c 92 //
simon 0:e3b759ab4b5c 93 // Where X is the encoding type [in our case X=4], and N is the number of
simon 0:e3b759ab4b5c 94 // pulses per revolution.
simon 0:e3b759ab4b5c 95 //
simon 0:e3b759ab4b5c 96 // Linear position can be calculated by:
simon 0:e3b759ab4b5c 97 //
simon 0:e3b759ab4b5c 98 // (pulse count / X * N) * (1 / PPI)
simon 0:e3b759ab4b5c 99 //
simon 0:e3b759ab4b5c 100 // Where X is encoding type [in our case X=4], N is the number of pulses per
simon 0:e3b759ab4b5c 101 // revolution, and PPI is pulses per inch, or the equivalent for any other
simon 0:e3b759ab4b5c 102 // unit of displacement. PPI can be calculated by taking the circumference
simon 0:e3b759ab4b5c 103 // of the wheel or encoder disk and dividing it by the number of pulses per
simon 0:e3b759ab4b5c 104 // revolution.
simon 0:e3b759ab4b5c 105 //****************************************************************************/
simon 0:e3b759ab4b5c 106
simon 0:e3b759ab4b5c 107 //****************************************************************************/
simon 0:e3b759ab4b5c 108 // Includes
simon 0:e3b759ab4b5c 109 //****************************************************************************/
simon 0:e3b759ab4b5c 110 #include "QEI.h"
simon 0:e3b759ab4b5c 111
simon 0:e3b759ab4b5c 112 QEI::QEI(PinName channelA, PinName channelB, PinName index, int pulsesPerRev) {
simon 0:e3b759ab4b5c 113
simon 0:e3b759ab4b5c 114 channelA_ = new InterruptIn(channelA);
simon 0:e3b759ab4b5c 115 channelB_ = new InterruptIn(channelB);
simon 0:e3b759ab4b5c 116 index_ = new InterruptIn(index);
simon 0:e3b759ab4b5c 117
simon 0:e3b759ab4b5c 118 pulses_ = 0;
simon 0:e3b759ab4b5c 119 revolutions_ = 0;
simon 0:e3b759ab4b5c 120 pulsesPerRev_ = pulsesPerRev;
simon 0:e3b759ab4b5c 121
simon 0:e3b759ab4b5c 122 //Workout what the current state is.
simon 0:e3b759ab4b5c 123 int chanA = channelA_->read();
simon 0:e3b759ab4b5c 124 int chanB = channelB_->read();
simon 0:e3b759ab4b5c 125
simon 0:e3b759ab4b5c 126 //2-bit state.
simon 0:e3b759ab4b5c 127 currState_ = (chanA << 1) | (chanB);
simon 0:e3b759ab4b5c 128 prevState_ = currState_;
simon 0:e3b759ab4b5c 129
simon 0:e3b759ab4b5c 130 channelA_->rise(this, &QEI::encode);
simon 0:e3b759ab4b5c 131 channelA_->fall(this, &QEI::encode);
simon 0:e3b759ab4b5c 132 channelB_->rise(this, &QEI::encode);
simon 0:e3b759ab4b5c 133 channelB_->fall(this, &QEI::encode);
simon 0:e3b759ab4b5c 134 //Index is optional.
simon 0:e3b759ab4b5c 135 if (index != NC) {
simon 0:e3b759ab4b5c 136 index_->rise(this, &QEI::index);
simon 0:e3b759ab4b5c 137 }
simon 0:e3b759ab4b5c 138
simon 0:e3b759ab4b5c 139 }
simon 0:e3b759ab4b5c 140
simon 0:e3b759ab4b5c 141 void QEI::reset(void) {
simon 0:e3b759ab4b5c 142
simon 0:e3b759ab4b5c 143 pulses_ = 0;
simon 0:e3b759ab4b5c 144 revolutions_ = 0;
simon 0:e3b759ab4b5c 145
simon 0:e3b759ab4b5c 146 }
simon 0:e3b759ab4b5c 147
simon 0:e3b759ab4b5c 148 int QEI::getCurrentState(void) {
simon 0:e3b759ab4b5c 149
simon 0:e3b759ab4b5c 150 return currState_;
simon 0:e3b759ab4b5c 151
simon 0:e3b759ab4b5c 152 }
simon 0:e3b759ab4b5c 153
simon 0:e3b759ab4b5c 154 int QEI::getPulses(void) {
simon 0:e3b759ab4b5c 155
simon 0:e3b759ab4b5c 156 return pulses_;
simon 0:e3b759ab4b5c 157
simon 0:e3b759ab4b5c 158 }
simon 0:e3b759ab4b5c 159
simon 0:e3b759ab4b5c 160 // There are four possible states for a quadrature encoder which correspond to
simon 0:e3b759ab4b5c 161 // 2-bit gray code.
simon 0:e3b759ab4b5c 162 //
simon 0:e3b759ab4b5c 163 // A state change is only valid if of only one bit has changed.
simon 0:e3b759ab4b5c 164 // A state change is invalid if both bits have changed.
simon 0:e3b759ab4b5c 165 //
simon 0:e3b759ab4b5c 166 // Clockwise Rotation ->
simon 0:e3b759ab4b5c 167 //
simon 0:e3b759ab4b5c 168 // 00 01 11 10 00
simon 0:e3b759ab4b5c 169 //
simon 0:e3b759ab4b5c 170 // <- Counter Clockwise Rotation
simon 0:e3b759ab4b5c 171 //
simon 0:e3b759ab4b5c 172 // If we observe any valid state changes going from left to right, we have
simon 0:e3b759ab4b5c 173 // moved one pulse clockwise [we will consider this "backward" or "negative"].
simon 0:e3b759ab4b5c 174 //
simon 0:e3b759ab4b5c 175 // If we observe any valid state changes going from right to left we have
simon 0:e3b759ab4b5c 176 // moved one pulse counter clockwise [we will consider this "forward" or
simon 0:e3b759ab4b5c 177 // "positive"].
simon 0:e3b759ab4b5c 178 //
simon 0:e3b759ab4b5c 179 // We might enter an invalid state for a number of reasons which are hard to
simon 0:e3b759ab4b5c 180 // predict - if this is the case, it is generally safe to ignore it, update
simon 0:e3b759ab4b5c 181 // the state and carry on, with the error correcting itself shortly after.
simon 0:e3b759ab4b5c 182 void QEI::encode(void) {
simon 0:e3b759ab4b5c 183
simon 0:e3b759ab4b5c 184 int change = 0;
simon 0:e3b759ab4b5c 185 int chanA = channelA_->read();
simon 0:e3b759ab4b5c 186 int chanB = channelB_->read();
simon 0:e3b759ab4b5c 187
simon 0:e3b759ab4b5c 188 //2-bit state.
simon 0:e3b759ab4b5c 189 currState_ = (chanA << 1) | (chanB);
simon 0:e3b759ab4b5c 190
simon 0:e3b759ab4b5c 191 //Entered an invalid state, or no change.
simon 0:e3b759ab4b5c 192 if ((currState_ ^ prevState_) == INVALID || currState_ == prevState_) {
simon 0:e3b759ab4b5c 193 //Even if the state was invalid, it will eventually
simon 0:e3b759ab4b5c 194 //correct itself if we simply update the state.
simon 0:e3b759ab4b5c 195 prevState_ = currState_;
simon 0:e3b759ab4b5c 196 }
simon 0:e3b759ab4b5c 197 //Entered a valid state.
simon 0:e3b759ab4b5c 198 else {
simon 0:e3b759ab4b5c 199 //2 bit state. Right hand bit of prev XOR left hand bit of current
simon 0:e3b759ab4b5c 200 //gives 0 if clockwise rotation and 1 if counter clockwise rotation.
simon 0:e3b759ab4b5c 201 change = (prevState_ & PREV_MASK) ^ ((currState_ & CURR_MASK) >> 1);
simon 0:e3b759ab4b5c 202
simon 0:e3b759ab4b5c 203 if (change == 0) {
simon 0:e3b759ab4b5c 204 change = -1;
simon 0:e3b759ab4b5c 205 }
simon 0:e3b759ab4b5c 206
simon 0:e3b759ab4b5c 207 pulses_ -= change;
simon 0:e3b759ab4b5c 208 prevState_ = currState_;
simon 0:e3b759ab4b5c 209 }
simon 0:e3b759ab4b5c 210
simon 0:e3b759ab4b5c 211 }
simon 0:e3b759ab4b5c 212
simon 0:e3b759ab4b5c 213 void QEI::index(void) {
simon 0:e3b759ab4b5c 214
simon 0:e3b759ab4b5c 215 revolutions_++;
simon 0:e3b759ab4b5c 216
simon 0:e3b759ab4b5c 217 }