Source code

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 edu.wpi.first.hal.SimBoolean;
008import edu.wpi.first.hal.SimDevice;
009import edu.wpi.first.hal.SimDouble;
010import edu.wpi.first.math.MathUtil;
011import edu.wpi.first.util.sendable.Sendable;
012import edu.wpi.first.util.sendable.SendableBuilder;
013import edu.wpi.first.util.sendable.SendableRegistry;
014import edu.wpi.first.wpilibj.AnalogTriggerOutput.AnalogTriggerType;
015
016/**
017 * Class for supporting duty cycle/PWM encoders, such as the US Digital MA3 with PWM Output, the
018 * CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag Encoder.
019 */
020public class DutyCycleEncoder implements Sendable, AutoCloseable {
021  private final DutyCycle m_dutyCycle;
022  private boolean m_ownsDutyCycle;
023  private DigitalInput m_digitalInput;
024  private AnalogTrigger m_analogTrigger;
025  private Counter m_counter;
026  private int m_frequencyThreshold = 100;
027  private double m_positionOffset;
028  private double m_distancePerRotation = 1.0;
029  private double m_lastPosition;
030  private double m_sensorMin;
031  private double m_sensorMax = 1.0;
032
033  private SimDevice m_simDevice;
034  private SimDouble m_simPosition;
035  private SimDouble m_simAbsolutePosition;
036  private SimDouble m_simDistancePerRotation;
037  private SimBoolean m_simIsConnected;
038
039  /**
040   * Construct a new DutyCycleEncoder on a specific channel.
041   *
042   * @param channel the channel to attach to
043   */
044  @SuppressWarnings("this-escape")
045  public DutyCycleEncoder(int channel) {
046    m_digitalInput = new DigitalInput(channel);
047    m_ownsDutyCycle = true;
048    m_dutyCycle = new DutyCycle(m_digitalInput);
049    init();
050  }
051
052  /**
053   * Construct a new DutyCycleEncoder attached to an existing DutyCycle object.
054   *
055   * @param dutyCycle the duty cycle to attach to
056   */
057  @SuppressWarnings("this-escape")
058  public DutyCycleEncoder(DutyCycle dutyCycle) {
059    m_dutyCycle = dutyCycle;
060    init();
061  }
062
063  /**
064   * Construct a new DutyCycleEncoder attached to a DigitalSource object.
065   *
066   * @param source the digital source to attach to
067   */
068  @SuppressWarnings("this-escape")
069  public DutyCycleEncoder(DigitalSource source) {
070    m_ownsDutyCycle = true;
071    m_dutyCycle = new DutyCycle(source);
072    init();
073  }
074
075  private void init() {
076    m_simDevice = SimDevice.create("DutyCycle:DutyCycleEncoder", m_dutyCycle.getSourceChannel());
077
078    if (m_simDevice != null) {
079      m_simPosition = m_simDevice.createDouble("position", SimDevice.Direction.kInput, 0.0);
080      m_simDistancePerRotation =
081          m_simDevice.createDouble("distance_per_rot", SimDevice.Direction.kOutput, 1.0);
082      m_simAbsolutePosition =
083          m_simDevice.createDouble("absPosition", SimDevice.Direction.kInput, 0.0);
084      m_simIsConnected = m_simDevice.createBoolean("connected", SimDevice.Direction.kInput, true);
085    } else {
086      m_counter = new Counter();
087      m_analogTrigger = new AnalogTrigger(m_dutyCycle);
088      m_analogTrigger.setLimitsDutyCycle(0.25, 0.75);
089      m_counter.setUpSource(m_analogTrigger, AnalogTriggerType.kRisingPulse);
090      m_counter.setDownSource(m_analogTrigger, AnalogTriggerType.kFallingPulse);
091    }
092
093    SendableRegistry.addLW(this, "DutyCycle Encoder", m_dutyCycle.getSourceChannel());
094  }
095
096  private double mapSensorRange(double pos) {
097    // map sensor range
098    if (pos < m_sensorMin) {
099      pos = m_sensorMin;
100    }
101    if (pos > m_sensorMax) {
102      pos = m_sensorMax;
103    }
104    pos = (pos - m_sensorMin) / (m_sensorMax - m_sensorMin);
105    return pos;
106  }
107
108  private boolean doubleEquals(double a, double b) {
109    double epsilon = 0.00001d;
110    return Math.abs(a - b) < epsilon;
111  }
112
113  /**
114   * Get the encoder value since the last reset.
115   *
116   * <p>This is reported in rotations since the last reset.
117   *
118   * @return the encoder value in rotations
119   */
120  public double get() {
121    if (m_simPosition != null) {
122      return m_simPosition.get();
123    }
124
125    // As the values are not atomic, keep trying until we get 2 reads of the same
126    // value
127    // If we don't within 10 attempts, error
128    for (int i = 0; i < 10; i++) {
129      double counter = m_counter.get();
130      double pos = m_dutyCycle.getOutput();
131      double counter2 = m_counter.get();
132      double pos2 = m_dutyCycle.getOutput();
133      if (counter == counter2 && doubleEquals(pos, pos2)) {
134        // map sensor range
135        pos = mapSensorRange(pos);
136        double position = counter + pos - m_positionOffset;
137        m_lastPosition = position;
138        return position;
139      }
140    }
141
142    DriverStation.reportWarning(
143        "Failed to read Analog Encoder. Potential Speed Overrun. Returning last value", false);
144    return m_lastPosition;
145  }
146
147  /**
148   * Get the absolute position of the duty cycle encoder.
149   *
150   * <p>getAbsolutePosition() - getPositionOffset() will give an encoder absolute position relative
151   * to the last reset. This could potentially be negative, which needs to be accounted for.
152   *
153   * <p>This will not account for rollovers, and will always be just the raw absolute position.
154   *
155   * @return the absolute position
156   */
157  public double getAbsolutePosition() {
158    if (m_simAbsolutePosition != null) {
159      return m_simAbsolutePosition.get();
160    }
161
162    return mapSensorRange(m_dutyCycle.getOutput());
163  }
164
165  /**
166   * Get the offset of position relative to the last reset.
167   *
168   * <p>getAbsolutePosition() - getPositionOffset() will give an encoder absolute position relative
169   * to the last reset. This could potentially be negative, which needs to be accounted for.
170   *
171   * @return the position offset
172   */
173  public double getPositionOffset() {
174    return m_positionOffset;
175  }
176
177  /**
178   * Set the position offset.
179   *
180   * <p>This must be in the range of 0-1.
181   *
182   * @param offset the offset
183   */
184  public void setPositionOffset(double offset) {
185    m_positionOffset = MathUtil.clamp(offset, 0.0, 1.0);
186  }
187
188  /**
189   * Set the encoder duty cycle range. As the encoder needs to maintain a duty cycle, the duty cycle
190   * cannot go all the way to 0% or all the way to 100%. For example, an encoder with a 4096 us
191   * period might have a minimum duty cycle of 1 us / 4096 us and a maximum duty cycle of 4095 /
192   * 4096 us. Setting the range will result in an encoder duty cycle less than or equal to the
193   * minimum being output as 0 rotation, the duty cycle greater than or equal to the maximum being
194   * output as 1 rotation, and values in between linearly scaled from 0 to 1.
195   *
196   * @param min minimum duty cycle (0-1 range)
197   * @param max maximum duty cycle (0-1 range)
198   */
199  public void setDutyCycleRange(double min, double max) {
200    m_sensorMin = MathUtil.clamp(min, 0.0, 1.0);
201    m_sensorMax = MathUtil.clamp(max, 0.0, 1.0);
202  }
203
204  /**
205   * Set the distance per rotation of the encoder. This sets the multiplier used to determine the
206   * distance driven based on the rotation value from the encoder. Set this value based on how far
207   * the mechanism travels in 1 rotation of the encoder, and factor in gearing reductions following
208   * the encoder shaft. This distance can be in any units you like, linear or angular.
209   *
210   * @param distancePerRotation the distance per rotation of the encoder
211   */
212  public void setDistancePerRotation(double distancePerRotation) {
213    m_distancePerRotation = distancePerRotation;
214    if (m_simDistancePerRotation != null) {
215      m_simDistancePerRotation.set(distancePerRotation);
216    }
217  }
218
219  /**
220   * Get the distance per rotation for this encoder.
221   *
222   * @return The scale factor that will be used to convert rotation to useful units.
223   */
224  public double getDistancePerRotation() {
225    return m_distancePerRotation;
226  }
227
228  /**
229   * Get the distance the sensor has driven since the last reset as scaled by the value from {@link
230   * #setDistancePerRotation(double)}.
231   *
232   * @return The distance driven since the last reset
233   */
234  public double getDistance() {
235    return get() * getDistancePerRotation();
236  }
237
238  /**
239   * Get the frequency in Hz of the duty cycle signal from the encoder.
240   *
241   * @return duty cycle frequency in Hz
242   */
243  public int getFrequency() {
244    return m_dutyCycle.getFrequency();
245  }
246
247  /** Reset the Encoder distance to zero. */
248  public void reset() {
249    if (m_counter != null) {
250      m_counter.reset();
251    }
252    if (m_simPosition != null) {
253      m_simPosition.set(0);
254    }
255    m_positionOffset = getAbsolutePosition();
256  }
257
258  /**
259   * Get if the sensor is connected
260   *
261   * <p>This uses the duty cycle frequency to determine if the sensor is connected. By default, a
262   * value of 100 Hz is used as the threshold, and this value can be changed with {@link
263   * #setConnectedFrequencyThreshold(int)}.
264   *
265   * @return true if the sensor is connected
266   */
267  public boolean isConnected() {
268    if (m_simIsConnected != null) {
269      return m_simIsConnected.get();
270    }
271    return getFrequency() > m_frequencyThreshold;
272  }
273
274  /**
275   * Change the frequency threshold for detecting connection used by {@link #isConnected()}.
276   *
277   * @param frequency the minimum frequency in Hz.
278   */
279  public void setConnectedFrequencyThreshold(int frequency) {
280    if (frequency < 0) {
281      frequency = 0;
282    }
283
284    m_frequencyThreshold = frequency;
285  }
286
287  @Override
288  public void close() {
289    if (m_counter != null) {
290      m_counter.close();
291    }
292    if (m_analogTrigger != null) {
293      m_analogTrigger.close();
294    }
295    if (m_ownsDutyCycle) {
296      m_dutyCycle.close();
297    }
298    if (m_digitalInput != null) {
299      m_digitalInput.close();
300    }
301    if (m_simDevice != null) {
302      m_simDevice.close();
303    }
304  }
305
306  /**
307   * Get the FPGA index for the DutyCycleEncoder.
308   *
309   * @return the FPGA index
310   */
311  public int getFPGAIndex() {
312    return m_dutyCycle.getFPGAIndex();
313  }
314
315  /**
316   * Get the channel of the source.
317   *
318   * @return the source channel
319   */
320  public int getSourceChannel() {
321    return m_dutyCycle.getSourceChannel();
322  }
323
324  @Override
325  public void initSendable(SendableBuilder builder) {
326    builder.setSmartDashboardType("AbsoluteEncoder");
327    builder.addDoubleProperty("Distance", this::getDistance, null);
328    builder.addDoubleProperty("Distance Per Rotation", this::getDistancePerRotation, null);
329    builder.addBooleanProperty("Is Connected", this::isConnected, null);
330  }
331}