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.HAL;
008import edu.wpi.first.hal.SimDevice;
009import edu.wpi.first.hal.SimDevice.Direction;
010import edu.wpi.first.hal.SimDouble;
011import edu.wpi.first.math.MathUtil;
012import edu.wpi.first.util.sendable.Sendable;
013import edu.wpi.first.util.sendable.SendableBuilder;
014import edu.wpi.first.util.sendable.SendableRegistry;
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 boolean m_ownsAnalogInput;
020  private double m_fullRange;
021  private double m_expectedZero;
022  private double m_sensorMin;
023  private double m_sensorMax = 1.0;
024  private boolean m_isInverted;
025
026  private SimDevice m_simDevice;
027  private SimDouble m_simPosition;
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   * @param fullRange the value to report at maximum travel
034   * @param expectedZero the reading where you would expect a 0 from get()
035   */
036  public AnalogEncoder(int channel, double fullRange, double expectedZero) {
037    this(new AnalogInput(channel), fullRange, expectedZero);
038    m_ownsAnalogInput = true;
039  }
040
041  /**
042   * Construct a new AnalogEncoder attached to a specific AnalogInput.
043   *
044   * @param analogInput the analog input to attach to
045   * @param fullRange the value to report at maximum travel
046   * @param expectedZero the reading where you would expect a 0 from get()
047   */
048  @SuppressWarnings("this-escape")
049  public AnalogEncoder(AnalogInput analogInput, double fullRange, double expectedZero) {
050    m_analogInput = analogInput;
051    init(fullRange, expectedZero);
052  }
053
054  /**
055   * Construct a new AnalogEncoder attached to a specific AnalogIn channel.
056   *
057   * <p>This has a fullRange of 1 and an expectedZero of 0.
058   *
059   * @param channel the analog input channel to attach to
060   */
061  public AnalogEncoder(int channel) {
062    this(channel, 1.0, 0.0);
063  }
064
065  /**
066   * Construct a new AnalogEncoder attached to a specific AnalogInput.
067   *
068   * <p>This has a fullRange of 1 and an expectedZero of 0.
069   *
070   * @param analogInput the analog input to attach to
071   */
072  @SuppressWarnings("this-escape")
073  public AnalogEncoder(AnalogInput analogInput) {
074    this(analogInput, 1.0, 0.0);
075  }
076
077  private void init(double fullRange, double expectedZero) {
078    m_simDevice = SimDevice.create("AnalogEncoder", m_analogInput.getChannel());
079
080    if (m_simDevice != null) {
081      m_simPosition = m_simDevice.createDouble("Position", Direction.kInput, 0.0);
082    }
083
084    m_fullRange = fullRange;
085    m_expectedZero = expectedZero;
086
087    HAL.reportUsage("IO", m_analogInput.getChannel(), "AnalogEncoder");
088
089    SendableRegistry.add(this, "Analog Encoder", m_analogInput.getChannel());
090  }
091
092  private double mapSensorRange(double pos) {
093    // map sensor range
094    if (pos < m_sensorMin) {
095      pos = m_sensorMin;
096    }
097    if (pos > m_sensorMax) {
098      pos = m_sensorMax;
099    }
100    pos = (pos - m_sensorMin) / (m_sensorMax - m_sensorMin);
101    return pos;
102  }
103
104  /**
105   * Get the encoder value.
106   *
107   * @return the encoder value scaled by the full range input
108   */
109  public double get() {
110    if (m_simPosition != null) {
111      return m_simPosition.get();
112    }
113
114    double analog = m_analogInput.getVoltage();
115    double pos = analog / RobotController.getVoltage3V3();
116
117    // Map sensor range if range isn't full
118    pos = mapSensorRange(pos);
119
120    // Compute full range and offset
121    pos = pos * m_fullRange - m_expectedZero;
122
123    // Map from 0 - Full Range
124    double result = MathUtil.inputModulus(pos, 0, m_fullRange);
125    // Invert if necessary
126    if (m_isInverted) {
127      return m_fullRange - result;
128    }
129    return result;
130  }
131
132  /**
133   * Set the encoder voltage percentage range. Analog sensors are not always fully stable at the end
134   * of their travel ranges. Shrinking this range down can help mitigate issues with that.
135   *
136   * @param min minimum voltage percentage (0-1 range)
137   * @param max maximum voltage percentage (0-1 range)
138   */
139  public void setVoltagePercentageRange(double min, double max) {
140    m_sensorMin = MathUtil.clamp(min, 0.0, 1.0);
141    m_sensorMax = MathUtil.clamp(max, 0.0, 1.0);
142  }
143
144  /**
145   * Set if this encoder is inverted.
146   *
147   * @param inverted true to invert the encoder, false otherwise
148   */
149  public void setInverted(boolean inverted) {
150    m_isInverted = inverted;
151  }
152
153  /**
154   * Get the channel number.
155   *
156   * @return The channel number.
157   */
158  public int getChannel() {
159    return m_analogInput.getChannel();
160  }
161
162  @Override
163  public void close() {
164    if (m_ownsAnalogInput) {
165      m_analogInput.close();
166    }
167    if (m_simDevice != null) {
168      m_simDevice.close();
169    }
170  }
171
172  @Override
173  public void initSendable(SendableBuilder builder) {
174    builder.setSmartDashboardType("AbsoluteEncoder");
175    builder.addDoubleProperty("Position", this::get, null);
176  }
177}