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