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