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