Chun Feng Huang / ENCODER_PROCESSOR

This is a library for processing encoder

ENCODER_PROCESSOR.cpp@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 * Includes
benson516 0:6614a0ae9ae8 138 */
benson516 0:6614a0ae9ae8 139 #include "ENCODER_PROCESSOR.h"
benson516 0:6614a0ae9ae8 140
benson516 0:6614a0ae9ae8 141 ENCODER_PROCESSOR::ENCODER_PROCESSOR(int pulsesPerRevolution_in, int size_MA_window_in, double sampling_time_in, Encoding encoding):
benson516 0:6614a0ae9ae8 142 pulsesPerRevolution(pulsesPerRevolution_in), size_MA_window(size_MA_window_in), Ts(sampling_time_in)
benson516 0:6614a0ae9ae8 143 {
benson516 0:6614a0ae9ae8 144 //
benson516 0:6614a0ae9ae8 145 is_initiated = false;
benson516 0:6614a0ae9ae8 146
benson516 0:6614a0ae9ae8 147
benson516 0:6614a0ae9ae8 148 //
benson516 0:6614a0ae9ae8 149 MA_window.assign(size_MA_window, 0);
benson516 0:6614a0ae9ae8 150
benson516 0:6614a0ae9ae8 151 delta_count = 0;
benson516 0:6614a0ae9ae8 152 idx_MA_array = 0;
benson516 0:6614a0ae9ae8 153
benson516 0:6614a0ae9ae8 154
benson516 0:6614a0ae9ae8 155 pulses_ = 0;
benson516 0:6614a0ae9ae8 156 revolutions_ = 0;
benson516 0:6614a0ae9ae8 157 encoding_ = encoding;
benson516 0:6614a0ae9ae8 158
benson516 0:6614a0ae9ae8 159 // Findout what the current state is.
benson516 0:6614a0ae9ae8 160 // int chanA = channelA_.read();
benson516 0:6614a0ae9ae8 161 // int chanB = channelB_.read();
benson516 0:6614a0ae9ae8 162
benson516 0:6614a0ae9ae8 163 //2-bit state.
benson516 0:6614a0ae9ae8 164 encoderState_now = 0; // (chanA << 1) | (chanB);
benson516 0:6614a0ae9ae8 165 encoderState_pre = encoderState_now;
benson516 0:6614a0ae9ae8 166
benson516 0:6614a0ae9ae8 167 // Attaching call back function
benson516 0:6614a0ae9ae8 168 //X2 encoding uses interrupts on only channel A.
benson516 0:6614a0ae9ae8 169 //X4 encoding uses interrupts on channel A and on channel B.
benson516 0:6614a0ae9ae8 170
benson516 0:6614a0ae9ae8 171
benson516 0:6614a0ae9ae8 172 // Unit transformation
benson516 0:6614a0ae9ae8 173 // Rotational speed
benson516 0:6614a0ae9ae8 174 count_2_rad_s = 1.0/(pulsesPerRevolution*1.0)*6.2831852/Ts/(size_MA_window*1.0);
benson516 0:6614a0ae9ae8 175 count_2_deg_s = 1.0/(pulsesPerRevolution*1.0)*360.0/Ts/(size_MA_window*1.0);
benson516 0:6614a0ae9ae8 176 // Angle
benson516 0:6614a0ae9ae8 177 count_2_rad = 1.0/(pulsesPerRevolution*1.0)*6.2831852;
benson516 0:6614a0ae9ae8 178 count_2_deg = 1.0/(pulsesPerRevolution*1.0)*360.0;
benson516 0:6614a0ae9ae8 179 }
benson516 0:6614a0ae9ae8 180
benson516 0:6614a0ae9ae8 181 // Process control
benson516 0:6614a0ae9ae8 182 void ENCODER_PROCESSOR::reset(void) {
benson516 0:6614a0ae9ae8 183
benson516 0:6614a0ae9ae8 184 pulses_ = 0;
benson516 0:6614a0ae9ae8 185 revolutions_ = 0;
benson516 0:6614a0ae9ae8 186
benson516 0:6614a0ae9ae8 187 }
benson516 0:6614a0ae9ae8 188
benson516 0:6614a0ae9ae8 189 // Encode (call Back)
benson516 0:6614a0ae9ae8 190 /////////////////////////
benson516 0:6614a0ae9ae8 191 // +-------------+
benson516 0:6614a0ae9ae8 192 // | X2 Encoding |
benson516 0:6614a0ae9ae8 193 // +-------------+
benson516 0:6614a0ae9ae8 194 //
benson516 0:6614a0ae9ae8 195 // When observing states two patterns will appear:
benson516 0:6614a0ae9ae8 196 //
benson516 0:6614a0ae9ae8 197 // Counter clockwise rotation:
benson516 0:6614a0ae9ae8 198 //
benson516 0:6614a0ae9ae8 199 // 10 -> 01 -> 10 -> 01 -> ...
benson516 0:6614a0ae9ae8 200 //
benson516 0:6614a0ae9ae8 201 // Clockwise rotation:
benson516 0:6614a0ae9ae8 202 //
benson516 0:6614a0ae9ae8 203 // 11 -> 00 -> 11 -> 00 -> ...
benson516 0:6614a0ae9ae8 204 //
benson516 0:6614a0ae9ae8 205 // We consider counter clockwise rotation to be "forward" and
benson516 0:6614a0ae9ae8 206 // counter clockwise to be "backward". Therefore pulse count will increase
benson516 0:6614a0ae9ae8 207 // during counter clockwise rotation and decrease during clockwise rotation.
benson516 0:6614a0ae9ae8 208 //
benson516 0:6614a0ae9ae8 209 // +-------------+
benson516 0:6614a0ae9ae8 210 // | X4 Encoding |
benson516 0:6614a0ae9ae8 211 // +-------------+
benson516 0:6614a0ae9ae8 212 //
benson516 0:6614a0ae9ae8 213 // There are four possible states for a quadrature encoder which correspond to
benson516 0:6614a0ae9ae8 214 // 2-bit gray code.
benson516 0:6614a0ae9ae8 215 //
benson516 0:6614a0ae9ae8 216 // A state change is only valid if of only one bit has changed.
benson516 0:6614a0ae9ae8 217 // A state change is invalid if both bits have changed.
benson516 0:6614a0ae9ae8 218 //
benson516 0:6614a0ae9ae8 219 // Clockwise Rotation ->
benson516 0:6614a0ae9ae8 220 // (The bits are constructed as "AB")
benson516 0:6614a0ae9ae8 221 // 00 01 11 10 00
benson516 0:6614a0ae9ae8 222 //
benson516 0:6614a0ae9ae8 223 // <- Counter Clockwise Rotation
benson516 0:6614a0ae9ae8 224 //
benson516 0:6614a0ae9ae8 225 // If we observe any valid state changes going from left to right, we have
benson516 0:6614a0ae9ae8 226 // moved one pulse clockwise [we will consider this "backward" or "negative"].
benson516 0:6614a0ae9ae8 227 //
benson516 0:6614a0ae9ae8 228 // If we observe any valid state changes going from right to left we have
benson516 0:6614a0ae9ae8 229 // moved one pulse counter clockwise [we will consider this "forward" or
benson516 0:6614a0ae9ae8 230 // "positive"].
benson516 0:6614a0ae9ae8 231 //
benson516 0:6614a0ae9ae8 232 // We might enter an invalid state for a number of reasons which are hard to
benson516 0:6614a0ae9ae8 233 // predict - if this is the case, it is generally safe to ignore it, update
benson516 0:6614a0ae9ae8 234 // the state and carry on, with the error correcting itself shortly after.
benson516 0:6614a0ae9ae8 235
benson516 0:6614a0ae9ae8 236
benson516 0:6614a0ae9ae8 237 //---------------------------------------------//
benson516 0:6614a0ae9ae8 238 // Call-back function for both pin A and pin B interupt (rise and fall)
benson516 0:6614a0ae9ae8 239 void ENCODER_PROCESSOR::IntrCB_pulseUpdate(int phase_A, int phase_B) {
benson516 0:6614a0ae9ae8 240
benson516 0:6614a0ae9ae8 241 if (!is_initiated){
benson516 0:6614a0ae9ae8 242 //2-bit state.
benson516 0:6614a0ae9ae8 243 encoderState_now = (phase_A << 1) | (phase_B); // AB
benson516 0:6614a0ae9ae8 244 encoderState_pre = encoderState_now;
benson516 0:6614a0ae9ae8 245 //
benson516 0:6614a0ae9ae8 246 is_initiated = true;
benson516 0:6614a0ae9ae8 247 //
benson516 0:6614a0ae9ae8 248 return;
benson516 0:6614a0ae9ae8 249 }
benson516 0:6614a0ae9ae8 250
benson516 0:6614a0ae9ae8 251 //--------------------------------//
benson516 0:6614a0ae9ae8 252 int change = 0;
benson516 0:6614a0ae9ae8 253
benson516 0:6614a0ae9ae8 254 //2-bit state.
benson516 0:6614a0ae9ae8 255 encoderState_now = (phase_A << 1) | (phase_B); // AB
benson516 0:6614a0ae9ae8 256
benson516 0:6614a0ae9ae8 257 if (encoding_ == X2_ENCODING) {
benson516 0:6614a0ae9ae8 258
benson516 0:6614a0ae9ae8 259 //11->00->11->00 is counter clockwise rotation or "forward".
benson516 0:6614a0ae9ae8 260 if ((encoderState_pre == 0x3 && encoderState_now == 0x0) ||
benson516 0:6614a0ae9ae8 261 (encoderState_pre == 0x0 && encoderState_now == 0x3)) {
benson516 0:6614a0ae9ae8 262
benson516 0:6614a0ae9ae8 263 pulses_++;
benson516 0:6614a0ae9ae8 264
benson516 0:6614a0ae9ae8 265 }
benson516 0:6614a0ae9ae8 266 //10->01->10->01 is clockwise rotation or "backward".
benson516 0:6614a0ae9ae8 267 else if ((encoderState_pre == 0x2 && encoderState_now == 0x1) ||
benson516 0:6614a0ae9ae8 268 (encoderState_pre == 0x1 && encoderState_now == 0x2)) {
benson516 0:6614a0ae9ae8 269
benson516 0:6614a0ae9ae8 270 pulses_--;
benson516 0:6614a0ae9ae8 271
benson516 0:6614a0ae9ae8 272 }
benson516 0:6614a0ae9ae8 273
benson516 0:6614a0ae9ae8 274 } else{ // if (encoding_ == X4_ENCODING) {
benson516 0:6614a0ae9ae8 275
benson516 0:6614a0ae9ae8 276 //Entered a new valid state.
benson516 0:6614a0ae9ae8 277 if (((encoderState_now ^ encoderState_pre) != INVALID) && (encoderState_now != encoderState_pre)) {
benson516 0:6614a0ae9ae8 278 // 2 bit state.
benson516 0:6614a0ae9ae8 279 // (Right hand bit of prev) XOR (left hand bit of current)
benson516 0:6614a0ae9ae8 280 // B_(n-1) xor A_n (because of the 90deg phase shift and A is leading B when rotates clockwise)
benson516 0:6614a0ae9ae8 281 // gives 0 if clockwise rotation and 1 if counter clockwise rotation.
benson516 0:6614a0ae9ae8 282 change = (encoderState_pre & PREV_MASK) ^ ((encoderState_now & CURR_MASK) >> 1);
benson516 0:6614a0ae9ae8 283
benson516 0:6614a0ae9ae8 284 if (change == 0) {
benson516 0:6614a0ae9ae8 285 change = -1;
benson516 0:6614a0ae9ae8 286 }
benson516 0:6614a0ae9ae8 287
benson516 0:6614a0ae9ae8 288 pulses_ -= change;
benson516 0:6614a0ae9ae8 289 }
benson516 0:6614a0ae9ae8 290
benson516 0:6614a0ae9ae8 291 }
benson516 0:6614a0ae9ae8 292
benson516 0:6614a0ae9ae8 293 encoderState_pre = encoderState_now;
benson516 0:6614a0ae9ae8 294
benson516 0:6614a0ae9ae8 295 }
benson516 0:6614a0ae9ae8 296 // (Un-necessary) Call-back function for index-pin interupt
benson516 0:6614a0ae9ae8 297 void ENCODER_PROCESSOR::IntrCB_indexUpdate(void) {
benson516 0:6614a0ae9ae8 298 revolutions_++;
benson516 0:6614a0ae9ae8 299 }
benson516 0:6614a0ae9ae8 300 //---------------------------------------------//
benson516 0:6614a0ae9ae8 301
benson516 0:6614a0ae9ae8 302
benson516 0:6614a0ae9ae8 303
benson516 0:6614a0ae9ae8 304 //---------------------------------------------//
benson516 0:6614a0ae9ae8 305 // Iterate at each timer interupt
benson516 0:6614a0ae9ae8 306 void ENCODER_PROCESSOR::iterateOnce(){ // Moving average: calculating the speed of the motor for feedback control
benson516 0:6614a0ae9ae8 307 // new input
benson516 0:6614a0ae9ae8 308 MA_window[idx_MA_array] = pulses_;
benson516 0:6614a0ae9ae8 309
benson516 0:6614a0ae9ae8 310 // idx_next: Actually, it is the oldest one.
benson516 0:6614a0ae9ae8 311 size_t idx_next = idx_MA_array + 1;
benson516 0:6614a0ae9ae8 312 if(idx_next > (size_MA_window - 1) ){idx_next = 0;}
benson516 0:6614a0ae9ae8 313 // Calculate the total number of pulses within the period
benson516 0:6614a0ae9ae8 314 delta_count = (MA_window[idx_MA_array] - MA_window[idx_next]);
benson516 0:6614a0ae9ae8 315 //
benson516 0:6614a0ae9ae8 316 idx_MA_array ++;
benson516 0:6614a0ae9ae8 317 if(idx_MA_array > (size_MA_window -1) ){idx_MA_array = 0;}
benson516 0:6614a0ae9ae8 318 }
benson516 0:6614a0ae9ae8 319 //---------------------------------------------//
benson516 0:6614a0ae9ae8 320
benson516 0:6614a0ae9ae8 321
benson516 0:6614a0ae9ae8 322 // Get states
benson516 0:6614a0ae9ae8 323 int ENCODER_PROCESSOR::getEncoderState(void) {
benson516 0:6614a0ae9ae8 324 return encoderState_now;
benson516 0:6614a0ae9ae8 325 }
benson516 0:6614a0ae9ae8 326
benson516 0:6614a0ae9ae8 327 int ENCODER_PROCESSOR::getPulses(void) {
benson516 0:6614a0ae9ae8 328 return pulses_;
benson516 0:6614a0ae9ae8 329 }
benson516 0:6614a0ae9ae8 330
benson516 0:6614a0ae9ae8 331 int ENCODER_PROCESSOR::getRevolutions(void) {
benson516 0:6614a0ae9ae8 332 return revolutions_;
benson516 0:6614a0ae9ae8 333 }
benson516 0:6614a0ae9ae8 334
benson516 0:6614a0ae9ae8 335 // Get results
benson516 0:6614a0ae9ae8 336 // Rotational speed
benson516 0:6614a0ae9ae8 337 double ENCODER_PROCESSOR::getAngularSpeed(){
benson516 0:6614a0ae9ae8 338 return delta_count*count_2_rad_s;
benson516 0:6614a0ae9ae8 339 }
benson516 0:6614a0ae9ae8 340
benson516 0:6614a0ae9ae8 341 double ENCODER_PROCESSOR::getAngularSpeed_deg_s(){
benson516 0:6614a0ae9ae8 342 return delta_count*count_2_deg_s;
benson516 0:6614a0ae9ae8 343 }
benson516 0:6614a0ae9ae8 344 // Angle
benson516 0:6614a0ae9ae8 345 double ENCODER_PROCESSOR::getAngle(bool is_ranged){ // rad, if is_ranged, return 0~2*PI
benson516 0:6614a0ae9ae8 346 //
benson516 0:6614a0ae9ae8 347 int pulse_temp = this->pulses_;
benson516 0:6614a0ae9ae8 348 //
benson516 0:6614a0ae9ae8 349 if (is_ranged){
benson516 0:6614a0ae9ae8 350 revolutions_ = pulse_temp/pulsesPerRevolution;
benson516 0:6614a0ae9ae8 351 return (pulse_temp % pulsesPerRevolution)*count_2_rad;
benson516 0:6614a0ae9ae8 352 }else{
benson516 0:6614a0ae9ae8 353 return (pulse_temp*count_2_rad);
benson516 0:6614a0ae9ae8 354 }
benson516 0:6614a0ae9ae8 355 }
benson516 0:6614a0ae9ae8 356 double ENCODER_PROCESSOR::getAngle_deg(bool is_ranged){ // deg, if is_ranged, return 0~360
benson516 0:6614a0ae9ae8 357 //
benson516 0:6614a0ae9ae8 358 int pulse_temp = this->pulses_;
benson516 0:6614a0ae9ae8 359 //
benson516 0:6614a0ae9ae8 360 if (is_ranged){
benson516 0:6614a0ae9ae8 361 revolutions_ = pulse_temp/pulsesPerRevolution;
benson516 0:6614a0ae9ae8 362 return (pulse_temp % pulsesPerRevolution)*count_2_deg;
benson516 0:6614a0ae9ae8 363 }else{
benson516 0:6614a0ae9ae8 364 return (pulse_temp*count_2_deg);
benson516 0:6614a0ae9ae8 365 }
benson516 0:6614a0ae9ae8 366 }