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.SimDevice;
008import edu.wpi.first.hal.SimDevice.Direction;
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/** Class for supporting continuous analog encoders, such as the US Digital MA3. */
017public class AnalogEncoder implements Sendable, AutoCloseable {
018  private final AnalogInput m_analogInput;
019  private AnalogTrigger m_analogTrigger;
020  private Counter m_counter;
021  private double m_positionOffset;
022  private double m_distancePerRotation = 1.0;
023  private double m_lastPosition;
024
025  private SimDevice m_simDevice;
026  private SimDouble m_simPosition;
027  private SimDouble m_simAbsolutePosition;
028
029  /**
030   * Construct a new AnalogEncoder attached to a specific AnalogIn channel.
031   *
032   * @param channel the analog input channel to attach to
033   */
034  public AnalogEncoder(int channel) {
035    this(new AnalogInput(channel));
036  }
037
038  /**
039   * Construct a new AnalogEncoder attached to a specific AnalogInput.
040   *
041   * @param analogInput the analog input to attach to
042   */
043  @SuppressWarnings("this-escape")
044  public AnalogEncoder(AnalogInput analogInput) {
045    m_analogInput = analogInput;
046    init();
047  }
048
049  private void init() {
050    m_analogTrigger = new AnalogTrigger(m_analogInput);
051    m_counter = new Counter();
052
053    m_simDevice = SimDevice.create("AnalogEncoder", m_analogInput.getChannel());
054
055    if (m_simDevice != null) {
056      m_simPosition = m_simDevice.createDouble("Position", Direction.kInput, 0.0);
057      m_simAbsolutePosition = m_simDevice.createDouble("absPosition", Direction.kInput, 0.0);
058    }
059
060    // Limits need to be 25% from each end
061    m_analogTrigger.setLimitsVoltage(1.25, 3.75);
062    m_counter.setUpSource(m_analogTrigger, AnalogTriggerType.kRisingPulse);
063    m_counter.setDownSource(m_analogTrigger, AnalogTriggerType.kFallingPulse);
064
065    SendableRegistry.addLW(this, "Analog Encoder", m_analogInput.getChannel());
066  }
067
068  private boolean doubleEquals(double a, double b) {
069    double epsilon = 0.00001d;
070    return Math.abs(a - b) < epsilon;
071  }
072
073  /**
074   * Get the encoder value since the last reset.
075   *
076   * <p>This is reported in rotations since the last reset.
077   *
078   * @return the encoder value in rotations
079   */
080  public double get() {
081    if (m_simPosition != null) {
082      return m_simPosition.get();
083    }
084
085    // As the values are not atomic, keep trying until we get 2 reads of the same
086    // value. If we don't within 10 attempts, warn
087    for (int i = 0; i < 10; i++) {
088      double counter = m_counter.get();
089      double pos = m_analogInput.getVoltage();
090      double counter2 = m_counter.get();
091      double pos2 = m_analogInput.getVoltage();
092      if (counter == counter2 && doubleEquals(pos, pos2)) {
093        pos = pos / RobotController.getVoltage5V();
094        double position = counter + pos - m_positionOffset;
095        m_lastPosition = position;
096        return position;
097      }
098    }
099
100    DriverStation.reportWarning(
101        "Failed to read Analog Encoder. Potential Speed Overrun. Returning last value", false);
102    return m_lastPosition;
103  }
104
105  /**
106   * Get the absolute position of the analog encoder.
107   *
108   * <p>getAbsolutePosition() - getPositionOffset() will give an encoder absolute position relative
109   * to the last reset. This could potentially be negative, which needs to be accounted for.
110   *
111   * <p>This will not account for rollovers, and will always be just the raw absolute position.
112   *
113   * @return the absolute position
114   */
115  public double getAbsolutePosition() {
116    if (m_simAbsolutePosition != null) {
117      return m_simAbsolutePosition.get();
118    }
119
120    return m_analogInput.getVoltage() / RobotController.getVoltage5V();
121  }
122
123  /**
124   * Get the offset of position relative to the last reset.
125   *
126   * <p>getAbsolutePosition() - getPositionOffset() will give an encoder absolute position relative
127   * to the last reset. This could potentially be negative, which needs to be accounted for.
128   *
129   * @return the position offset
130   */
131  public double getPositionOffset() {
132    return m_positionOffset;
133  }
134
135  /**
136   * Set the position offset.
137   *
138   * <p>This must be in the range of 0-1.
139   *
140   * @param offset the offset
141   */
142  public void setPositionOffset(double offset) {
143    m_positionOffset = MathUtil.clamp(offset, 0.0, 1.0);
144  }
145
146  /**
147   * Set the distance per rotation of the encoder. This sets the multiplier used to determine the
148   * distance driven based on the rotation value from the encoder. Set this value based on how far
149   * the mechanism travels in 1 rotation of the encoder, and factor in gearing reductions following
150   * the encoder shaft. This distance can be in any units you like, linear or angular.
151   *
152   * @param distancePerRotation the distance per rotation of the encoder
153   */
154  public void setDistancePerRotation(double distancePerRotation) {
155    m_distancePerRotation = distancePerRotation;
156  }
157
158  /**
159   * Get the distance per rotation for this encoder.
160   *
161   * @return The scale factor that will be used to convert rotation to useful units.
162   */
163  public double getDistancePerRotation() {
164    return m_distancePerRotation;
165  }
166
167  /**
168   * Get the distance the sensor has driven since the last reset as scaled by the value from {@link
169   * #setDistancePerRotation(double)}.
170   *
171   * @return The distance driven since the last reset
172   */
173  public double getDistance() {
174    return get() * getDistancePerRotation();
175  }
176
177  /**
178   * Get the channel number.
179   *
180   * @return The channel number.
181   */
182  public int getChannel() {
183    return m_analogInput.getChannel();
184  }
185
186  /** Reset the Encoder distance to zero. */
187  public void reset() {
188    m_counter.reset();
189    m_positionOffset = m_analogInput.getVoltage() / RobotController.getVoltage5V();
190  }
191
192  @Override
193  public void close() {
194    m_counter.close();
195    m_analogTrigger.close();
196    if (m_simDevice != null) {
197      m_simDevice.close();
198    }
199  }
200
201  @Override
202  public void initSendable(SendableBuilder builder) {
203    builder.setSmartDashboardType("AbsoluteEncoder");
204    builder.addDoubleProperty("Distance", this::getDistance, null);
205    builder.addDoubleProperty("Distance Per Rotation", this::getDistancePerRotation, null);
206  }
207}