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;
014
015/**
016 * Class for supporting duty cycle/PWM encoders, such as the US Digital MA3 with PWM Output, the
017 * CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag Encoder.
018 */
019public class DutyCycleEncoder implements Sendable, AutoCloseable {
020  private final DutyCycle m_dutyCycle;
021  private boolean m_ownsDutyCycle;
022  private double m_frequencyThreshold = 100;
023  private double m_fullRange;
024  private double m_expectedZero;
025  private double m_periodNanos;
026  private double m_sensorMin;
027  private double m_sensorMax = 1.0;
028  private boolean m_isInverted;
029
030  private SimDevice m_simDevice;
031  private SimDouble m_simPosition;
032  private SimBoolean m_simIsConnected;
033
034  /**
035   * Construct a new DutyCycleEncoder on a specific channel.
036   *
037   * @param channel the channel to attach to
038   * @param fullRange the value to report at maximum travel
039   * @param expectedZero the reading where you would expect a 0 from get()
040   */
041  @SuppressWarnings("this-escape")
042  public DutyCycleEncoder(int channel, double fullRange, double expectedZero) {
043    m_ownsDutyCycle = true;
044    m_dutyCycle = new DutyCycle(channel);
045    init(fullRange, expectedZero);
046  }
047
048  /**
049   * Construct a new DutyCycleEncoder attached to an existing DutyCycle object.
050   *
051   * @param dutyCycle the duty cycle to attach to
052   * @param fullRange the value to report at maximum travel
053   * @param expectedZero the reading where you would expect a 0 from get()
054   */
055  @SuppressWarnings("this-escape")
056  public DutyCycleEncoder(DutyCycle dutyCycle, double fullRange, double expectedZero) {
057    m_dutyCycle = dutyCycle;
058    init(fullRange, expectedZero);
059  }
060
061  /**
062   * Construct a new DutyCycleEncoder on a specific channel.
063   *
064   * <p>This has a fullRange of 1 and an expectedZero of 0.
065   *
066   * @param channel the channel to attach to
067   */
068  @SuppressWarnings("this-escape")
069  public DutyCycleEncoder(int channel) {
070    this(channel, 1.0, 0.0);
071  }
072
073  /**
074   * Construct a new DutyCycleEncoder attached to an existing DutyCycle object.
075   *
076   * <p>This has a fullRange of 1 and an expectedZero of 0.
077   *
078   * @param dutyCycle the duty cycle to attach to
079   */
080  @SuppressWarnings("this-escape")
081  public DutyCycleEncoder(DutyCycle dutyCycle) {
082    this(dutyCycle, 1.0, 0.0);
083  }
084
085  private void init(double fullRange, double expectedZero) {
086    m_simDevice = SimDevice.create("DutyCycle:DutyCycleEncoder", m_dutyCycle.getSourceChannel());
087
088    if (m_simDevice != null) {
089      m_simPosition = m_simDevice.createDouble("Position", SimDevice.Direction.kInput, 0.0);
090      m_simIsConnected = m_simDevice.createBoolean("Connected", SimDevice.Direction.kInput, true);
091    }
092
093    m_fullRange = fullRange;
094    m_expectedZero = expectedZero;
095
096    SendableRegistry.add(this, "DutyCycle Encoder", m_dutyCycle.getSourceChannel());
097  }
098
099  private double mapSensorRange(double pos) {
100    // map sensor range
101    if (pos < m_sensorMin) {
102      pos = m_sensorMin;
103    }
104    if (pos > m_sensorMax) {
105      pos = m_sensorMax;
106    }
107    pos = (pos - m_sensorMin) / (m_sensorMax - m_sensorMin);
108    return pos;
109  }
110
111  /**
112   * Get the encoder value since the last reset.
113   *
114   * <p>This is reported in rotations since the last reset.
115   *
116   * @return the encoder value in rotations
117   */
118  public double get() {
119    if (m_simPosition != null) {
120      return m_simPosition.get();
121    }
122
123    double pos;
124    // Compute output percentage (0-1)
125    if (m_periodNanos == 0.0) {
126      pos = m_dutyCycle.getOutput();
127    } else {
128      int highTime = m_dutyCycle.getHighTimeNanoseconds();
129      pos = highTime / m_periodNanos;
130    }
131
132    // Map sensor range if range isn't full
133    pos = mapSensorRange(pos);
134
135    // Compute full range and offset
136    pos = pos * m_fullRange - m_expectedZero;
137
138    // Map from 0 - Full Range
139    double result = MathUtil.inputModulus(pos, 0, m_fullRange);
140    // Invert if necessary
141    if (m_isInverted) {
142      return m_fullRange - result;
143    }
144    return result;
145  }
146
147  /**
148   * Set the encoder duty cycle range. As the encoder needs to maintain a duty cycle, the duty cycle
149   * cannot go all the way to 0% or all the way to 100%. For example, an encoder with a 4096 us
150   * period might have a minimum duty cycle of 1 us / 4096 us and a maximum duty cycle of 4095 /
151   * 4096 us. Setting the range will result in an encoder duty cycle less than or equal to the
152   * minimum being output as 0 rotation, the duty cycle greater than or equal to the maximum being
153   * output as 1 rotation, and values in between linearly scaled from 0 to 1.
154   *
155   * @param min minimum duty cycle (0-1 range)
156   * @param max maximum duty cycle (0-1 range)
157   */
158  public void setDutyCycleRange(double min, double max) {
159    m_sensorMin = MathUtil.clamp(min, 0.0, 1.0);
160    m_sensorMax = MathUtil.clamp(max, 0.0, 1.0);
161  }
162
163  /**
164   * Get the frequency in Hz of the duty cycle signal from the encoder.
165   *
166   * @return duty cycle frequency in Hz
167   */
168  public double getFrequency() {
169    return m_dutyCycle.getFrequency();
170  }
171
172  /**
173   * Get if the sensor is connected
174   *
175   * <p>This uses the duty cycle frequency to determine if the sensor is connected. By default, a
176   * value of 100 Hz is used as the threshold, and this value can be changed with {@link
177   * #setConnectedFrequencyThreshold(int)}.
178   *
179   * @return true if the sensor is connected
180   */
181  public boolean isConnected() {
182    if (m_simIsConnected != null) {
183      return m_simIsConnected.get();
184    }
185    return getFrequency() > m_frequencyThreshold;
186  }
187
188  /**
189   * Change the frequency threshold for detecting connection used by {@link #isConnected()}.
190   *
191   * @param frequency the minimum frequency in Hz.
192   */
193  public void setConnectedFrequencyThreshold(double frequency) {
194    if (frequency < 0) {
195      frequency = 0;
196    }
197
198    m_frequencyThreshold = frequency;
199  }
200
201  /**
202   * Sets the assumed frequency of the connected device.
203   *
204   * <p>By default, the DutyCycle engine has to compute the frequency of the input signal. This can
205   * result in both delayed readings and jumpy readings. To solve this, you can pass the expected
206   * frequency of the sensor to this function. This will use that frequency to compute the DutyCycle
207   * percentage, rather than the computed frequency.
208   *
209   * @param frequency the assumed frequency of the sensor
210   */
211  public void setAssumedFrequency(double frequency) {
212    if (frequency == 0.0) {
213      m_periodNanos = 0.0;
214    } else {
215      m_periodNanos = 1000000000 / frequency;
216    }
217  }
218
219  /**
220   * Set if this encoder is inverted.
221   *
222   * @param inverted true to invert the encoder, false otherwise
223   */
224  public void setInverted(boolean inverted) {
225    m_isInverted = inverted;
226  }
227
228  @Override
229  public void close() {
230    if (m_ownsDutyCycle) {
231      m_dutyCycle.close();
232    }
233    if (m_simDevice != null) {
234      m_simDevice.close();
235    }
236  }
237
238  /**
239   * Get the channel of the source.
240   *
241   * @return the source channel
242   */
243  public int getSourceChannel() {
244    return m_dutyCycle.getSourceChannel();
245  }
246
247  @Override
248  public void initSendable(SendableBuilder builder) {
249    builder.setSmartDashboardType("AbsoluteEncoder");
250    builder.addDoubleProperty("Position", this::get, null);
251    builder.addBooleanProperty("Is Connected", this::isConnected, null);
252  }
253}