Chun Feng Huang / ENCODER_PROCESSOR

This is a library for processing encoder

ENCODER_PROCESSOR.h@0:6614a0ae9ae8, 2017-05-08 (annotated)

Committer:
benson516
Date:
Mon May 08 17:30:36 2017 +0000
Revision:
0:6614a0ae9ae8
New library for processing encoder

Who changed what in which revision?

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