001// Copyright (c) FIRST and other WPILib contributors. 002// Open Source Software; you can modify and/or share it under the terms of 003// the WPILib BSD license file in the root directory of this project. 004 005package edu.wpi.first.wpilibj; 006 007import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; 008 009import edu.wpi.first.hal.EncoderJNI; 010import edu.wpi.first.hal.FRCNetComm.tResourceType; 011import edu.wpi.first.hal.HAL; 012import edu.wpi.first.hal.SimDevice; 013import edu.wpi.first.hal.util.AllocationException; 014import edu.wpi.first.util.sendable.Sendable; 015import edu.wpi.first.util.sendable.SendableBuilder; 016import edu.wpi.first.util.sendable.SendableRegistry; 017 018/** 019 * Class to read quadrature encoders. 020 * 021 * <p>Quadrature encoders are devices that count shaft rotation and can sense direction. The output 022 * of the Encoder class is an integer that can count either up or down, and can go negative for 023 * reverse direction counting. When creating Encoders, a direction can be supplied that inverts the 024 * sense of the output to make code more readable if the encoder is mounted such that forward 025 * movement generates negative values. Quadrature encoders have two digital outputs, an A Channel 026 * and a B Channel, that are out of phase with each other for direction sensing. 027 * 028 * <p>All encoders will immediately start counting - reset() them if you need them to be zeroed 029 * before use. 030 */ 031public class Encoder implements CounterBase, Sendable, AutoCloseable { 032 public enum IndexingType { 033 kResetWhileHigh(0), 034 kResetWhileLow(1), 035 kResetOnFallingEdge(2), 036 kResetOnRisingEdge(3); 037 038 public final int value; 039 040 IndexingType(int value) { 041 this.value = value; 042 } 043 } 044 045 /** The 'a' source. */ 046 protected DigitalSource m_aSource; // the A phase of the quad encoder 047 048 /** The 'b' source. */ 049 protected DigitalSource m_bSource; // the B phase of the quad encoder 050 051 /** The index source. */ 052 protected DigitalSource m_indexSource; // Index on some encoders 053 054 private boolean m_allocatedA; 055 private boolean m_allocatedB; 056 private boolean m_allocatedI; 057 private final EncodingType m_encodingType; 058 059 int m_encoder; // the HAL encoder object 060 061 /** 062 * Common initialization code for Encoders. This code allocates resources for Encoders and is 063 * common to all constructors. 064 * 065 * <p>The encoder will start counting immediately. 066 * 067 * @param reverseDirection If true, counts down instead of up (this is all relative) 068 */ 069 private void initEncoder(boolean reverseDirection, final EncodingType type) { 070 m_encoder = 071 EncoderJNI.initializeEncoder( 072 m_aSource.getPortHandleForRouting(), 073 m_aSource.getAnalogTriggerTypeForRouting(), 074 m_bSource.getPortHandleForRouting(), 075 m_bSource.getAnalogTriggerTypeForRouting(), 076 reverseDirection, 077 type.value); 078 079 int fpgaIndex = getFPGAIndex(); 080 HAL.report(tResourceType.kResourceType_Encoder, fpgaIndex + 1, type.value + 1); 081 SendableRegistry.addLW(this, "Encoder", fpgaIndex); 082 } 083 084 /** 085 * Encoder constructor. Construct a Encoder given a and b channels. 086 * 087 * <p>The encoder will start counting immediately. 088 * 089 * @param channelA The 'a' channel DIO channel. 0-9 are on-board, 10-25 are on the MXP port 090 * @param channelB The 'b' channel DIO channel. 0-9 are on-board, 10-25 are on the MXP port 091 * @param reverseDirection represents the orientation of the encoder and inverts the output values 092 * if necessary so forward represents positive values. 093 */ 094 public Encoder(final int channelA, final int channelB, boolean reverseDirection) { 095 this(channelA, channelB, reverseDirection, EncodingType.k4X); 096 } 097 098 /** 099 * Encoder constructor. Construct an Encoder given a and b channels. 100 * 101 * <p>The encoder will start counting immediately. 102 * 103 * @param channelA The a channel digital input channel. 104 * @param channelB The b channel digital input channel. 105 */ 106 public Encoder(final int channelA, final int channelB) { 107 this(channelA, channelB, false); 108 } 109 110 /** 111 * Encoder constructor. Construct an Encoder given a and b channels. 112 * 113 * <p>The encoder will start counting immediately. 114 * 115 * @param channelA The a channel digital input channel. 116 * @param channelB The b channel digital input channel. 117 * @param reverseDirection represents the orientation of the encoder and inverts the output values 118 * if necessary so forward represents positive values. 119 * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X decoding. If 4X is 120 * selected, then an encoder FPGA object is used and the returned counts will be 4x the 121 * encoder spec'd value since all rising and falling edges are counted. If 1X or 2X are 122 * selected, then a counter object will be used and the returned value will either exactly 123 * match the spec'd count or be double (2x) the spec'd count. 124 */ 125 @SuppressWarnings("this-escape") 126 public Encoder( 127 final int channelA, 128 final int channelB, 129 boolean reverseDirection, 130 final EncodingType encodingType) { 131 requireNonNullParam(encodingType, "encodingType", "Encoder"); 132 133 m_allocatedA = true; 134 m_allocatedB = true; 135 m_allocatedI = false; 136 m_aSource = new DigitalInput(channelA); 137 m_bSource = new DigitalInput(channelB); 138 m_encodingType = encodingType; 139 SendableRegistry.addChild(this, m_aSource); 140 SendableRegistry.addChild(this, m_bSource); 141 initEncoder(reverseDirection, encodingType); 142 } 143 144 /** 145 * Encoder constructor. Construct an Encoder given a and b channels. Using an index pulse forces 146 * 4x encoding 147 * 148 * <p>The encoder will start counting immediately. 149 * 150 * @param channelA The a channel digital input channel. 151 * @param channelB The b channel digital input channel. 152 * @param indexChannel The index channel digital input channel. 153 * @param reverseDirection represents the orientation of the encoder and inverts the output values 154 * if necessary so forward represents positive values. 155 */ 156 @SuppressWarnings("this-escape") 157 public Encoder( 158 final int channelA, final int channelB, final int indexChannel, boolean reverseDirection) { 159 this(channelA, channelB, reverseDirection); 160 m_allocatedI = true; 161 m_indexSource = new DigitalInput(indexChannel); 162 SendableRegistry.addChild(this, m_indexSource); 163 setIndexSource(m_indexSource); 164 } 165 166 /** 167 * Encoder constructor. Construct an Encoder given a and b channels. Using an index pulse forces 168 * 4x encoding 169 * 170 * <p>The encoder will start counting immediately. 171 * 172 * @param channelA The a channel digital input channel. 173 * @param channelB The b channel digital input channel. 174 * @param indexChannel The index channel digital input channel. 175 */ 176 public Encoder(final int channelA, final int channelB, final int indexChannel) { 177 this(channelA, channelB, indexChannel, false); 178 } 179 180 /** 181 * Encoder constructor. Construct an Encoder given a and b channels as digital inputs. This is 182 * used in the case where the digital inputs are shared. The Encoder class will not allocate the 183 * digital inputs and assume that they already are counted. 184 * 185 * <p>The encoder will start counting immediately. 186 * 187 * @param sourceA The source that should be used for the 'a' channel. 188 * @param sourceB the source that should be used for the 'b' channel. 189 * @param reverseDirection represents the orientation of the encoder and inverts the output values 190 * if necessary so forward represents positive values. 191 */ 192 public Encoder(DigitalSource sourceA, DigitalSource sourceB, boolean reverseDirection) { 193 this(sourceA, sourceB, reverseDirection, EncodingType.k4X); 194 } 195 196 /** 197 * Encoder constructor. Construct an Encoder given a and b channels as digital inputs. This is 198 * used in the case where the digital inputs are shared. The Encoder class will not allocate the 199 * digital inputs and assume that they already are counted. 200 * 201 * <p>The encoder will start counting immediately. 202 * 203 * @param sourceA The source that should be used for the 'a' channel. 204 * @param sourceB the source that should be used for the 'b' channel. 205 */ 206 public Encoder(DigitalSource sourceA, DigitalSource sourceB) { 207 this(sourceA, sourceB, false); 208 } 209 210 /** 211 * Encoder constructor. Construct an Encoder given a and b channels as digital inputs. This is 212 * used in the case where the digital inputs are shared. The Encoder class will not allocate the 213 * digital inputs and assume that they already are counted. 214 * 215 * <p>The encoder will start counting immediately. 216 * 217 * @param sourceA The source that should be used for the 'a' channel. 218 * @param sourceB the source that should be used for the 'b' channel. 219 * @param reverseDirection represents the orientation of the encoder and inverts the output values 220 * if necessary so forward represents positive values. 221 * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X decoding. If 4X is 222 * selected, then an encoder FPGA object is used and the returned counts will be 4x the 223 * encoder spec'd value since all rising and falling edges are counted. If 1X or 2X are 224 * selected then a counter object will be used and the returned value will either exactly 225 * match the spec'd count or be double (2x) the spec'd count. 226 */ 227 @SuppressWarnings("this-escape") 228 public Encoder( 229 DigitalSource sourceA, 230 DigitalSource sourceB, 231 boolean reverseDirection, 232 final EncodingType encodingType) { 233 requireNonNullParam(sourceA, "sourceA", "Encoder"); 234 requireNonNullParam(sourceB, "sourceB", "Encoder"); 235 requireNonNullParam(encodingType, "encodingType", "Encoder"); 236 237 m_allocatedA = false; 238 m_allocatedB = false; 239 m_allocatedI = false; 240 m_encodingType = encodingType; 241 m_aSource = sourceA; 242 m_bSource = sourceB; 243 initEncoder(reverseDirection, encodingType); 244 } 245 246 /** 247 * Encoder constructor. Construct an Encoder given a, b and index channels as digital inputs. This 248 * is used in the case where the digital inputs are shared. The Encoder class will not allocate 249 * the digital inputs and assume that they already are counted. 250 * 251 * <p>The encoder will start counting immediately. 252 * 253 * @param sourceA The source that should be used for the 'a' channel. 254 * @param sourceB the source that should be used for the 'b' channel. 255 * @param indexSource the source that should be used for the index channel. 256 * @param reverseDirection represents the orientation of the encoder and inverts the output values 257 * if necessary so forward represents positive values. 258 */ 259 public Encoder( 260 DigitalSource sourceA, 261 DigitalSource sourceB, 262 DigitalSource indexSource, 263 boolean reverseDirection) { 264 this(sourceA, sourceB, reverseDirection); 265 m_allocatedI = false; 266 m_indexSource = indexSource; 267 setIndexSource(indexSource); 268 } 269 270 /** 271 * Encoder constructor. Construct an Encoder given a, b and index channels as digital inputs. This 272 * is used in the case where the digital inputs are shared. The Encoder class will not allocate 273 * the digital inputs and assume that they already are counted. 274 * 275 * <p>The encoder will start counting immediately. 276 * 277 * @param sourceA The source that should be used for the 'a' channel. 278 * @param sourceB the source that should be used for the 'b' channel. 279 * @param indexSource the source that should be used for the index channel. 280 */ 281 public Encoder(DigitalSource sourceA, DigitalSource sourceB, DigitalSource indexSource) { 282 this(sourceA, sourceB, indexSource, false); 283 } 284 285 /** 286 * Get the FPGA index of the encoder. 287 * 288 * @return The Encoder's FPGA index. 289 */ 290 public int getFPGAIndex() { 291 return EncoderJNI.getEncoderFPGAIndex(m_encoder); 292 } 293 294 /** 295 * Used to divide raw edge counts down to spec'd counts. 296 * 297 * @return The encoding scale factor 1x, 2x, or 4x, per the requested encoding type. 298 */ 299 public int getEncodingScale() { 300 return EncoderJNI.getEncoderEncodingScale(m_encoder); 301 } 302 303 @Override 304 public void close() { 305 SendableRegistry.remove(this); 306 if (m_aSource != null && m_allocatedA) { 307 m_aSource.close(); 308 m_allocatedA = false; 309 } 310 if (m_bSource != null && m_allocatedB) { 311 m_bSource.close(); 312 m_allocatedB = false; 313 } 314 if (m_indexSource != null && m_allocatedI) { 315 m_indexSource.close(); 316 m_allocatedI = false; 317 } 318 319 m_aSource = null; 320 m_bSource = null; 321 m_indexSource = null; 322 EncoderJNI.freeEncoder(m_encoder); 323 m_encoder = 0; 324 } 325 326 /** 327 * Gets the raw value from the encoder. The raw value is the actual count unscaled by the 1x, 2x, 328 * or 4x scale factor. 329 * 330 * @return Current raw count from the encoder 331 */ 332 public int getRaw() { 333 return EncoderJNI.getEncoderRaw(m_encoder); 334 } 335 336 /** 337 * Gets the current count. Returns the current count on the Encoder. This method compensates for 338 * the decoding type. 339 * 340 * @return Current count from the Encoder adjusted for the 1x, 2x, or 4x scale factor. 341 */ 342 @Override 343 public int get() { 344 return EncoderJNI.getEncoder(m_encoder); 345 } 346 347 /** Reset the Encoder distance to zero. Resets the current count to zero on the encoder. */ 348 @Override 349 public void reset() { 350 EncoderJNI.resetEncoder(m_encoder); 351 } 352 353 /** 354 * Returns the period of the most recent pulse. Returns the period of the most recent Encoder 355 * pulse in seconds. This method compensates for the decoding type. 356 * 357 * <p><b>Warning:</b> This returns unscaled periods. Use getRate() for rates that are scaled using 358 * the value from setDistancePerPulse(). 359 * 360 * @return Period in seconds of the most recent pulse. 361 * @deprecated Use getRate() in favor of this method. 362 */ 363 @Override 364 @Deprecated 365 public double getPeriod() { 366 return EncoderJNI.getEncoderPeriod(m_encoder); 367 } 368 369 /** 370 * Sets the maximum period for stopped detection. Sets the value that represents the maximum 371 * period of the Encoder before it will assume that the attached device is stopped. This timeout 372 * allows users to determine if the wheels or other shaft has stopped rotating. This method 373 * compensates for the decoding type. 374 * 375 * @param maxPeriod The maximum time between rising and falling edges before the FPGA will report 376 * the device stopped. This is expressed in seconds. 377 * @deprecated Use setMinRate() in favor of this method. This takes unscaled periods and 378 * setMinRate() scales using value from setDistancePerPulse(). 379 */ 380 @Override 381 @Deprecated 382 public void setMaxPeriod(double maxPeriod) { 383 EncoderJNI.setEncoderMaxPeriod(m_encoder, maxPeriod); 384 } 385 386 /** 387 * Determine if the encoder is stopped. Using the MaxPeriod value, a boolean is returned that is 388 * true if the encoder is considered stopped and false if it is still moving. A stopped encoder is 389 * one where the most recent pulse width exceeds the MaxPeriod. 390 * 391 * @return True if the encoder is considered stopped. 392 */ 393 @Override 394 public boolean getStopped() { 395 return EncoderJNI.getEncoderStopped(m_encoder); 396 } 397 398 /** 399 * The last direction the encoder value changed. 400 * 401 * @return The last direction the encoder value changed. 402 */ 403 @Override 404 public boolean getDirection() { 405 return EncoderJNI.getEncoderDirection(m_encoder); 406 } 407 408 /** 409 * Get the distance the robot has driven since the last reset as scaled by the value from {@link 410 * #setDistancePerPulse(double)}. 411 * 412 * @return The distance driven since the last reset 413 */ 414 public double getDistance() { 415 return EncoderJNI.getEncoderDistance(m_encoder); 416 } 417 418 /** 419 * Get the current rate of the encoder. Units are distance per second as scaled by the value from 420 * setDistancePerPulse(). 421 * 422 * @return The current rate of the encoder. 423 */ 424 public double getRate() { 425 return EncoderJNI.getEncoderRate(m_encoder); 426 } 427 428 /** 429 * Set the minimum rate of the device before the hardware reports it stopped. 430 * 431 * @param minRate The minimum rate. The units are in distance per second as scaled by the value 432 * from setDistancePerPulse(). 433 */ 434 public void setMinRate(double minRate) { 435 EncoderJNI.setEncoderMinRate(m_encoder, minRate); 436 } 437 438 /** 439 * Set the distance per pulse for this encoder. This sets the multiplier used to determine the 440 * distance driven based on the count value from the encoder. Do not include the decoding type in 441 * this scale. The library already compensates for the decoding type. Set this value based on the 442 * encoder's rated Pulses per Revolution and factor in gearing reductions following the encoder 443 * shaft. This distance can be in any units you like, linear or angular. 444 * 445 * @param distancePerPulse The scale factor that will be used to convert pulses to useful units. 446 */ 447 public void setDistancePerPulse(double distancePerPulse) { 448 EncoderJNI.setEncoderDistancePerPulse(m_encoder, distancePerPulse); 449 } 450 451 /** 452 * Get the distance per pulse for this encoder. 453 * 454 * @return The scale factor that will be used to convert pulses to useful units. 455 */ 456 public double getDistancePerPulse() { 457 return EncoderJNI.getEncoderDistancePerPulse(m_encoder); 458 } 459 460 /** 461 * Set the direction sensing for this encoder. This sets the direction sensing on the encoder so 462 * that it could count in the correct software direction regardless of the mounting. 463 * 464 * @param reverseDirection true if the encoder direction should be reversed 465 */ 466 public void setReverseDirection(boolean reverseDirection) { 467 EncoderJNI.setEncoderReverseDirection(m_encoder, reverseDirection); 468 } 469 470 /** 471 * Set the Samples to Average which specifies the number of samples of the timer to average when 472 * calculating the period. Perform averaging to account for mechanical imperfections or as 473 * oversampling to increase resolution. 474 * 475 * @param samplesToAverage The number of samples to average from 1 to 127. 476 */ 477 public void setSamplesToAverage(int samplesToAverage) { 478 EncoderJNI.setEncoderSamplesToAverage(m_encoder, samplesToAverage); 479 } 480 481 /** 482 * Get the Samples to Average which specifies the number of samples of the timer to average when 483 * calculating the period. Perform averaging to account for mechanical imperfections or as 484 * oversampling to increase resolution. 485 * 486 * @return SamplesToAverage The number of samples being averaged (from 1 to 127) 487 */ 488 public int getSamplesToAverage() { 489 return EncoderJNI.getEncoderSamplesToAverage(m_encoder); 490 } 491 492 /** 493 * Set the index source for the encoder. When this source is activated, the encoder count 494 * automatically resets. 495 * 496 * @param channel A DIO channel to set as the encoder index 497 */ 498 public final void setIndexSource(int channel) { 499 setIndexSource(channel, IndexingType.kResetOnRisingEdge); 500 } 501 502 /** 503 * Set the index source for the encoder. When this source is activated, the encoder count 504 * automatically resets. 505 * 506 * @param source A digital source to set as the encoder index 507 */ 508 public final void setIndexSource(DigitalSource source) { 509 setIndexSource(source, IndexingType.kResetOnRisingEdge); 510 } 511 512 /** 513 * Set the index source for the encoder. When this source rises, the encoder count automatically 514 * resets. 515 * 516 * @param channel A DIO channel to set as the encoder index 517 * @param type The state that will cause the encoder to reset 518 */ 519 public final void setIndexSource(int channel, IndexingType type) { 520 if (m_allocatedI) { 521 throw new AllocationException("Digital Input for Indexing already allocated"); 522 } 523 m_indexSource = new DigitalInput(channel); 524 m_allocatedI = true; 525 SendableRegistry.addChild(this, m_indexSource); 526 setIndexSource(m_indexSource, type); 527 } 528 529 /** 530 * Set the index source for the encoder. When this source rises, the encoder count automatically 531 * resets. 532 * 533 * @param source A digital source to set as the encoder index 534 * @param type The state that will cause the encoder to reset 535 */ 536 public final void setIndexSource(DigitalSource source, IndexingType type) { 537 EncoderJNI.setEncoderIndexSource( 538 m_encoder, 539 source.getPortHandleForRouting(), 540 source.getAnalogTriggerTypeForRouting(), 541 type.value); 542 } 543 544 /** 545 * Indicates this input is used by a simulated device. 546 * 547 * @param device simulated device handle 548 */ 549 public void setSimDevice(SimDevice device) { 550 EncoderJNI.setEncoderSimDevice(m_encoder, device.getNativeHandle()); 551 } 552 553 /** 554 * Gets the decoding scale factor for scaling raw values to full counts. 555 * 556 * @return decoding scale factor 557 */ 558 public double getDecodingScaleFactor() { 559 switch (m_encodingType) { 560 case k1X: 561 return 1.0; 562 case k2X: 563 return 0.5; 564 case k4X: 565 return 0.25; 566 default: 567 return 0.0; 568 } 569 } 570 571 @Override 572 public void initSendable(SendableBuilder builder) { 573 if (EncoderJNI.getEncoderEncodingType(m_encoder) == EncodingType.k4X.value) { 574 builder.setSmartDashboardType("Quadrature Encoder"); 575 } else { 576 builder.setSmartDashboardType("Encoder"); 577 } 578 579 builder.addDoubleProperty("Speed", this::getRate, null); 580 builder.addDoubleProperty("Distance", this::getDistance, null); 581 builder.addDoubleProperty("Distance per Tick", this::getDistancePerPulse, null); 582 } 583}