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.SimBoolean; 008import edu.wpi.first.hal.SimDevice; 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; 014 015/** 016 * Class for supporting duty cycle/PWM encoders, such as the US Digital MA3 with PWM Output, the 017 * CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag Encoder. 018 */ 019public class DutyCycleEncoder implements Sendable, AutoCloseable { 020 private final DutyCycle m_dutyCycle; 021 private boolean m_ownsDutyCycle; 022 private double m_frequencyThreshold = 100; 023 private double m_fullRange; 024 private double m_expectedZero; 025 private double m_periodNanos; 026 private double m_sensorMin; 027 private double m_sensorMax = 1.0; 028 private boolean m_isInverted; 029 030 private SimDevice m_simDevice; 031 private SimDouble m_simPosition; 032 private SimBoolean m_simIsConnected; 033 034 /** 035 * Construct a new DutyCycleEncoder on a specific channel. 036 * 037 * @param channel the channel to attach to 038 * @param fullRange the value to report at maximum travel 039 * @param expectedZero the reading where you would expect a 0 from get() 040 */ 041 @SuppressWarnings("this-escape") 042 public DutyCycleEncoder(int channel, double fullRange, double expectedZero) { 043 m_ownsDutyCycle = true; 044 m_dutyCycle = new DutyCycle(channel); 045 init(fullRange, expectedZero); 046 } 047 048 /** 049 * Construct a new DutyCycleEncoder attached to an existing DutyCycle object. 050 * 051 * @param dutyCycle the duty cycle to attach to 052 * @param fullRange the value to report at maximum travel 053 * @param expectedZero the reading where you would expect a 0 from get() 054 */ 055 @SuppressWarnings("this-escape") 056 public DutyCycleEncoder(DutyCycle dutyCycle, double fullRange, double expectedZero) { 057 m_dutyCycle = dutyCycle; 058 init(fullRange, expectedZero); 059 } 060 061 /** 062 * Construct a new DutyCycleEncoder on a specific channel. 063 * 064 * <p>This has a fullRange of 1 and an expectedZero of 0. 065 * 066 * @param channel the channel to attach to 067 */ 068 @SuppressWarnings("this-escape") 069 public DutyCycleEncoder(int channel) { 070 this(channel, 1.0, 0.0); 071 } 072 073 /** 074 * Construct a new DutyCycleEncoder attached to an existing DutyCycle object. 075 * 076 * <p>This has a fullRange of 1 and an expectedZero of 0. 077 * 078 * @param dutyCycle the duty cycle to attach to 079 */ 080 @SuppressWarnings("this-escape") 081 public DutyCycleEncoder(DutyCycle dutyCycle) { 082 this(dutyCycle, 1.0, 0.0); 083 } 084 085 private void init(double fullRange, double expectedZero) { 086 m_simDevice = SimDevice.create("DutyCycle:DutyCycleEncoder", m_dutyCycle.getSourceChannel()); 087 088 if (m_simDevice != null) { 089 m_simPosition = m_simDevice.createDouble("Position", SimDevice.Direction.kInput, 0.0); 090 m_simIsConnected = m_simDevice.createBoolean("Connected", SimDevice.Direction.kInput, true); 091 } 092 093 m_fullRange = fullRange; 094 m_expectedZero = expectedZero; 095 096 SendableRegistry.add(this, "DutyCycle Encoder", m_dutyCycle.getSourceChannel()); 097 } 098 099 private double mapSensorRange(double pos) { 100 // map sensor range 101 if (pos < m_sensorMin) { 102 pos = m_sensorMin; 103 } 104 if (pos > m_sensorMax) { 105 pos = m_sensorMax; 106 } 107 pos = (pos - m_sensorMin) / (m_sensorMax - m_sensorMin); 108 return pos; 109 } 110 111 /** 112 * Get the encoder value since the last reset. 113 * 114 * <p>This is reported in rotations since the last reset. 115 * 116 * @return the encoder value in rotations 117 */ 118 public double get() { 119 if (m_simPosition != null) { 120 return m_simPosition.get(); 121 } 122 123 double pos; 124 // Compute output percentage (0-1) 125 if (m_periodNanos == 0.0) { 126 pos = m_dutyCycle.getOutput(); 127 } else { 128 int highTime = m_dutyCycle.getHighTimeNanoseconds(); 129 pos = highTime / m_periodNanos; 130 } 131 132 // Map sensor range if range isn't full 133 pos = mapSensorRange(pos); 134 135 // Compute full range and offset 136 pos = pos * m_fullRange - m_expectedZero; 137 138 // Map from 0 - Full Range 139 double result = MathUtil.inputModulus(pos, 0, m_fullRange); 140 // Invert if necessary 141 if (m_isInverted) { 142 return m_fullRange - result; 143 } 144 return result; 145 } 146 147 /** 148 * Set the encoder duty cycle range. As the encoder needs to maintain a duty cycle, the duty cycle 149 * cannot go all the way to 0% or all the way to 100%. For example, an encoder with a 4096 us 150 * period might have a minimum duty cycle of 1 us / 4096 us and a maximum duty cycle of 4095 / 151 * 4096 us. Setting the range will result in an encoder duty cycle less than or equal to the 152 * minimum being output as 0 rotation, the duty cycle greater than or equal to the maximum being 153 * output as 1 rotation, and values in between linearly scaled from 0 to 1. 154 * 155 * @param min minimum duty cycle (0-1 range) 156 * @param max maximum duty cycle (0-1 range) 157 */ 158 public void setDutyCycleRange(double min, double max) { 159 m_sensorMin = MathUtil.clamp(min, 0.0, 1.0); 160 m_sensorMax = MathUtil.clamp(max, 0.0, 1.0); 161 } 162 163 /** 164 * Get the frequency in Hz of the duty cycle signal from the encoder. 165 * 166 * @return duty cycle frequency in Hz 167 */ 168 public double getFrequency() { 169 return m_dutyCycle.getFrequency(); 170 } 171 172 /** 173 * Get if the sensor is connected 174 * 175 * <p>This uses the duty cycle frequency to determine if the sensor is connected. By default, a 176 * value of 100 Hz is used as the threshold, and this value can be changed with {@link 177 * #setConnectedFrequencyThreshold(int)}. 178 * 179 * @return true if the sensor is connected 180 */ 181 public boolean isConnected() { 182 if (m_simIsConnected != null) { 183 return m_simIsConnected.get(); 184 } 185 return getFrequency() > m_frequencyThreshold; 186 } 187 188 /** 189 * Change the frequency threshold for detecting connection used by {@link #isConnected()}. 190 * 191 * @param frequency the minimum frequency in Hz. 192 */ 193 public void setConnectedFrequencyThreshold(double frequency) { 194 if (frequency < 0) { 195 frequency = 0; 196 } 197 198 m_frequencyThreshold = frequency; 199 } 200 201 /** 202 * Sets the assumed frequency of the connected device. 203 * 204 * <p>By default, the DutyCycle engine has to compute the frequency of the input signal. This can 205 * result in both delayed readings and jumpy readings. To solve this, you can pass the expected 206 * frequency of the sensor to this function. This will use that frequency to compute the DutyCycle 207 * percentage, rather than the computed frequency. 208 * 209 * @param frequency the assumed frequency of the sensor 210 */ 211 public void setAssumedFrequency(double frequency) { 212 if (frequency == 0.0) { 213 m_periodNanos = 0.0; 214 } else { 215 m_periodNanos = 1000000000 / frequency; 216 } 217 } 218 219 /** 220 * Set if this encoder is inverted. 221 * 222 * @param inverted true to invert the encoder, false otherwise 223 */ 224 public void setInverted(boolean inverted) { 225 m_isInverted = inverted; 226 } 227 228 @Override 229 public void close() { 230 if (m_ownsDutyCycle) { 231 m_dutyCycle.close(); 232 } 233 if (m_simDevice != null) { 234 m_simDevice.close(); 235 } 236 } 237 238 /** 239 * Get the channel of the source. 240 * 241 * @return the source channel 242 */ 243 public int getSourceChannel() { 244 return m_dutyCycle.getSourceChannel(); 245 } 246 247 @Override 248 public void initSendable(SendableBuilder builder) { 249 builder.setSmartDashboardType("AbsoluteEncoder"); 250 builder.addDoubleProperty("Position", this::get, null); 251 builder.addBooleanProperty("Is Connected", this::isConnected, null); 252 } 253}