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}