Thomas Lew / Mbed 2 deprecated FreeFlyerROS

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed ros_lib_kinetic

QEI.h@7:794bd40830c1, 2021-03-02 (annotated)

Committer:
thomasjlew
Date:
Tue Mar 02 22:49:24 2021 +0000
Revision:
7:794bd40830c1
Parent:
1:40bdbe1a93b7 
Embedded code for RSS 2021 Safe Active Learning submission - increased control frequency / reduced PWM duty cycle.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Knillinux 1:40bdbe1a93b7 1 /**
Knillinux 1:40bdbe1a93b7 2 * @author Aaron Berk
Knillinux 1:40bdbe1a93b7 3 *
Knillinux 1:40bdbe1a93b7 4 * @section LICENSE
Knillinux 1:40bdbe1a93b7 5 *
Knillinux 1:40bdbe1a93b7 6 * Copyright (c) 2010 ARM Limited
Knillinux 1:40bdbe1a93b7 7 *
Knillinux 1:40bdbe1a93b7 8 * Permission is hereby granted, free of charge, to any person obtaining a copy
Knillinux 1:40bdbe1a93b7 9 * of this software and associated documentation files (the "Software"), to deal
Knillinux 1:40bdbe1a93b7 10 * in the Software without restriction, including without limitation the rights
Knillinux 1:40bdbe1a93b7 11 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
Knillinux 1:40bdbe1a93b7 12 * copies of the Software, and to permit persons to whom the Software is
Knillinux 1:40bdbe1a93b7 13 * furnished to do so, subject to the following conditions:
Knillinux 1:40bdbe1a93b7 14 *
Knillinux 1:40bdbe1a93b7 15 * The above copyright notice and this permission notice shall be included in
Knillinux 1:40bdbe1a93b7 16 * all copies or substantial portions of the Software.
Knillinux 1:40bdbe1a93b7 17 *
Knillinux 1:40bdbe1a93b7 18 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
Knillinux 1:40bdbe1a93b7 19 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
Knillinux 1:40bdbe1a93b7 20 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
Knillinux 1:40bdbe1a93b7 21 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
Knillinux 1:40bdbe1a93b7 22 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
Knillinux 1:40bdbe1a93b7 23 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
Knillinux 1:40bdbe1a93b7 24 * THE SOFTWARE.
Knillinux 1:40bdbe1a93b7 25 *
Knillinux 1:40bdbe1a93b7 26 * @section DESCRIPTION
Knillinux 1:40bdbe1a93b7 27 *
Knillinux 1:40bdbe1a93b7 28 * Quadrature Encoder Interface.
Knillinux 1:40bdbe1a93b7 29 *
Knillinux 1:40bdbe1a93b7 30 * A quadrature encoder consists of two code tracks on a disc which are 90
Knillinux 1:40bdbe1a93b7 31 * degrees out of phase. It can be used to determine how far a wheel has
Knillinux 1:40bdbe1a93b7 32 * rotated, relative to a known starting position.
Knillinux 1:40bdbe1a93b7 33 *
Knillinux 1:40bdbe1a93b7 34 * Only one code track changes at a time leading to a more robust system than
Knillinux 1:40bdbe1a93b7 35 * a single track, because any jitter around any edge won't cause a state
Knillinux 1:40bdbe1a93b7 36 * change as the other track will remain constant.
Knillinux 1:40bdbe1a93b7 37 *
Knillinux 1:40bdbe1a93b7 38 * Encoders can be a homebrew affair, consisting of infrared emitters/receivers
Knillinux 1:40bdbe1a93b7 39 * and paper code tracks consisting of alternating black and white sections;
Knillinux 1:40bdbe1a93b7 40 * alternatively, complete disk and PCB emitter/receiver encoder systems can
Knillinux 1:40bdbe1a93b7 41 * be bought, but the interface, regardless of implementation is the same.
Knillinux 1:40bdbe1a93b7 42 *
Knillinux 1:40bdbe1a93b7 43 * +-----+ +-----+ +-----+
Knillinux 1:40bdbe1a93b7 44 * Channel A | ^ | | | | |
Knillinux 1:40bdbe1a93b7 45 * ---+ ^ +-----+ +-----+ +-----
Knillinux 1:40bdbe1a93b7 46 * ^ ^
Knillinux 1:40bdbe1a93b7 47 * ^ +-----+ +-----+ +-----+
Knillinux 1:40bdbe1a93b7 48 * Channel B ^ | | | | | |
Knillinux 1:40bdbe1a93b7 49 * ------+ +-----+ +-----+ +-----
Knillinux 1:40bdbe1a93b7 50 * ^ ^
Knillinux 1:40bdbe1a93b7 51 * ^ ^
Knillinux 1:40bdbe1a93b7 52 * 90deg
Knillinux 1:40bdbe1a93b7 53 *
Knillinux 1:40bdbe1a93b7 54 * The interface uses X2 encoding by default which calculates the pulse count
Knillinux 1:40bdbe1a93b7 55 * based on reading the current state after each rising and falling edge of
Knillinux 1:40bdbe1a93b7 56 * channel A.
Knillinux 1:40bdbe1a93b7 57 *
Knillinux 1:40bdbe1a93b7 58 * +-----+ +-----+ +-----+
Knillinux 1:40bdbe1a93b7 59 * Channel A | | | | | |
Knillinux 1:40bdbe1a93b7 60 * ---+ +-----+ +-----+ +-----
Knillinux 1:40bdbe1a93b7 61 * ^ ^ ^ ^ ^
Knillinux 1:40bdbe1a93b7 62 * ^ +-----+ ^ +-----+ ^ +-----+
Knillinux 1:40bdbe1a93b7 63 * Channel B ^ | ^ | ^ | ^ | ^ | |
Knillinux 1:40bdbe1a93b7 64 * ------+ ^ +-----+ ^ +-----+ +--
Knillinux 1:40bdbe1a93b7 65 * ^ ^ ^ ^ ^
Knillinux 1:40bdbe1a93b7 66 * ^ ^ ^ ^ ^
Knillinux 1:40bdbe1a93b7 67 * Pulse count 0 1 2 3 4 5 ...
Knillinux 1:40bdbe1a93b7 68 *
Knillinux 1:40bdbe1a93b7 69 * This interface can also use X4 encoding which calculates the pulse count
Knillinux 1:40bdbe1a93b7 70 * based on reading the current state after each rising and falling edge of
Knillinux 1:40bdbe1a93b7 71 * either channel.
Knillinux 1:40bdbe1a93b7 72 *
Knillinux 1:40bdbe1a93b7 73 * +-----+ +-----+ +-----+
Knillinux 1:40bdbe1a93b7 74 * Channel A | | | | | |
Knillinux 1:40bdbe1a93b7 75 * ---+ +-----+ +-----+ +-----
Knillinux 1:40bdbe1a93b7 76 * ^ ^ ^ ^ ^
Knillinux 1:40bdbe1a93b7 77 * ^ +-----+ ^ +-----+ ^ +-----+
Knillinux 1:40bdbe1a93b7 78 * Channel B ^ | ^ | ^ | ^ | ^ | |
Knillinux 1:40bdbe1a93b7 79 * ------+ ^ +-----+ ^ +-----+ +--
Knillinux 1:40bdbe1a93b7 80 * ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
Knillinux 1:40bdbe1a93b7 81 * ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
Knillinux 1:40bdbe1a93b7 82 * Pulse count 0 1 2 3 4 5 6 7 8 9 ...
Knillinux 1:40bdbe1a93b7 83 *
Knillinux 1:40bdbe1a93b7 84 * It defaults
Knillinux 1:40bdbe1a93b7 85 *
Knillinux 1:40bdbe1a93b7 86 * An optional index channel can be used which determines when a full
Knillinux 1:40bdbe1a93b7 87 * revolution has occured.
Knillinux 1:40bdbe1a93b7 88 *
Knillinux 1:40bdbe1a93b7 89 * If a 4 pules per revolution encoder was used, with X4 encoding,
Knillinux 1:40bdbe1a93b7 90 * the following would be observed.
Knillinux 1:40bdbe1a93b7 91 *
Knillinux 1:40bdbe1a93b7 92 * +-----+ +-----+ +-----+
Knillinux 1:40bdbe1a93b7 93 * Channel A | | | | | |
Knillinux 1:40bdbe1a93b7 94 * ---+ +-----+ +-----+ +-----
Knillinux 1:40bdbe1a93b7 95 * ^ ^ ^ ^ ^
Knillinux 1:40bdbe1a93b7 96 * ^ +-----+ ^ +-----+ ^ +-----+
Knillinux 1:40bdbe1a93b7 97 * Channel B ^ | ^ | ^ | ^ | ^ | |
Knillinux 1:40bdbe1a93b7 98 * ------+ ^ +-----+ ^ +-----+ +--
Knillinux 1:40bdbe1a93b7 99 * ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
Knillinux 1:40bdbe1a93b7 100 * ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
Knillinux 1:40bdbe1a93b7 101 * ^ ^ ^ +--+ ^ ^ +--+ ^
Knillinux 1:40bdbe1a93b7 102 * ^ ^ ^ | | ^ ^ | | ^
Knillinux 1:40bdbe1a93b7 103 * Index ------------+ +--------+ +-----------
Knillinux 1:40bdbe1a93b7 104 * ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
Knillinux 1:40bdbe1a93b7 105 * Pulse count 0 1 2 3 4 5 6 7 8 9 ...
Knillinux 1:40bdbe1a93b7 106 * Rev. count 0 1 2
Knillinux 1:40bdbe1a93b7 107 *
Knillinux 1:40bdbe1a93b7 108 * Rotational position in degrees can be calculated by:
Knillinux 1:40bdbe1a93b7 109 *
Knillinux 1:40bdbe1a93b7 110 * (pulse count / X * N) * 360
Knillinux 1:40bdbe1a93b7 111 *
Knillinux 1:40bdbe1a93b7 112 * Where X is the encoding type [e.g. X4 encoding => X=4], and N is the number
Knillinux 1:40bdbe1a93b7 113 * of pulses per revolution.
Knillinux 1:40bdbe1a93b7 114 *
Knillinux 1:40bdbe1a93b7 115 * Linear position can be calculated by:
Knillinux 1:40bdbe1a93b7 116 *
Knillinux 1:40bdbe1a93b7 117 * (pulse count / X * N) * (1 / PPI)
Knillinux 1:40bdbe1a93b7 118 *
Knillinux 1:40bdbe1a93b7 119 * Where X is encoding type [e.g. X4 encoding => X=44], N is the number of
Knillinux 1:40bdbe1a93b7 120 * pulses per revolution, and PPI is pulses per inch, or the equivalent for
Knillinux 1:40bdbe1a93b7 121 * any other unit of displacement. PPI can be calculated by taking the
Knillinux 1:40bdbe1a93b7 122 * circumference of the wheel or encoder disk and dividing it by the number
Knillinux 1:40bdbe1a93b7 123 * of pulses per revolution.
Knillinux 1:40bdbe1a93b7 124 */
Knillinux 1:40bdbe1a93b7 125
Knillinux 1:40bdbe1a93b7 126 #ifndef QEI_H
Knillinux 1:40bdbe1a93b7 127 #define QEI_H
Knillinux 1:40bdbe1a93b7 128
Knillinux 1:40bdbe1a93b7 129 /**
Knillinux 1:40bdbe1a93b7 130 * Includes
Knillinux 1:40bdbe1a93b7 131 */
Knillinux 1:40bdbe1a93b7 132 #include "mbed.h"
Knillinux 1:40bdbe1a93b7 133
Knillinux 1:40bdbe1a93b7 134 /**
Knillinux 1:40bdbe1a93b7 135 * Defines
Knillinux 1:40bdbe1a93b7 136 */
Knillinux 1:40bdbe1a93b7 137 #define PREV_MASK 0x1 //Mask for the previous state in determining direction
Knillinux 1:40bdbe1a93b7 138 //of rotation.
Knillinux 1:40bdbe1a93b7 139 #define CURR_MASK 0x2 //Mask for the current state in determining direction
Knillinux 1:40bdbe1a93b7 140 //of rotation.
Knillinux 1:40bdbe1a93b7 141 #define INVALID 0x3 //XORing two states where both bits have changed.
Knillinux 1:40bdbe1a93b7 142
Knillinux 1:40bdbe1a93b7 143 /**
Knillinux 1:40bdbe1a93b7 144 * Quadrature Encoder Interface.
Knillinux 1:40bdbe1a93b7 145 */
Knillinux 1:40bdbe1a93b7 146 class QEI {
Knillinux 1:40bdbe1a93b7 147
Knillinux 1:40bdbe1a93b7 148 public:
Knillinux 1:40bdbe1a93b7 149
Knillinux 1:40bdbe1a93b7 150 typedef enum Encoding {
Knillinux 1:40bdbe1a93b7 151
Knillinux 1:40bdbe1a93b7 152 X2_ENCODING,
Knillinux 1:40bdbe1a93b7 153 X4_ENCODING
Knillinux 1:40bdbe1a93b7 154
Knillinux 1:40bdbe1a93b7 155 } Encoding;
Knillinux 1:40bdbe1a93b7 156
Knillinux 1:40bdbe1a93b7 157 /**
Knillinux 1:40bdbe1a93b7 158 * Constructor.
Knillinux 1:40bdbe1a93b7 159 *
Knillinux 1:40bdbe1a93b7 160 * Reads the current values on channel A and channel B to determine the
Knillinux 1:40bdbe1a93b7 161 * initial state.
Knillinux 1:40bdbe1a93b7 162 *
Knillinux 1:40bdbe1a93b7 163 * Attaches the encode function to the rise/fall interrupt edges of
Knillinux 1:40bdbe1a93b7 164 * channels A and B to perform X4 encoding.
Knillinux 1:40bdbe1a93b7 165 *
Knillinux 1:40bdbe1a93b7 166 * Attaches the index function to the rise interrupt edge of channel index
Knillinux 1:40bdbe1a93b7 167 * (if it is used) to count revolutions.
Knillinux 1:40bdbe1a93b7 168 *
Knillinux 1:40bdbe1a93b7 169 * @param channelA mbed pin for channel A input.
Knillinux 1:40bdbe1a93b7 170 * @param channelB mbed pin for channel B input.
Knillinux 1:40bdbe1a93b7 171 * @param index mbed pin for optional index channel input,
Knillinux 1:40bdbe1a93b7 172 * (pass NC if not needed).
Knillinux 1:40bdbe1a93b7 173 * @param pulsesPerRev Number of pulses in one revolution.
Knillinux 1:40bdbe1a93b7 174 * @param encoding The encoding to use. Uses X2 encoding by default. X2
Knillinux 1:40bdbe1a93b7 175 * encoding uses interrupts on the rising and falling edges
Knillinux 1:40bdbe1a93b7 176 * of only channel A where as X4 uses them on both
Knillinux 1:40bdbe1a93b7 177 * channels.
Knillinux 1:40bdbe1a93b7 178 */
Knillinux 1:40bdbe1a93b7 179 QEI(PinName channelA, PinName channelB, PinName index, int pulsesPerRev, int pulsesPerSpeedMeas, Encoding encoding = X2_ENCODING);
Knillinux 1:40bdbe1a93b7 180
Knillinux 1:40bdbe1a93b7 181 /**
Knillinux 1:40bdbe1a93b7 182 * Reset the encoder.
Knillinux 1:40bdbe1a93b7 183 *
Knillinux 1:40bdbe1a93b7 184 * Sets the pulses and revolutions count to zero.
Knillinux 1:40bdbe1a93b7 185 */
Knillinux 1:40bdbe1a93b7 186 void reset(void);
Knillinux 1:40bdbe1a93b7 187
Knillinux 1:40bdbe1a93b7 188 /**
Knillinux 1:40bdbe1a93b7 189 * Read the state of the encoder.
Knillinux 1:40bdbe1a93b7 190 *
Knillinux 1:40bdbe1a93b7 191 * @return The current state of the encoder as a 2-bit number, where:
Knillinux 1:40bdbe1a93b7 192 * bit 1 = The reading from channel B
Knillinux 1:40bdbe1a93b7 193 * bit 2 = The reading from channel A
Knillinux 1:40bdbe1a93b7 194 */
Knillinux 1:40bdbe1a93b7 195 int getCurrentState(void);
Knillinux 1:40bdbe1a93b7 196
Knillinux 1:40bdbe1a93b7 197 /**
Knillinux 1:40bdbe1a93b7 198 * Read the number of pulses recorded by the encoder.
Knillinux 1:40bdbe1a93b7 199 *
Knillinux 1:40bdbe1a93b7 200 * @return Number of pulses which have occured.
Knillinux 1:40bdbe1a93b7 201 */
Knillinux 1:40bdbe1a93b7 202 int getPulses(void);
Knillinux 1:40bdbe1a93b7 203
Knillinux 1:40bdbe1a93b7 204 /**
Knillinux 1:40bdbe1a93b7 205 * Read the speed measured by the encoder.
Knillinux 1:40bdbe1a93b7 206 *
Knillinux 1:40bdbe1a93b7 207 * @return Speed measured by the encoder in counts per second.
Knillinux 1:40bdbe1a93b7 208 */
Knillinux 1:40bdbe1a93b7 209 float getSpeed(void);
Knillinux 1:40bdbe1a93b7 210
Knillinux 1:40bdbe1a93b7 211 /**
Knillinux 1:40bdbe1a93b7 212 * Read the number of revolutions recorded by the encoder on the index channel.
Knillinux 1:40bdbe1a93b7 213 *
Knillinux 1:40bdbe1a93b7 214 * @return Number of revolutions which have occured on the index channel.
Knillinux 1:40bdbe1a93b7 215 */
Knillinux 1:40bdbe1a93b7 216 int getRevolutions(void);
Knillinux 1:40bdbe1a93b7 217
Knillinux 1:40bdbe1a93b7 218 private:
Knillinux 1:40bdbe1a93b7 219
Knillinux 1:40bdbe1a93b7 220 /**
Knillinux 1:40bdbe1a93b7 221 * Update the pulse count.
Knillinux 1:40bdbe1a93b7 222 *
Knillinux 1:40bdbe1a93b7 223 * Called on every rising/falling edge of channels A/B.
Knillinux 1:40bdbe1a93b7 224 *
Knillinux 1:40bdbe1a93b7 225 * Reads the state of the channels and determines whether a pulse forward
Knillinux 1:40bdbe1a93b7 226 * or backward has occured, updating the count appropriately.
Knillinux 1:40bdbe1a93b7 227 */
Knillinux 1:40bdbe1a93b7 228 void encode(void);
Knillinux 1:40bdbe1a93b7 229
Knillinux 1:40bdbe1a93b7 230 /**
Knillinux 1:40bdbe1a93b7 231 * Called on every rising edge of channel index to update revolution
Knillinux 1:40bdbe1a93b7 232 * count by one.
Knillinux 1:40bdbe1a93b7 233 */
Knillinux 1:40bdbe1a93b7 234 void index(void);
Knillinux 1:40bdbe1a93b7 235
Knillinux 1:40bdbe1a93b7 236 Encoding encoding_;
Knillinux 1:40bdbe1a93b7 237
Knillinux 1:40bdbe1a93b7 238 InterruptIn channelA_;
Knillinux 1:40bdbe1a93b7 239 InterruptIn channelB_;
Knillinux 1:40bdbe1a93b7 240 InterruptIn index_;
Knillinux 1:40bdbe1a93b7 241
Knillinux 1:40bdbe1a93b7 242 int pulsesPerRev_;
Knillinux 1:40bdbe1a93b7 243 int prevState_;
Knillinux 1:40bdbe1a93b7 244 int currState_;
Knillinux 1:40bdbe1a93b7 245 int pulsesPerSpeedMeas_;
Knillinux 1:40bdbe1a93b7 246 Timer timer;
Knillinux 1:40bdbe1a93b7 247
Knillinux 1:40bdbe1a93b7 248 volatile int pulses_;
Knillinux 1:40bdbe1a93b7 249 volatile int revolutions_;
Knillinux 1:40bdbe1a93b7 250
Knillinux 1:40bdbe1a93b7 251 float speed_; // [counts/s]
Knillinux 1:40bdbe1a93b7 252
Knillinux 1:40bdbe1a93b7 253 };
Knillinux 1:40bdbe1a93b7 254
Knillinux 1:40bdbe1a93b7 255 #endif /* QEI_H */