[wpilib] Rewrite DutyCycleEncoder and AnalogEncoder (#6398)

This commit is contained in:
Thad House
2024-05-24 11:53:56 -07:00
committed by GitHub
parent 294c9946ae
commit d05c7c125b
18 changed files with 638 additions and 883 deletions

View File

@@ -11,167 +11,143 @@ import edu.wpi.first.math.MathUtil;
import edu.wpi.first.util.sendable.Sendable;
import edu.wpi.first.util.sendable.SendableBuilder;
import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.AnalogTriggerOutput.AnalogTriggerType;
/** Class for supporting continuous analog encoders, such as the US Digital MA3. */
public class AnalogEncoder implements Sendable, AutoCloseable {
private final AnalogInput m_analogInput;
private AnalogTrigger m_analogTrigger;
private Counter m_counter;
private double m_positionOffset;
private double m_distancePerRotation = 1.0;
private double m_lastPosition;
private boolean m_ownsAnalogInput;
private double m_fullRange;
private double m_expectedZero;
private double m_sensorMin;
private double m_sensorMax = 1.0;
private boolean m_isInverted;
private SimDevice m_simDevice;
private SimDouble m_simPosition;
private SimDouble m_simAbsolutePosition;
/**
* Construct a new AnalogEncoder attached to a specific AnalogIn channel.
*
* @param channel the analog input channel to attach to
* @param fullRange the value to report at maximum travel
* @param expectedZero the reading where you would expect a 0 from get()
*/
public AnalogEncoder(int channel) {
this(new AnalogInput(channel));
public AnalogEncoder(int channel, double fullRange, double expectedZero) {
this(new AnalogInput(channel), fullRange, expectedZero);
m_ownsAnalogInput = true;
}
/**
* Construct a new AnalogEncoder attached to a specific AnalogInput.
*
* @param analogInput the analog input to attach to
* @param fullRange the value to report at maximum travel
* @param expectedZero the reading where you would expect a 0 from get()
*/
@SuppressWarnings("this-escape")
public AnalogEncoder(AnalogInput analogInput, double fullRange, double expectedZero) {
m_analogInput = analogInput;
init(fullRange, expectedZero);
}
/**
* Construct a new AnalogEncoder attached to a specific AnalogIn channel.
*
* <p>This has a fullRange of 1 and an expectedZero of 0.
*
* @param channel the analog input channel to attach to
*/
public AnalogEncoder(int channel) {
this(channel, 1.0, 0.0);
}
/**
* Construct a new AnalogEncoder attached to a specific AnalogInput.
*
* <p>This has a fullRange of 1 and an expectedZero of 0.
*
* @param analogInput the analog input to attach to
*/
@SuppressWarnings("this-escape")
public AnalogEncoder(AnalogInput analogInput) {
m_analogInput = analogInput;
init();
this(analogInput, 1.0, 0.0);
}
private void init() {
m_analogTrigger = new AnalogTrigger(m_analogInput);
m_counter = new Counter();
private void init(double fullRange, double expectedZero) {
m_simDevice = SimDevice.create("AnalogEncoder", m_analogInput.getChannel());
if (m_simDevice != null) {
m_simPosition = m_simDevice.createDouble("Position", Direction.kInput, 0.0);
m_simAbsolutePosition = m_simDevice.createDouble("absPosition", Direction.kInput, 0.0);
}
// Limits need to be 25% from each end
m_analogTrigger.setLimitsVoltage(1.25, 3.75);
m_counter.setUpSource(m_analogTrigger, AnalogTriggerType.kRisingPulse);
m_counter.setDownSource(m_analogTrigger, AnalogTriggerType.kFallingPulse);
m_fullRange = fullRange;
m_expectedZero = expectedZero;
SendableRegistry.addLW(this, "Analog Encoder", m_analogInput.getChannel());
}
private boolean doubleEquals(double a, double b) {
double epsilon = 0.00001d;
return Math.abs(a - b) < epsilon;
private double mapSensorRange(double pos) {
// map sensor range
if (pos < m_sensorMin) {
pos = m_sensorMin;
}
if (pos > m_sensorMax) {
pos = m_sensorMax;
}
pos = (pos - m_sensorMin) / (m_sensorMax - m_sensorMin);
return pos;
}
/**
* Get the encoder value since the last reset.
* Get the encoder value.
*
* <p>This is reported in rotations since the last reset.
* <p>By default, this will not count rollovers. If that behavior is necessary, call
* configureRolloverCounting(true).
*
* @return the encoder value in rotations
* @return the encoder value
*/
public double get() {
if (m_simPosition != null) {
return m_simPosition.get();
}
// As the values are not atomic, keep trying until we get 2 reads of the same
// value. If we don't within 10 attempts, warn
for (int i = 0; i < 10; i++) {
double counter = m_counter.get();
double pos = m_analogInput.getVoltage();
double counter2 = m_counter.get();
double pos2 = m_analogInput.getVoltage();
if (counter == counter2 && doubleEquals(pos, pos2)) {
pos = pos / RobotController.getVoltage5V();
double position = counter + pos - m_positionOffset;
m_lastPosition = position;
return position;
}
double analog = m_analogInput.getVoltage();
double pos = analog / RobotController.getVoltage5V();
// Map sensor range if range isn't full
pos = mapSensorRange(pos);
// Compute full range and offset
pos = pos * m_fullRange - m_expectedZero;
// Map from 0 - Full Range
double result = MathUtil.inputModulus(pos, 0, m_fullRange);
// Invert if necessary
if (m_isInverted) {
return m_fullRange - result;
}
DriverStation.reportWarning(
"Failed to read Analog Encoder. Potential Speed Overrun. Returning last value", false);
return m_lastPosition;
return result;
}
/**
* Get the absolute position of the analog encoder.
* Set the encoder voltage percentage range. Analog sensors are not always fully stable at the end
* of their travel ranges. Shrinking this range down can help mitigate issues with that.
*
* <p>getAbsolutePosition() - getPositionOffset() will give an encoder absolute position relative
* to the last reset. This could potentially be negative, which needs to be accounted for.
*
* <p>This will not account for rollovers, and will always be just the raw absolute position.
*
* @return the absolute position
* @param min minimum voltage percentage (0-1 range)
* @param max maximum voltage percentage (0-1 range)
*/
public double getAbsolutePosition() {
if (m_simAbsolutePosition != null) {
return m_simAbsolutePosition.get();
}
return m_analogInput.getVoltage() / RobotController.getVoltage5V();
public void setVoltagePercentageRange(double min, double max) {
m_sensorMin = MathUtil.clamp(min, 0.0, 1.0);
m_sensorMax = MathUtil.clamp(max, 0.0, 1.0);
}
/**
* Get the offset of position relative to the last reset.
* Set if this encoder is inverted.
*
* <p>getAbsolutePosition() - getPositionOffset() will give an encoder absolute position relative
* to the last reset. This could potentially be negative, which needs to be accounted for.
*
* @return the position offset
* @param inverted true to invert the encoder, false otherwise
*/
public double getPositionOffset() {
return m_positionOffset;
}
/**
* Set the position offset.
*
* <p>This must be in the range of 0-1.
*
* @param offset the offset
*/
public void setPositionOffset(double offset) {
m_positionOffset = MathUtil.clamp(offset, 0.0, 1.0);
}
/**
* Set the distance per rotation of the encoder. This sets the multiplier used to determine the
* distance driven based on the rotation value from the encoder. Set this value based on how far
* the mechanism travels in 1 rotation of the encoder, and factor in gearing reductions following
* the encoder shaft. This distance can be in any units you like, linear or angular.
*
* @param distancePerRotation the distance per rotation of the encoder
*/
public void setDistancePerRotation(double distancePerRotation) {
m_distancePerRotation = distancePerRotation;
}
/**
* Get the distance per rotation for this encoder.
*
* @return The scale factor that will be used to convert rotation to useful units.
*/
public double getDistancePerRotation() {
return m_distancePerRotation;
}
/**
* Get the distance the sensor has driven since the last reset as scaled by the value from {@link
* #setDistancePerRotation(double)}.
*
* @return The distance driven since the last reset
*/
public double getDistance() {
return get() * getDistancePerRotation();
public void setInverted(boolean inverted) {
m_isInverted = inverted;
}
/**
@@ -183,16 +159,11 @@ public class AnalogEncoder implements Sendable, AutoCloseable {
return m_analogInput.getChannel();
}
/** Reset the Encoder distance to zero. */
public void reset() {
m_counter.reset();
m_positionOffset = m_analogInput.getVoltage() / RobotController.getVoltage5V();
}
@Override
public void close() {
m_counter.close();
m_analogTrigger.close();
if (m_ownsAnalogInput) {
m_analogInput.close();
}
if (m_simDevice != null) {
m_simDevice.close();
}
@@ -201,7 +172,6 @@ public class AnalogEncoder implements Sendable, AutoCloseable {
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("AbsoluteEncoder");
builder.addDoubleProperty("Distance", this::getDistance, null);
builder.addDoubleProperty("Distance Per Rotation", this::getDistancePerRotation, null);
builder.addDoubleProperty("Position", this::get, null);
}
}

View File

@@ -11,7 +11,6 @@ import edu.wpi.first.math.MathUtil;
import edu.wpi.first.util.sendable.Sendable;
import edu.wpi.first.util.sendable.SendableBuilder;
import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.AnalogTriggerOutput.AnalogTriggerType;
/**
* Class for supporting duty cycle/PWM encoders, such as the US Digital MA3 with PWM Output, the
@@ -21,75 +20,107 @@ public class DutyCycleEncoder implements Sendable, AutoCloseable {
private final DutyCycle m_dutyCycle;
private boolean m_ownsDutyCycle;
private DigitalInput m_digitalInput;
private AnalogTrigger m_analogTrigger;
private Counter m_counter;
private int m_frequencyThreshold = 100;
private double m_positionOffset;
private double m_distancePerRotation = 1.0;
private double m_lastPosition;
private double m_fullRange;
private double m_expectedZero;
private double m_periodNanos;
private double m_sensorMin;
private double m_sensorMax = 1.0;
private boolean m_isInverted;
private SimDevice m_simDevice;
private SimDouble m_simPosition;
private SimDouble m_simAbsolutePosition;
private SimDouble m_simDistancePerRotation;
private SimBoolean m_simIsConnected;
/**
* Construct a new DutyCycleEncoder on a specific channel.
*
* @param channel the channel to attach to
* @param fullRange the value to report at maximum travel
* @param expectedZero the reading where you would expect a 0 from get()
*/
@SuppressWarnings("this-escape")
public DutyCycleEncoder(int channel) {
public DutyCycleEncoder(int channel, double fullRange, double expectedZero) {
m_digitalInput = new DigitalInput(channel);
m_ownsDutyCycle = true;
m_dutyCycle = new DutyCycle(m_digitalInput);
init();
init(fullRange, expectedZero);
}
/**
* Construct a new DutyCycleEncoder attached to an existing DutyCycle object.
*
* @param dutyCycle the duty cycle to attach to
* @param fullRange the value to report at maximum travel
* @param expectedZero the reading where you would expect a 0 from get()
*/
@SuppressWarnings("this-escape")
public DutyCycleEncoder(DutyCycle dutyCycle) {
public DutyCycleEncoder(DutyCycle dutyCycle, double fullRange, double expectedZero) {
m_dutyCycle = dutyCycle;
init();
init(fullRange, expectedZero);
}
/**
* Construct a new DutyCycleEncoder attached to a DigitalSource object.
*
* @param source the digital source to attach to
* @param fullRange the value to report at maximum travel
* @param expectedZero the reading where you would expect a 0 from get()
*/
@SuppressWarnings("this-escape")
public DutyCycleEncoder(DigitalSource source, double fullRange, double expectedZero) {
m_ownsDutyCycle = true;
m_dutyCycle = new DutyCycle(source);
init(fullRange, expectedZero);
}
/**
* Construct a new DutyCycleEncoder on a specific channel.
*
* <p>This has a fullRange of 1 and an expectedZero of 0.
*
* @param channel the channel to attach to
*/
@SuppressWarnings("this-escape")
public DutyCycleEncoder(int channel) {
this(channel, 1.0, 0.0);
}
/**
* Construct a new DutyCycleEncoder attached to an existing DutyCycle object.
*
* <p>This has a fullRange of 1 and an expectedZero of 0.
*
* @param dutyCycle the duty cycle to attach to
*/
@SuppressWarnings("this-escape")
public DutyCycleEncoder(DutyCycle dutyCycle) {
this(dutyCycle, 1.0, 0.0);
}
/**
* Construct a new DutyCycleEncoder attached to a DigitalSource object.
*
* <p>This has a fullRange of 1 and an expectedZero of 0.
*
* @param source the digital source to attach to
*/
@SuppressWarnings("this-escape")
public DutyCycleEncoder(DigitalSource source) {
m_ownsDutyCycle = true;
m_dutyCycle = new DutyCycle(source);
init();
this(source, 1.0, 0.0);
}
private void init() {
private void init(double fullRange, double expectedZero) {
m_simDevice = SimDevice.create("DutyCycle:DutyCycleEncoder", m_dutyCycle.getSourceChannel());
if (m_simDevice != null) {
m_simPosition = m_simDevice.createDouble("position", SimDevice.Direction.kInput, 0.0);
m_simDistancePerRotation =
m_simDevice.createDouble("distance_per_rot", SimDevice.Direction.kOutput, 1.0);
m_simAbsolutePosition =
m_simDevice.createDouble("absPosition", SimDevice.Direction.kInput, 0.0);
m_simIsConnected = m_simDevice.createBoolean("connected", SimDevice.Direction.kInput, true);
} else {
m_counter = new Counter();
m_analogTrigger = new AnalogTrigger(m_dutyCycle);
m_analogTrigger.setLimitsDutyCycle(0.25, 0.75);
m_counter.setUpSource(m_analogTrigger, AnalogTriggerType.kRisingPulse);
m_counter.setDownSource(m_analogTrigger, AnalogTriggerType.kFallingPulse);
m_simPosition = m_simDevice.createDouble("Position", SimDevice.Direction.kInput, 0.0);
m_simIsConnected = m_simDevice.createBoolean("Connected", SimDevice.Direction.kInput, true);
}
m_fullRange = fullRange;
m_expectedZero = expectedZero;
SendableRegistry.addLW(this, "DutyCycle Encoder", m_dutyCycle.getSourceChannel());
}
@@ -105,11 +136,6 @@ public class DutyCycleEncoder implements Sendable, AutoCloseable {
return pos;
}
private boolean doubleEquals(double a, double b) {
double epsilon = 0.00001d;
return Math.abs(a - b) < epsilon;
}
/**
* Get the encoder value since the last reset.
*
@@ -122,67 +148,28 @@ public class DutyCycleEncoder implements Sendable, AutoCloseable {
return m_simPosition.get();
}
// As the values are not atomic, keep trying until we get 2 reads of the same
// value
// If we don't within 10 attempts, error
for (int i = 0; i < 10; i++) {
double counter = m_counter.get();
double pos = m_dutyCycle.getOutput();
double counter2 = m_counter.get();
double pos2 = m_dutyCycle.getOutput();
if (counter == counter2 && doubleEquals(pos, pos2)) {
// map sensor range
pos = mapSensorRange(pos);
double position = counter + pos - m_positionOffset;
m_lastPosition = position;
return position;
}
double pos;
// Compute output percentage (0-1)
if (m_periodNanos == 0.0) {
pos = m_dutyCycle.getOutput();
} else {
int highTime = m_dutyCycle.getHighTimeNanoseconds();
pos = highTime / m_periodNanos;
}
DriverStation.reportWarning(
"Failed to read Analog Encoder. Potential Speed Overrun. Returning last value", false);
return m_lastPosition;
}
// Map sensor range if range isn't full
pos = mapSensorRange(pos);
/**
* Get the absolute position of the duty cycle encoder.
*
* <p>getAbsolutePosition() - getPositionOffset() will give an encoder absolute position relative
* to the last reset. This could potentially be negative, which needs to be accounted for.
*
* <p>This will not account for rollovers, and will always be just the raw absolute position.
*
* @return the absolute position
*/
public double getAbsolutePosition() {
if (m_simAbsolutePosition != null) {
return m_simAbsolutePosition.get();
// Compute full range and offset
pos = pos * m_fullRange - m_expectedZero;
// Map from 0 - Full Range
double result = MathUtil.inputModulus(pos, 0, m_fullRange);
// Invert if necessary
if (m_isInverted) {
return m_fullRange - result;
}
return mapSensorRange(m_dutyCycle.getOutput());
}
/**
* Get the offset of position relative to the last reset.
*
* <p>getAbsolutePosition() - getPositionOffset() will give an encoder absolute position relative
* to the last reset. This could potentially be negative, which needs to be accounted for.
*
* @return the position offset
*/
public double getPositionOffset() {
return m_positionOffset;
}
/**
* Set the position offset.
*
* <p>This must be in the range of 0-1.
*
* @param offset the offset
*/
public void setPositionOffset(double offset) {
m_positionOffset = MathUtil.clamp(offset, 0.0, 1.0);
return result;
}
/**
@@ -201,40 +188,6 @@ public class DutyCycleEncoder implements Sendable, AutoCloseable {
m_sensorMax = MathUtil.clamp(max, 0.0, 1.0);
}
/**
* Set the distance per rotation of the encoder. This sets the multiplier used to determine the
* distance driven based on the rotation value from the encoder. Set this value based on how far
* the mechanism travels in 1 rotation of the encoder, and factor in gearing reductions following
* the encoder shaft. This distance can be in any units you like, linear or angular.
*
* @param distancePerRotation the distance per rotation of the encoder
*/
public void setDistancePerRotation(double distancePerRotation) {
m_distancePerRotation = distancePerRotation;
if (m_simDistancePerRotation != null) {
m_simDistancePerRotation.set(distancePerRotation);
}
}
/**
* Get the distance per rotation for this encoder.
*
* @return The scale factor that will be used to convert rotation to useful units.
*/
public double getDistancePerRotation() {
return m_distancePerRotation;
}
/**
* Get the distance the sensor has driven since the last reset as scaled by the value from {@link
* #setDistancePerRotation(double)}.
*
* @return The distance driven since the last reset
*/
public double getDistance() {
return get() * getDistancePerRotation();
}
/**
* Get the frequency in Hz of the duty cycle signal from the encoder.
*
@@ -244,17 +197,6 @@ public class DutyCycleEncoder implements Sendable, AutoCloseable {
return m_dutyCycle.getFrequency();
}
/** Reset the Encoder distance to zero. */
public void reset() {
if (m_counter != null) {
m_counter.reset();
}
if (m_simPosition != null) {
m_simPosition.set(0);
}
m_positionOffset = getAbsolutePosition();
}
/**
* Get if the sensor is connected
*
@@ -284,14 +226,35 @@ public class DutyCycleEncoder implements Sendable, AutoCloseable {
m_frequencyThreshold = frequency;
}
/**
* Sets the assumed frequency of the connected device.
*
* <p>By default, the DutyCycle engine has to compute the frequency of the input signal. This can
* result in both delayed readings and jumpy readings. To solve this, you can pass the expected
* frequency of the sensor to this function. This will use that frequency to compute the DutyCycle
* percentage, rather than the computed frequency.
*
* @param frequency the assumed frequency of the sensor
*/
public void setAssumedFrequency(double frequency) {
if (frequency == 0.0) {
m_periodNanos = 0.0;
} else {
m_periodNanos = 1000000000 / frequency;
}
}
/**
* Set if this encoder is inverted.
*
* @param inverted true to invert the encoder, false otherwise
*/
public void setInverted(boolean inverted) {
m_isInverted = inverted;
}
@Override
public void close() {
if (m_counter != null) {
m_counter.close();
}
if (m_analogTrigger != null) {
m_analogTrigger.close();
}
if (m_ownsDutyCycle) {
m_dutyCycle.close();
}
@@ -324,8 +287,7 @@ public class DutyCycleEncoder implements Sendable, AutoCloseable {
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("AbsoluteEncoder");
builder.addDoubleProperty("Distance", this::getDistance, null);
builder.addDoubleProperty("Distance Per Rotation", this::getDistancePerRotation, null);
builder.addDoubleProperty("Position", this::get, null);
builder.addBooleanProperty("Is Connected", this::isConnected, null);
}
}

View File

@@ -5,7 +5,6 @@
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.AnalogEncoder;
/** Class to control a simulated analog encoder. */
@@ -24,21 +23,12 @@ public class AnalogEncoderSim {
}
/**
* Set the position using an {@link Rotation2d}.
* Set the position.
*
* @param angle The angle.
* @param value The position.
*/
public void setPosition(Rotation2d angle) {
setTurns(angle.getDegrees() / 360.0);
}
/**
* Set the position of the encoder.
*
* @param turns The position.
*/
public void setTurns(double turns) {
m_simPosition.set(turns);
public void set(double value) {
m_simPosition.set(value);
}
/**
@@ -46,16 +36,7 @@ public class AnalogEncoderSim {
*
* @return The simulated position.
*/
public double getTurns() {
public double get() {
return m_simPosition.get();
}
/**
* Get the position as a {@link Rotation2d}.
*
* @return The position as a {@link Rotation2d}.
*/
public Rotation2d getPosition() {
return Rotation2d.fromDegrees(getTurns() * 360.0);
}
}

View File

@@ -11,8 +11,6 @@ import edu.wpi.first.wpilibj.DutyCycleEncoder;
/** Class to control a simulated duty cycle encoder. */
public class DutyCycleEncoderSim {
private final SimDouble m_simPosition;
private final SimDouble m_simDistancePerRotation;
private final SimDouble m_simAbsolutePosition;
private final SimBoolean m_simIsConnected;
/**
@@ -31,10 +29,8 @@ public class DutyCycleEncoderSim {
*/
public DutyCycleEncoderSim(int channel) {
SimDeviceSim wrappedSimDevice = new SimDeviceSim("DutyCycle:DutyCycleEncoder", channel);
m_simPosition = wrappedSimDevice.getDouble("position");
m_simDistancePerRotation = wrappedSimDevice.getDouble("distance_per_rot");
m_simAbsolutePosition = wrappedSimDevice.getDouble("absPosition");
m_simIsConnected = wrappedSimDevice.getBoolean("connected");
m_simPosition = wrappedSimDevice.getDouble("Position");
m_simIsConnected = wrappedSimDevice.getBoolean("Connected");
}
/**
@@ -47,57 +43,12 @@ public class DutyCycleEncoderSim {
}
/**
* Set the position in turns.
* Set the position.
*
* @param turns The position.
* @param value The position.
*/
public void set(double turns) {
m_simPosition.set(turns);
}
/**
* Get the distance.
*
* @return The distance.
*/
public double getDistance() {
return m_simPosition.get() * m_simDistancePerRotation.get();
}
/**
* Set the distance.
*
* @param distance The distance.
*/
public void setDistance(double distance) {
m_simPosition.set(distance / m_simDistancePerRotation.get());
}
/**
* Get the absolute position.
*
* @return The absolute position
*/
public double getAbsolutePosition() {
return m_simAbsolutePosition.get();
}
/**
* Set the absolute position.
*
* @param position The absolute position
*/
public void setAbsolutePosition(double position) {
m_simAbsolutePosition.set(position);
}
/**
* Get the distance per rotation for this encoder.
*
* @return The scale factor that will be used to convert rotation to useful units.
*/
public double getDistancePerRotation() {
return m_simDistancePerRotation.get();
public void set(double value) {
m_simPosition.set(value);
}
/**