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