From 7c248d3b63df69a93a514df8bb72645e63b09e5e Mon Sep 17 00:00:00 2001 From: Thad House Date: Sun, 25 Feb 2024 13:47:04 -0800 Subject: [PATCH 01/30] Deprecate AbsoluteEncoder classes, Replace DCE with DutyCyclePotentiometer --- myRobot/src/main/java/frc/robot/MyRobot.java | 13 +- .../edu/wpi/first/wpilibj/AnalogEncoder.java | 8 +- .../first/wpilibj/AnalogPotentiometer.java | 174 +++++++-- .../wpi/first/wpilibj/DutyCycleEncoder.java | 4 + .../first/wpilibj/DutyCyclePotentiometer.java | 330 ++++++++++++++++++ .../wpilibj/simulation/AnalogEncoderSim.java | 2 + .../simulation/DutyCycleEncoderSim.java | 2 + 7 files changed, 498 insertions(+), 35 deletions(-) create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCyclePotentiometer.java diff --git a/myRobot/src/main/java/frc/robot/MyRobot.java b/myRobot/src/main/java/frc/robot/MyRobot.java index a5f88bd3f36..5a9a56c493c 100644 --- a/myRobot/src/main/java/frc/robot/MyRobot.java +++ b/myRobot/src/main/java/frc/robot/MyRobot.java @@ -4,15 +4,21 @@ package frc.robot; +import edu.wpi.first.wpilibj.DutyCyclePotentiometer; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class MyRobot extends TimedRobot { + DutyCyclePotentiometer pot; /** * This function is run when the robot is first started up and should be used for any * initialization code. */ @Override - public void robotInit() {} + public void robotInit() { + pot = new DutyCyclePotentiometer(0); + pot.setAssumedFrequency(967.77); + } /** This function is run once each time the robot enters autonomous mode. */ @Override @@ -36,5 +42,8 @@ public void testPeriodic() {} /** This function is called periodically during all modes. */ @Override - public void robotPeriodic() {} + public void robotPeriodic() { + SmartDashboard.putNumber("DC", pot.get()); + SmartDashboard.putNumber("Freq", pot.getFrequency()); + } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java index 727ad2cd3de..767815bfd32 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java @@ -13,7 +13,13 @@ 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. */ +/** + * Class for supporting continuous analog encoders, such as the US Digital MA3. + * + * @deprecated Use AnalogPotentiometer instead. If rollover support is necessary, + * use enableRolloverSupport() on AnalogPotentiometer. + */ +@Deprecated(since = "2025", forRemoval = true) public class AnalogEncoder implements Sendable, AutoCloseable { private final AnalogInput m_analogInput; private AnalogTrigger m_analogTrigger; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java index 15473495528..19e5ec4b87d 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java @@ -4,13 +4,20 @@ package edu.wpi.first.wpilibj; +import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; +import edu.wpi.first.hal.SimDevice; +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.hal.SimDevice.Direction; 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 reading analog potentiometers. Analog potentiometers read in an analog voltage that - * corresponds to a position. The position is in whichever units you choose, by way of the scaling + * Class for reading analog potentiometers. Analog potentiometers read in an + * analog voltage that + * corresponds to a position. The position is in whichever units you choose, by + * way of the scaling * and offset constants passed to the constructor. */ public class AnalogPotentiometer implements Sendable, AutoCloseable { @@ -18,20 +25,42 @@ public class AnalogPotentiometer implements Sendable, AutoCloseable { private boolean m_initAnalogInput; private double m_fullRange; private double m_offset; + private AnalogTrigger m_rotationTrigger; + private Counter m_rotationCounter; + private double m_lastPosition; + + private SimDevice m_simDevice; + private SimDouble m_simPosition; + + private void init() { + m_simDevice = SimDevice.create("AnalogPotentiometer", m_analogInput.getChannel()); + + if (m_simDevice != null) { + m_simPosition = m_simDevice.createDouble("Position", Direction.kInput, 0.0); + } + } /** * AnalogPotentiometer constructor. * - *

Use the fullRange and offset values so that the output produces meaningful values. I.E: you - * have a 270 degree potentiometer, and you want the output to be degrees with the halfway point - * as 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 since the halfway - * point after scaling is 135 degrees. This will calculate the result from the fullRange times the + *

+ * Use the fullRange and offset values so that the output produces meaningful + * values. I.E: you + * have a 270 degree potentiometer, and you want the output to be degrees with + * the halfway point + * as 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 + * since the halfway + * point after scaling is 135 degrees. This will calculate the result from the + * fullRange times the * fraction of the supply voltage, plus the offset. * - * @param channel The analog input channel this potentiometer is plugged into. 0-3 are on-board - * and 4-7 are on the MXP port. - * @param fullRange The scaling to multiply the fraction by to get a meaningful unit. - * @param offset The offset to add to the scaled value for controlling the zero value + * @param channel The analog input channel this potentiometer is plugged into. + * 0-3 are on-board + * and 4-7 are on the MXP port. + * @param fullRange The scaling to multiply the fraction by to get a meaningful + * unit. + * @param offset The offset to add to the scaled value for controlling the + * zero value */ @SuppressWarnings("this-escape") public AnalogPotentiometer(final int channel, double fullRange, double offset) { @@ -43,37 +72,51 @@ public AnalogPotentiometer(final int channel, double fullRange, double offset) { /** * AnalogPotentiometer constructor. * - *

Use the fullRange and offset values so that the output produces meaningful values. I.E: you - * have a 270 degree potentiometer, and you want the output to be degrees with the halfway point - * as 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 since the halfway - * point after scaling is 135 degrees. This will calculate the result from the fullRange times the + *

+ * Use the fullRange and offset values so that the output produces meaningful + * values. I.E: you + * have a 270 degree potentiometer, and you want the output to be degrees with + * the halfway point + * as 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 + * since the halfway + * point after scaling is 135 degrees. This will calculate the result from the + * fullRange times the * fraction of the supply voltage, plus the offset. * - * @param input The {@link AnalogInput} this potentiometer is plugged into. - * @param fullRange The angular value (in desired units) representing the full 0-5V range of the - * input. - * @param offset The angular value (in desired units) representing the angular output at 0V. + * @param input The {@link AnalogInput} this potentiometer is plugged into. + * @param fullRange The angular value (in desired units) representing the full + * 0-5V range of the + * input. + * @param offset The angular value (in desired units) representing the + * angular output at 0V. */ @SuppressWarnings("this-escape") public AnalogPotentiometer(final AnalogInput input, double fullRange, double offset) { SendableRegistry.addLW(this, "AnalogPotentiometer", input.getChannel()); - m_analogInput = input; + m_analogInput = requireNonNullParam(input, "input", "AnalogPotentiometer"); m_initAnalogInput = false; m_fullRange = fullRange; m_offset = offset; + + init(); } /** * AnalogPotentiometer constructor. * - *

Use the scale value so that the output produces meaningful values. I.E: you have a 270 - * degree potentiometer, and you want the output to be degrees with the starting point as 0 + *

+ * Use the scale value so that the output produces meaningful values. I.E: you + * have a 270 + * degree potentiometer, and you want the output to be degrees with the starting + * point as 0 * degrees. The scale value is 270.0(degrees). * - * @param channel The analog input channel this potentiometer is plugged into. 0-3 are on-board - * and 4-7 are on the MXP port. - * @param scale The scaling to multiply the voltage by to get a meaningful unit. + * @param channel The analog input channel this potentiometer is plugged into. + * 0-3 are on-board + * and 4-7 are on the MXP port. + * @param scale The scaling to multiply the voltage by to get a meaningful + * unit. */ public AnalogPotentiometer(final int channel, double scale) { this(channel, scale, 0); @@ -82,8 +125,11 @@ public AnalogPotentiometer(final int channel, double scale) { /** * AnalogPotentiometer constructor. * - *

Use the fullRange and offset values so that the output produces meaningful values. I.E: you - * have a 270 degree potentiometer, and you want the output to be degrees with the starting point + *

+ * Use the fullRange and offset values so that the output produces meaningful + * values. I.E: you + * have a 270 degree potentiometer, and you want the output to be degrees with + * the starting point * as 0 degrees. The scale value is 270.0(degrees). * * @param input The {@link AnalogInput} this potentiometer is plugged into. @@ -96,10 +142,12 @@ public AnalogPotentiometer(final AnalogInput input, double scale) { /** * AnalogPotentiometer constructor. * - *

The potentiometer will return a value between 0 and 1.0. + *

+ * The potentiometer will return a value between 0 and 1.0. * - * @param channel The analog input channel this potentiometer is plugged into. 0-3 are on-board - * and 4-7 are on the MXP port. + * @param channel The analog input channel this potentiometer is plugged into. + * 0-3 are on-board + * and 4-7 are on the MXP port. */ public AnalogPotentiometer(final int channel) { this(channel, 1, 0); @@ -108,7 +156,8 @@ public AnalogPotentiometer(final int channel) { /** * AnalogPotentiometer constructor. * - *

The potentiometer will return a value between 0 and 1.0. + *

+ * The potentiometer will return a value between 0 and 1.0. * * @param input The {@link AnalogInput} this potentiometer is plugged into. */ @@ -116,19 +165,73 @@ public AnalogPotentiometer(final AnalogInput input) { this(input, 1, 0); } + private boolean doubleEquals(double a, double b) { + double epsilon = 0.00001d; + return Math.abs(a - b) < epsilon; + } + + private double getRollovers() { + 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_rotationCounter.get(); + double pos = m_analogInput.getVoltage(); + double counter2 = m_rotationCounter.get(); + double pos2 = m_analogInput.getVoltage(); + if (counter == counter2 && doubleEquals(pos, pos2)) { + pos = pos / RobotController.getVoltage5V(); + double position = (counter + pos) * m_fullRange + m_offset; + m_lastPosition = position; + return position; + } + } + + DriverStation.reportWarning( + "Failed to read Analog Encoder. Potential Speed Overrun. Returning last value", false); + return m_lastPosition; + } + /** * Get the current reading of the potentiometer. * * @return The current position of the potentiometer. */ public double get() { - if (m_analogInput == null) { - return m_offset; + if (m_simPosition != null) { + return m_simPosition.get(); + } + + if (m_rotationCounter != null) { + return getRollovers(); } + return (m_analogInput.getAverageVoltage() / RobotController.getVoltage5V()) * m_fullRange + m_offset; } + public void enableRolloverSupport() { + if (m_rotationCounter != null) { + return; + } + m_rotationTrigger = new AnalogTrigger(m_analogInput); + m_rotationCounter = new Counter(); + + // Limits need to be 25% from each end + m_rotationTrigger.setLimitsVoltage(1.25, 3.75); + m_rotationCounter.setUpSource(m_rotationTrigger, AnalogTriggerType.kRisingPulse); + m_rotationCounter.setDownSource(m_rotationTrigger, AnalogTriggerType.kFallingPulse); + } + + public void resetRollovers() { + if (m_rotationCounter != null) { + m_rotationCounter.reset(); + } + } + @Override public void initSendable(SendableBuilder builder) { if (m_analogInput != null) { @@ -140,10 +243,17 @@ public void initSendable(SendableBuilder builder) { @Override public void close() { SendableRegistry.remove(this); + if (m_rotationCounter != null) { + m_rotationCounter.close(); + m_rotationTrigger.close(); + m_rotationCounter = null; + m_rotationTrigger = null; + } if (m_initAnalogInput) { m_analogInput.close(); m_analogInput = null; m_initAnalogInput = false; } + } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java index a0858cef922..c9c7383cce3 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java @@ -16,7 +16,11 @@ /** * Class for supporting duty cycle/PWM encoders, such as the US Digital MA3 with PWM Output, the * CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag Encoder. + * + * @deprecated Use DutyCyclePotentiometer instead. Rollover support is not supported for + * duty cycle encoders, there is no reliable way to do so. */ +@Deprecated(since = "2025", forRemoval = true) public class DutyCycleEncoder implements Sendable, AutoCloseable { private final DutyCycle m_dutyCycle; private boolean m_ownsDutyCycle; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCyclePotentiometer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCyclePotentiometer.java new file mode 100644 index 00000000000..81268d12e51 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCyclePotentiometer.java @@ -0,0 +1,330 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj; + +import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; + +import edu.wpi.first.hal.SimBoolean; +import edu.wpi.first.hal.SimDevice; +import edu.wpi.first.hal.SimDevice.Direction; +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.util.sendable.SendableRegistry; + +/** + * Class for reading duty cyle potentiometers. Duty cycle potentiometers read in + * an pwm signal that + * corresponds to a position. The position is in whichever units you choose, by + * way of the scaling + * and offset constants passed to the constructor. + */ +public class DutyCyclePotentiometer implements Sendable, AutoCloseable { + private DutyCycle m_dutyCycle; + private DigitalSource m_digitalSource; + private boolean m_ownsDutyCycle; + private double m_fullRange; + private double m_offset; + private int m_frequencyThreshold = 100; + private double m_periodNanos; + + private SimDevice m_simDevice; + private SimDouble m_simPosition; + private SimBoolean m_simIsConnected; + + private final void init(double fullRange, double offset) { + SendableRegistry.addLW(this, "DutyCyclePotentiometer", m_dutyCycle.getFPGAIndex()); + m_fullRange = fullRange; + m_offset = offset; + + m_simDevice = SimDevice.create("DutyCyclePotentiometer", m_dutyCycle.getFPGAIndex()); + + if (m_simDevice != null) { + m_simPosition = m_simDevice.createDouble("Position", Direction.kInput, 0.0); + m_simIsConnected = m_simDevice.createBoolean("Connected", SimDevice.Direction.kInput, true); + } + } + + /** + * AnalogPotentiometer constructor. + * + *

+ * Use the fullRange and offset values so that the output produces meaningful + * values. I.E: you + * have a 270 degree potentiometer, and you want the output to be degrees with + * the halfway point + * as 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 + * since the halfway + * point after scaling is 135 degrees. This will calculate the result from the + * fullRange times the + * fraction of the supply voltage, plus the offset. + * + * @param channel The analog input channel this potentiometer is plugged into. + * 0-3 are on-board + * and 4-7 are on the MXP port. + * @param fullRange The scaling to multiply the fraction by to get a meaningful + * unit. + * @param offset The offset to add to the scaled value for controlling the + * zero value + */ + @SuppressWarnings("this-escape") + public DutyCyclePotentiometer(final int channel, double fullRange, double offset) { + m_digitalSource = new DigitalInput(channel); + m_dutyCycle = new DutyCycle(m_digitalSource); + m_ownsDutyCycle = true; + init(fullRange, offset); + SendableRegistry.addChild(this, m_dutyCycle); + } + + /** + * AnalogPotentiometer constructor. + * + *

+ * Use the fullRange and offset values so that the output produces meaningful + * values. I.E: you + * have a 270 degree potentiometer, and you want the output to be degrees with + * the halfway point + * as 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 + * since the halfway + * point after scaling is 135 degrees. This will calculate the result from the + * fullRange times the + * fraction of the supply voltage, plus the offset. + * + * @param input The {@link DutyCycle} this potentiometer is plugged into. + * @param fullRange The angular value (in desired units) representing the full + * 0-5V range of the + * input. + * @param offset The angular value (in desired units) representing the + * angular output at 0V. + */ + @SuppressWarnings("this-escape") + public DutyCyclePotentiometer(final DigitalSource input, double fullRange, double offset) { + m_dutyCycle = new DutyCycle(requireNonNullParam(input, "input", "DutyCyclePotentiometer")); + m_ownsDutyCycle = true; + init(fullRange, offset); + } + + /** + * AnalogPotentiometer constructor. + * + *

+ * Use the fullRange and offset values so that the output produces meaningful + * values. I.E: you + * have a 270 degree potentiometer, and you want the output to be degrees with + * the halfway point + * as 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 + * since the halfway + * point after scaling is 135 degrees. This will calculate the result from the + * fullRange times the + * fraction of the supply voltage, plus the offset. + * + * @param input The {@link DutyCycle} this potentiometer is plugged into. + * @param fullRange The angular value (in desired units) representing the full + * 0-5V range of the + * input. + * @param offset The angular value (in desired units) representing the + * angular output at 0V. + */ + @SuppressWarnings("this-escape") + public DutyCyclePotentiometer(final DutyCycle input, double fullRange, double offset) { + m_dutyCycle = requireNonNullParam(input, "input", "DutyCyclePotentiometer"); + init(fullRange, offset); + } + + /** + * AnalogPotentiometer constructor. + * + *

+ * Use the scale value so that the output produces meaningful values. I.E: you + * have a 270 + * degree potentiometer, and you want the output to be degrees with the starting + * point as 0 + * degrees. The scale value is 270.0(degrees). + * + * @param channel The analog input channel this potentiometer is plugged into. + * 0-3 are on-board + * and 4-7 are on the MXP port. + * @param scale The scaling to multiply the voltage by to get a meaningful + * unit. + */ + public DutyCyclePotentiometer(final int channel, double scale) { + this(channel, scale, 0); + } + + /** + * AnalogPotentiometer constructor. + * + *

+ * Use the fullRange and offset values so that the output produces meaningful + * values. I.E: you + * have a 270 degree potentiometer, and you want the output to be degrees with + * the starting point + * as 0 degrees. The scale value is 270.0(degrees). + * + * @param input The {@link DutyCycle} this potentiometer is plugged into. + * @param scale The scaling to multiply the voltage by to get a meaningful unit. + */ + public DutyCyclePotentiometer(final DigitalSource input, double scale) { + this(input, scale, 0); + } + + /** + * AnalogPotentiometer constructor. + * + *

+ * Use the fullRange and offset values so that the output produces meaningful + * values. I.E: you + * have a 270 degree potentiometer, and you want the output to be degrees with + * the starting point + * as 0 degrees. The scale value is 270.0(degrees). + * + * @param input The {@link DutyCycle} this potentiometer is plugged into. + * @param scale The scaling to multiply the voltage by to get a meaningful unit. + */ + public DutyCyclePotentiometer(final DutyCycle input, double scale) { + this(input, scale, 0); + } + + /** + * AnalogPotentiometer constructor. + * + *

+ * The potentiometer will return a value between 0 and 1.0. + * + * @param channel The analog input channel this potentiometer is plugged into. + * 0-3 are on-board + * and 4-7 are on the MXP port. + */ + public DutyCyclePotentiometer(final int channel) { + this(channel, 1, 0); + } + + /** + * AnalogPotentiometer constructor. + * + *

+ * The potentiometer will return a value between 0 and 1.0. + * + * @param input The {@link DutyCycle} this potentiometer is plugged into. + */ + public DutyCyclePotentiometer(final DigitalSource input) { + this(input, 1, 0); + } + + /** + * AnalogPotentiometer constructor. + * + *

+ * The potentiometer will return a value between 0 and 1.0. + * + * @param input The {@link DutyCycle} this potentiometer is plugged into. + */ + public DutyCyclePotentiometer(final DutyCycle input) { + this(input, 1, 0); + } + + /** + * Get the current reading of the potentiometer. + * + * @return The current position of the potentiometer. + */ + public double get() { + if (m_simPosition != null) { + return m_simPosition.get(); + } + double pos; + if (m_periodNanos == 0.0) { + pos = m_dutyCycle.getOutput(); + } else { + int highTime = m_dutyCycle.getHighTimeNanoseconds(); + pos = highTime / m_periodNanos; + } + + return pos * m_fullRange + m_offset; + } + + /** + * Get the frequency in Hz of the duty cycle signal from the encoder. + * + * @return duty cycle frequency in Hz + */ + public int getFrequency() { + return m_dutyCycle.getFrequency(); + } + + /** + * Get if the sensor is connected + * + *

+ * This uses the duty cycle frequency to determine if the sensor is connected. + * By default, a + * value of 100 Hz is used as the threshold, and this value can be changed with + * {@link + * #setConnectedFrequencyThreshold(int)}. + * + * @return true if the sensor is connected + */ + public boolean isConnected() { + if (m_simIsConnected != null) { + return m_simIsConnected.get(); + } + return getFrequency() > m_frequencyThreshold; + } + + /** + * Change the frequency threshold for detecting connection used by + * {@link #isConnected()}. + * + * @param frequency the minimum frequency in Hz. + */ + public void setConnectedFrequencyThreshold(int frequency) { + if (frequency < 0) { + frequency = 0; + } + + m_frequencyThreshold = frequency; + } + + /** + * Sets the assumed frequency of the connected device. + * + *

+ * 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 + */ + public void setAssumedFrequency(double frequency) { + if (frequency == 0.0) { + m_periodNanos = 0.0; + } else { + m_periodNanos = 1000000000 / frequency; + } + } + + @Override + public void initSendable(SendableBuilder builder) { + if (m_dutyCycle != null) { + builder.setSmartDashboardType("Analog Input"); + builder.addDoubleProperty("Value", this::get, null); + } + } + + @Override + public void close() { + SendableRegistry.remove(this); + if (m_ownsDutyCycle) { + m_dutyCycle.close(); + m_dutyCycle = null; + m_ownsDutyCycle = false; + } + if (m_digitalSource != null) { + m_digitalSource.close(); + } + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java index 6b116731a5a..13712439431 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj.AnalogEncoder; /** Class to control a simulated analog encoder. */ +@Deprecated() public class AnalogEncoderSim { private final SimDouble m_simPosition; @@ -17,6 +18,7 @@ public class AnalogEncoderSim { * * @param encoder AnalogEncoder to simulate */ + @SuppressWarnings("removal") public AnalogEncoderSim(AnalogEncoder encoder) { SimDeviceSim wrappedSimDevice = new SimDeviceSim("AnalogEncoder" + "[" + encoder.getChannel() + "]"); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java index 3c2c741b62d..2da8e4cd769 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj.DutyCycleEncoder; /** Class to control a simulated duty cycle encoder. */ +@Deprecated() public class DutyCycleEncoderSim { private final SimDouble m_simPosition; private final SimDouble m_simDistancePerRotation; @@ -20,6 +21,7 @@ public class DutyCycleEncoderSim { * * @param encoder DutyCycleEncoder to simulate */ + @SuppressWarnings("removal") public DutyCycleEncoderSim(DutyCycleEncoder encoder) { this(encoder.getSourceChannel()); } From d048b86bf6ef05760ff434f697809c5a25e20ed8 Mon Sep 17 00:00:00 2001 From: Thad House Date: Sun, 25 Feb 2024 23:32:13 -0800 Subject: [PATCH 02/30] Keep with AnalogEncoder and DutyCycleEncoder --- myRobot/src/main/java/frc/robot/MyRobot.java | 6 +- .../edu/wpi/first/wpilibj/AnalogEncoder.java | 160 +++------ .../first/wpilibj/AnalogPotentiometer.java | 174 ++------- .../wpi/first/wpilibj/DutyCycleEncoder.java | 195 +++-------- .../first/wpilibj/DutyCyclePotentiometer.java | 330 ------------------ .../wpilibj/simulation/AnalogEncoderSim.java | 2 - .../simulation/DutyCycleEncoderSim.java | 55 +-- 7 files changed, 128 insertions(+), 794 deletions(-) delete mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCyclePotentiometer.java diff --git a/myRobot/src/main/java/frc/robot/MyRobot.java b/myRobot/src/main/java/frc/robot/MyRobot.java index 5a9a56c493c..da767823e35 100644 --- a/myRobot/src/main/java/frc/robot/MyRobot.java +++ b/myRobot/src/main/java/frc/robot/MyRobot.java @@ -4,19 +4,19 @@ package frc.robot; -import edu.wpi.first.wpilibj.DutyCyclePotentiometer; +import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class MyRobot extends TimedRobot { - DutyCyclePotentiometer pot; + DutyCycleEncoder pot; /** * This function is run when the robot is first started up and should be used for any * initialization code. */ @Override public void robotInit() { - pot = new DutyCyclePotentiometer(0); + pot = new DutyCycleEncoder(0, 1, 0.5); pot.setAssumedFrequency(967.77); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java index 767815bfd32..cae42a8fcd8 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java @@ -11,34 +11,29 @@ 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. - * - * @deprecated Use AnalogPotentiometer instead. If rollover support is necessary, - * use enableRolloverSupport() on AnalogPotentiometer. */ -@Deprecated(since = "2025", forRemoval = true) 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 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 */ - 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; } /** @@ -47,33 +42,34 @@ public AnalogEncoder(int channel) { * @param analogInput the analog input to attach to */ @SuppressWarnings("this-escape") - public AnalogEncoder(AnalogInput analogInput) { + public AnalogEncoder(AnalogInput analogInput, double fullRange, double expectedZero) { m_analogInput = analogInput; - init(); + init(fullRange, expectedZero); } - 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; } /** @@ -88,97 +84,31 @@ public double get() { 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 pos = m_analogInput.getVoltage() / RobotController.getVoltage5V(); - 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 analog encoder. - * - *

getAbsolutePosition() - getPositionOffset() will give an encoder absolute position relative - * to the last reset. This could potentially be negative, which needs to be accounted for. - * - *

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; - return m_analogInput.getVoltage() / RobotController.getVoltage5V(); + // Map from 0 - Full Range + return MathUtil.inputModulus(pos, 0, m_fullRange); } /** - * Get the offset of position relative to the last reset. - * - *

getAbsolutePosition() - getPositionOffset() will give an encoder absolute position relative - * to the last reset. This could potentially be negative, which needs to be accounted for. + * 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. * - * @return the position offset + * @param min minimum voltage percentage (0-1 range) + * @param max maximum voltage percentage (0-1 range) */ - public double getPositionOffset() { - return m_positionOffset; + 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); } - /** - * Set the position offset. - * - *

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(); - } /** * Get the channel number. @@ -189,16 +119,11 @@ public int getChannel() { 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(); } @@ -207,7 +132,6 @@ public void close() { @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); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java index 19e5ec4b87d..15473495528 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java @@ -4,20 +4,13 @@ package edu.wpi.first.wpilibj; -import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; -import edu.wpi.first.hal.SimDevice; -import edu.wpi.first.hal.SimDouble; -import edu.wpi.first.hal.SimDevice.Direction; 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 reading analog potentiometers. Analog potentiometers read in an - * analog voltage that - * corresponds to a position. The position is in whichever units you choose, by - * way of the scaling + * Class for reading analog potentiometers. Analog potentiometers read in an analog voltage that + * corresponds to a position. The position is in whichever units you choose, by way of the scaling * and offset constants passed to the constructor. */ public class AnalogPotentiometer implements Sendable, AutoCloseable { @@ -25,42 +18,20 @@ public class AnalogPotentiometer implements Sendable, AutoCloseable { private boolean m_initAnalogInput; private double m_fullRange; private double m_offset; - private AnalogTrigger m_rotationTrigger; - private Counter m_rotationCounter; - private double m_lastPosition; - - private SimDevice m_simDevice; - private SimDouble m_simPosition; - - private void init() { - m_simDevice = SimDevice.create("AnalogPotentiometer", m_analogInput.getChannel()); - - if (m_simDevice != null) { - m_simPosition = m_simDevice.createDouble("Position", Direction.kInput, 0.0); - } - } /** * AnalogPotentiometer constructor. * - *

- * Use the fullRange and offset values so that the output produces meaningful - * values. I.E: you - * have a 270 degree potentiometer, and you want the output to be degrees with - * the halfway point - * as 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 - * since the halfway - * point after scaling is 135 degrees. This will calculate the result from the - * fullRange times the + *

Use the fullRange and offset values so that the output produces meaningful values. I.E: you + * have a 270 degree potentiometer, and you want the output to be degrees with the halfway point + * as 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 since the halfway + * point after scaling is 135 degrees. This will calculate the result from the fullRange times the * fraction of the supply voltage, plus the offset. * - * @param channel The analog input channel this potentiometer is plugged into. - * 0-3 are on-board - * and 4-7 are on the MXP port. - * @param fullRange The scaling to multiply the fraction by to get a meaningful - * unit. - * @param offset The offset to add to the scaled value for controlling the - * zero value + * @param channel The analog input channel this potentiometer is plugged into. 0-3 are on-board + * and 4-7 are on the MXP port. + * @param fullRange The scaling to multiply the fraction by to get a meaningful unit. + * @param offset The offset to add to the scaled value for controlling the zero value */ @SuppressWarnings("this-escape") public AnalogPotentiometer(final int channel, double fullRange, double offset) { @@ -72,51 +43,37 @@ public AnalogPotentiometer(final int channel, double fullRange, double offset) { /** * AnalogPotentiometer constructor. * - *

- * Use the fullRange and offset values so that the output produces meaningful - * values. I.E: you - * have a 270 degree potentiometer, and you want the output to be degrees with - * the halfway point - * as 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 - * since the halfway - * point after scaling is 135 degrees. This will calculate the result from the - * fullRange times the + *

Use the fullRange and offset values so that the output produces meaningful values. I.E: you + * have a 270 degree potentiometer, and you want the output to be degrees with the halfway point + * as 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 since the halfway + * point after scaling is 135 degrees. This will calculate the result from the fullRange times the * fraction of the supply voltage, plus the offset. * - * @param input The {@link AnalogInput} this potentiometer is plugged into. - * @param fullRange The angular value (in desired units) representing the full - * 0-5V range of the - * input. - * @param offset The angular value (in desired units) representing the - * angular output at 0V. + * @param input The {@link AnalogInput} this potentiometer is plugged into. + * @param fullRange The angular value (in desired units) representing the full 0-5V range of the + * input. + * @param offset The angular value (in desired units) representing the angular output at 0V. */ @SuppressWarnings("this-escape") public AnalogPotentiometer(final AnalogInput input, double fullRange, double offset) { SendableRegistry.addLW(this, "AnalogPotentiometer", input.getChannel()); - m_analogInput = requireNonNullParam(input, "input", "AnalogPotentiometer"); + m_analogInput = input; m_initAnalogInput = false; m_fullRange = fullRange; m_offset = offset; - - init(); } /** * AnalogPotentiometer constructor. * - *

- * Use the scale value so that the output produces meaningful values. I.E: you - * have a 270 - * degree potentiometer, and you want the output to be degrees with the starting - * point as 0 + *

Use the scale value so that the output produces meaningful values. I.E: you have a 270 + * degree potentiometer, and you want the output to be degrees with the starting point as 0 * degrees. The scale value is 270.0(degrees). * - * @param channel The analog input channel this potentiometer is plugged into. - * 0-3 are on-board - * and 4-7 are on the MXP port. - * @param scale The scaling to multiply the voltage by to get a meaningful - * unit. + * @param channel The analog input channel this potentiometer is plugged into. 0-3 are on-board + * and 4-7 are on the MXP port. + * @param scale The scaling to multiply the voltage by to get a meaningful unit. */ public AnalogPotentiometer(final int channel, double scale) { this(channel, scale, 0); @@ -125,11 +82,8 @@ public AnalogPotentiometer(final int channel, double scale) { /** * AnalogPotentiometer constructor. * - *

- * Use the fullRange and offset values so that the output produces meaningful - * values. I.E: you - * have a 270 degree potentiometer, and you want the output to be degrees with - * the starting point + *

Use the fullRange and offset values so that the output produces meaningful values. I.E: you + * have a 270 degree potentiometer, and you want the output to be degrees with the starting point * as 0 degrees. The scale value is 270.0(degrees). * * @param input The {@link AnalogInput} this potentiometer is plugged into. @@ -142,12 +96,10 @@ public AnalogPotentiometer(final AnalogInput input, double scale) { /** * AnalogPotentiometer constructor. * - *

- * The potentiometer will return a value between 0 and 1.0. + *

The potentiometer will return a value between 0 and 1.0. * - * @param channel The analog input channel this potentiometer is plugged into. - * 0-3 are on-board - * and 4-7 are on the MXP port. + * @param channel The analog input channel this potentiometer is plugged into. 0-3 are on-board + * and 4-7 are on the MXP port. */ public AnalogPotentiometer(final int channel) { this(channel, 1, 0); @@ -156,8 +108,7 @@ public AnalogPotentiometer(final int channel) { /** * AnalogPotentiometer constructor. * - *

- * The potentiometer will return a value between 0 and 1.0. + *

The potentiometer will return a value between 0 and 1.0. * * @param input The {@link AnalogInput} this potentiometer is plugged into. */ @@ -165,73 +116,19 @@ public AnalogPotentiometer(final AnalogInput input) { this(input, 1, 0); } - private boolean doubleEquals(double a, double b) { - double epsilon = 0.00001d; - return Math.abs(a - b) < epsilon; - } - - private double getRollovers() { - 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_rotationCounter.get(); - double pos = m_analogInput.getVoltage(); - double counter2 = m_rotationCounter.get(); - double pos2 = m_analogInput.getVoltage(); - if (counter == counter2 && doubleEquals(pos, pos2)) { - pos = pos / RobotController.getVoltage5V(); - double position = (counter + pos) * m_fullRange + m_offset; - m_lastPosition = position; - return position; - } - } - - DriverStation.reportWarning( - "Failed to read Analog Encoder. Potential Speed Overrun. Returning last value", false); - return m_lastPosition; - } - /** * Get the current reading of the potentiometer. * * @return The current position of the potentiometer. */ public double get() { - if (m_simPosition != null) { - return m_simPosition.get(); - } - - if (m_rotationCounter != null) { - return getRollovers(); + if (m_analogInput == null) { + return m_offset; } - return (m_analogInput.getAverageVoltage() / RobotController.getVoltage5V()) * m_fullRange + m_offset; } - public void enableRolloverSupport() { - if (m_rotationCounter != null) { - return; - } - m_rotationTrigger = new AnalogTrigger(m_analogInput); - m_rotationCounter = new Counter(); - - // Limits need to be 25% from each end - m_rotationTrigger.setLimitsVoltage(1.25, 3.75); - m_rotationCounter.setUpSource(m_rotationTrigger, AnalogTriggerType.kRisingPulse); - m_rotationCounter.setDownSource(m_rotationTrigger, AnalogTriggerType.kFallingPulse); - } - - public void resetRollovers() { - if (m_rotationCounter != null) { - m_rotationCounter.reset(); - } - } - @Override public void initSendable(SendableBuilder builder) { if (m_analogInput != null) { @@ -243,17 +140,10 @@ public void initSendable(SendableBuilder builder) { @Override public void close() { SendableRegistry.remove(this); - if (m_rotationCounter != null) { - m_rotationCounter.close(); - m_rotationTrigger.close(); - m_rotationCounter = null; - m_rotationTrigger = null; - } if (m_initAnalogInput) { m_analogInput.close(); m_analogInput = null; m_initAnalogInput = false; } - } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java index c9c7383cce3..c3b85b4c296 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java @@ -11,33 +11,24 @@ 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 * CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag Encoder. - * - * @deprecated Use DutyCyclePotentiometer instead. Rollover support is not supported for - * duty cycle encoders, there is no reliable way to do so. */ -@Deprecated(since = "2025", forRemoval = true) 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 SimDevice m_simDevice; private SimDouble m_simPosition; - private SimDouble m_simAbsolutePosition; - private SimDouble m_simDistancePerRotation; private SimBoolean m_simIsConnected; /** @@ -46,11 +37,11 @@ public class DutyCycleEncoder implements Sendable, AutoCloseable { * @param channel the channel to attach to */ @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); } /** @@ -59,9 +50,9 @@ public DutyCycleEncoder(int channel) { * @param dutyCycle the duty cycle to attach to */ @SuppressWarnings("this-escape") - public DutyCycleEncoder(DutyCycle dutyCycle) { + public DutyCycleEncoder(DutyCycle dutyCycle, double fullRange, double expectedZero) { m_dutyCycle = dutyCycle; - init(); + init(fullRange, expectedZero); } /** @@ -70,30 +61,23 @@ public DutyCycleEncoder(DutyCycle dutyCycle) { * @param source the digital source to attach to */ @SuppressWarnings("this-escape") - public DutyCycleEncoder(DigitalSource source) { + public DutyCycleEncoder(DigitalSource source, double fullRange, double expectedZero) { m_ownsDutyCycle = true; m_dutyCycle = new DutyCycle(source); - init(); + init(fullRange, expectedZero); } - 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()); } @@ -109,11 +93,6 @@ private double mapSensorRange(double pos) { 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. * @@ -126,67 +105,23 @@ public double get() { 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; - } - } - - DriverStation.reportWarning( - "Failed to read Analog Encoder. Potential Speed Overrun. Returning last value", false); - return m_lastPosition; - } - - /** - * Get the absolute position of the duty cycle encoder. - * - *

getAbsolutePosition() - getPositionOffset() will give an encoder absolute position relative - * to the last reset. This could potentially be negative, which needs to be accounted for. - * - *

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(); + 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; } - return mapSensorRange(m_dutyCycle.getOutput()); - } + // Map sensor range if range isn't full + pos = mapSensorRange(pos); - /** - * Get the offset of position relative to the last reset. - * - *

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; - } + // Compute full range and offset + pos = pos * m_fullRange - m_expectedZero; - /** - * Set the position offset. - * - *

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); + // Map from 0 - Full Range + return MathUtil.inputModulus(pos, 0, m_fullRange); } /** @@ -205,40 +140,6 @@ public void setDutyCycleRange(double min, double max) { 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. * @@ -248,17 +149,6 @@ public int getFrequency() { 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 * @@ -288,14 +178,28 @@ public void setConnectedFrequencyThreshold(int frequency) { m_frequencyThreshold = frequency; } + /** + * Sets the assumed frequency of the connected device. + * + *

+ * 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 + */ + public void setAssumedFrequency(double frequency) { + if (frequency == 0.0) { + m_periodNanos = 0.0; + } else { + m_periodNanos = 1000000000 / frequency; + } + } + @Override public void close() { - if (m_counter != null) { - m_counter.close(); - } - if (m_analogTrigger != null) { - m_analogTrigger.close(); - } if (m_ownsDutyCycle) { m_dutyCycle.close(); } @@ -328,8 +232,7 @@ public int getSourceChannel() { @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); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCyclePotentiometer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCyclePotentiometer.java deleted file mode 100644 index 81268d12e51..00000000000 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCyclePotentiometer.java +++ /dev/null @@ -1,330 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package edu.wpi.first.wpilibj; - -import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; - -import edu.wpi.first.hal.SimBoolean; -import edu.wpi.first.hal.SimDevice; -import edu.wpi.first.hal.SimDevice.Direction; -import edu.wpi.first.hal.SimDouble; -import edu.wpi.first.util.sendable.Sendable; -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.util.sendable.SendableRegistry; - -/** - * Class for reading duty cyle potentiometers. Duty cycle potentiometers read in - * an pwm signal that - * corresponds to a position. The position is in whichever units you choose, by - * way of the scaling - * and offset constants passed to the constructor. - */ -public class DutyCyclePotentiometer implements Sendable, AutoCloseable { - private DutyCycle m_dutyCycle; - private DigitalSource m_digitalSource; - private boolean m_ownsDutyCycle; - private double m_fullRange; - private double m_offset; - private int m_frequencyThreshold = 100; - private double m_periodNanos; - - private SimDevice m_simDevice; - private SimDouble m_simPosition; - private SimBoolean m_simIsConnected; - - private final void init(double fullRange, double offset) { - SendableRegistry.addLW(this, "DutyCyclePotentiometer", m_dutyCycle.getFPGAIndex()); - m_fullRange = fullRange; - m_offset = offset; - - m_simDevice = SimDevice.create("DutyCyclePotentiometer", m_dutyCycle.getFPGAIndex()); - - if (m_simDevice != null) { - m_simPosition = m_simDevice.createDouble("Position", Direction.kInput, 0.0); - m_simIsConnected = m_simDevice.createBoolean("Connected", SimDevice.Direction.kInput, true); - } - } - - /** - * AnalogPotentiometer constructor. - * - *

- * Use the fullRange and offset values so that the output produces meaningful - * values. I.E: you - * have a 270 degree potentiometer, and you want the output to be degrees with - * the halfway point - * as 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 - * since the halfway - * point after scaling is 135 degrees. This will calculate the result from the - * fullRange times the - * fraction of the supply voltage, plus the offset. - * - * @param channel The analog input channel this potentiometer is plugged into. - * 0-3 are on-board - * and 4-7 are on the MXP port. - * @param fullRange The scaling to multiply the fraction by to get a meaningful - * unit. - * @param offset The offset to add to the scaled value for controlling the - * zero value - */ - @SuppressWarnings("this-escape") - public DutyCyclePotentiometer(final int channel, double fullRange, double offset) { - m_digitalSource = new DigitalInput(channel); - m_dutyCycle = new DutyCycle(m_digitalSource); - m_ownsDutyCycle = true; - init(fullRange, offset); - SendableRegistry.addChild(this, m_dutyCycle); - } - - /** - * AnalogPotentiometer constructor. - * - *

- * Use the fullRange and offset values so that the output produces meaningful - * values. I.E: you - * have a 270 degree potentiometer, and you want the output to be degrees with - * the halfway point - * as 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 - * since the halfway - * point after scaling is 135 degrees. This will calculate the result from the - * fullRange times the - * fraction of the supply voltage, plus the offset. - * - * @param input The {@link DutyCycle} this potentiometer is plugged into. - * @param fullRange The angular value (in desired units) representing the full - * 0-5V range of the - * input. - * @param offset The angular value (in desired units) representing the - * angular output at 0V. - */ - @SuppressWarnings("this-escape") - public DutyCyclePotentiometer(final DigitalSource input, double fullRange, double offset) { - m_dutyCycle = new DutyCycle(requireNonNullParam(input, "input", "DutyCyclePotentiometer")); - m_ownsDutyCycle = true; - init(fullRange, offset); - } - - /** - * AnalogPotentiometer constructor. - * - *

- * Use the fullRange and offset values so that the output produces meaningful - * values. I.E: you - * have a 270 degree potentiometer, and you want the output to be degrees with - * the halfway point - * as 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 - * since the halfway - * point after scaling is 135 degrees. This will calculate the result from the - * fullRange times the - * fraction of the supply voltage, plus the offset. - * - * @param input The {@link DutyCycle} this potentiometer is plugged into. - * @param fullRange The angular value (in desired units) representing the full - * 0-5V range of the - * input. - * @param offset The angular value (in desired units) representing the - * angular output at 0V. - */ - @SuppressWarnings("this-escape") - public DutyCyclePotentiometer(final DutyCycle input, double fullRange, double offset) { - m_dutyCycle = requireNonNullParam(input, "input", "DutyCyclePotentiometer"); - init(fullRange, offset); - } - - /** - * AnalogPotentiometer constructor. - * - *

- * Use the scale value so that the output produces meaningful values. I.E: you - * have a 270 - * degree potentiometer, and you want the output to be degrees with the starting - * point as 0 - * degrees. The scale value is 270.0(degrees). - * - * @param channel The analog input channel this potentiometer is plugged into. - * 0-3 are on-board - * and 4-7 are on the MXP port. - * @param scale The scaling to multiply the voltage by to get a meaningful - * unit. - */ - public DutyCyclePotentiometer(final int channel, double scale) { - this(channel, scale, 0); - } - - /** - * AnalogPotentiometer constructor. - * - *

- * Use the fullRange and offset values so that the output produces meaningful - * values. I.E: you - * have a 270 degree potentiometer, and you want the output to be degrees with - * the starting point - * as 0 degrees. The scale value is 270.0(degrees). - * - * @param input The {@link DutyCycle} this potentiometer is plugged into. - * @param scale The scaling to multiply the voltage by to get a meaningful unit. - */ - public DutyCyclePotentiometer(final DigitalSource input, double scale) { - this(input, scale, 0); - } - - /** - * AnalogPotentiometer constructor. - * - *

- * Use the fullRange and offset values so that the output produces meaningful - * values. I.E: you - * have a 270 degree potentiometer, and you want the output to be degrees with - * the starting point - * as 0 degrees. The scale value is 270.0(degrees). - * - * @param input The {@link DutyCycle} this potentiometer is plugged into. - * @param scale The scaling to multiply the voltage by to get a meaningful unit. - */ - public DutyCyclePotentiometer(final DutyCycle input, double scale) { - this(input, scale, 0); - } - - /** - * AnalogPotentiometer constructor. - * - *

- * The potentiometer will return a value between 0 and 1.0. - * - * @param channel The analog input channel this potentiometer is plugged into. - * 0-3 are on-board - * and 4-7 are on the MXP port. - */ - public DutyCyclePotentiometer(final int channel) { - this(channel, 1, 0); - } - - /** - * AnalogPotentiometer constructor. - * - *

- * The potentiometer will return a value between 0 and 1.0. - * - * @param input The {@link DutyCycle} this potentiometer is plugged into. - */ - public DutyCyclePotentiometer(final DigitalSource input) { - this(input, 1, 0); - } - - /** - * AnalogPotentiometer constructor. - * - *

- * The potentiometer will return a value between 0 and 1.0. - * - * @param input The {@link DutyCycle} this potentiometer is plugged into. - */ - public DutyCyclePotentiometer(final DutyCycle input) { - this(input, 1, 0); - } - - /** - * Get the current reading of the potentiometer. - * - * @return The current position of the potentiometer. - */ - public double get() { - if (m_simPosition != null) { - return m_simPosition.get(); - } - double pos; - if (m_periodNanos == 0.0) { - pos = m_dutyCycle.getOutput(); - } else { - int highTime = m_dutyCycle.getHighTimeNanoseconds(); - pos = highTime / m_periodNanos; - } - - return pos * m_fullRange + m_offset; - } - - /** - * Get the frequency in Hz of the duty cycle signal from the encoder. - * - * @return duty cycle frequency in Hz - */ - public int getFrequency() { - return m_dutyCycle.getFrequency(); - } - - /** - * Get if the sensor is connected - * - *

- * This uses the duty cycle frequency to determine if the sensor is connected. - * By default, a - * value of 100 Hz is used as the threshold, and this value can be changed with - * {@link - * #setConnectedFrequencyThreshold(int)}. - * - * @return true if the sensor is connected - */ - public boolean isConnected() { - if (m_simIsConnected != null) { - return m_simIsConnected.get(); - } - return getFrequency() > m_frequencyThreshold; - } - - /** - * Change the frequency threshold for detecting connection used by - * {@link #isConnected()}. - * - * @param frequency the minimum frequency in Hz. - */ - public void setConnectedFrequencyThreshold(int frequency) { - if (frequency < 0) { - frequency = 0; - } - - m_frequencyThreshold = frequency; - } - - /** - * Sets the assumed frequency of the connected device. - * - *

- * 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 - */ - public void setAssumedFrequency(double frequency) { - if (frequency == 0.0) { - m_periodNanos = 0.0; - } else { - m_periodNanos = 1000000000 / frequency; - } - } - - @Override - public void initSendable(SendableBuilder builder) { - if (m_dutyCycle != null) { - builder.setSmartDashboardType("Analog Input"); - builder.addDoubleProperty("Value", this::get, null); - } - } - - @Override - public void close() { - SendableRegistry.remove(this); - if (m_ownsDutyCycle) { - m_dutyCycle.close(); - m_dutyCycle = null; - m_ownsDutyCycle = false; - } - if (m_digitalSource != null) { - m_digitalSource.close(); - } - } -} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java index 13712439431..6b116731a5a 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java @@ -9,7 +9,6 @@ import edu.wpi.first.wpilibj.AnalogEncoder; /** Class to control a simulated analog encoder. */ -@Deprecated() public class AnalogEncoderSim { private final SimDouble m_simPosition; @@ -18,7 +17,6 @@ public class AnalogEncoderSim { * * @param encoder AnalogEncoder to simulate */ - @SuppressWarnings("removal") public AnalogEncoderSim(AnalogEncoder encoder) { SimDeviceSim wrappedSimDevice = new SimDeviceSim("AnalogEncoder" + "[" + encoder.getChannel() + "]"); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java index 2da8e4cd769..786c16eea17 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java @@ -9,11 +9,8 @@ import edu.wpi.first.wpilibj.DutyCycleEncoder; /** Class to control a simulated duty cycle encoder. */ -@Deprecated() public class DutyCycleEncoderSim { private final SimDouble m_simPosition; - private final SimDouble m_simDistancePerRotation; - private final SimDouble m_simAbsolutePosition; private final SimBoolean m_simIsConnected; /** @@ -21,7 +18,6 @@ public class DutyCycleEncoderSim { * * @param encoder DutyCycleEncoder to simulate */ - @SuppressWarnings("removal") public DutyCycleEncoderSim(DutyCycleEncoder encoder) { this(encoder.getSourceChannel()); } @@ -33,10 +29,8 @@ public DutyCycleEncoderSim(DutyCycleEncoder encoder) { */ 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"); } /** @@ -57,51 +51,6 @@ 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(); - } - /** * Get if the encoder is connected. * From 797eb84e7496716a7df8f6a101cfc886d37d2604 Mon Sep 17 00:00:00 2001 From: Thad House Date: Sun, 25 Feb 2024 23:59:48 -0800 Subject: [PATCH 03/30] Add inversion --- myRobot/src/main/java/frc/robot/MyRobot.java | 9 +++++++-- .../java/edu/wpi/first/wpilibj/AnalogEncoder.java | 12 +++++++++++- .../java/edu/wpi/first/wpilibj/DutyCycleEncoder.java | 12 +++++++++++- .../src/main/java/edu/wpi/first/math/MathUtil.java | 4 ++++ 4 files changed, 33 insertions(+), 4 deletions(-) diff --git a/myRobot/src/main/java/frc/robot/MyRobot.java b/myRobot/src/main/java/frc/robot/MyRobot.java index da767823e35..d63e534882b 100644 --- a/myRobot/src/main/java/frc/robot/MyRobot.java +++ b/myRobot/src/main/java/frc/robot/MyRobot.java @@ -4,6 +4,7 @@ package frc.robot; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -16,7 +17,7 @@ public class MyRobot extends TimedRobot { */ @Override public void robotInit() { - pot = new DutyCycleEncoder(0, 1, 0.5); + pot = new DutyCycleEncoder(0, 3, 0.5); pot.setAssumedFrequency(967.77); } @@ -43,7 +44,11 @@ public void testPeriodic() {} /** This function is called periodically during all modes. */ @Override public void robotPeriodic() { - SmartDashboard.putNumber("DC", pot.get()); + double val = pot.get() + 1; + SmartDashboard.putNumber("DC4", val); + SmartDashboard.putNumber("DC3", MathUtil.invertInput(val, 1, 4)); + SmartDashboard.putNumber("DC2", MathUtil.inputModulus(pot.get(), -0.1, 0.9)); + SmartDashboard.putNumber("DC", MathUtil.inputModulus(MathUtil.invertInput(pot.get(), 0, 1), -0.1, 0.9)); SmartDashboard.putNumber("Freq", pot.getFrequency()); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java index cae42a8fcd8..021e8cc06d4 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java @@ -22,6 +22,7 @@ public class AnalogEncoder implements Sendable, AutoCloseable { 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; @@ -93,7 +94,12 @@ public double get() { pos = pos * m_fullRange - m_expectedZero; // Map from 0 - Full Range - return MathUtil.inputModulus(pos, 0, m_fullRange); + double result = MathUtil.inputModulus(pos, 0, m_fullRange); + // Invert if necessary + if (m_isInverted) { + return m_fullRange - result; + } + return result; } /** @@ -109,6 +115,10 @@ public void setVoltagePercentageRange(double min, double max) { m_sensorMax = MathUtil.clamp(max, 0.0, 1.0); } + public void SetInverted(boolean inverted) { + m_isInverted = inverted; + } + /** * Get the channel number. diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java index c3b85b4c296..9d2b025f856 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java @@ -26,6 +26,7 @@ public class DutyCycleEncoder implements Sendable, AutoCloseable { 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; @@ -121,7 +122,12 @@ public double get() { pos = pos * m_fullRange - m_expectedZero; // Map from 0 - Full Range - return MathUtil.inputModulus(pos, 0, m_fullRange); + double result = MathUtil.inputModulus(pos, 0, m_fullRange); + // Invert if necessary + if (m_isInverted) { + return m_fullRange - result; + } + return result; } /** @@ -198,6 +204,10 @@ public void setAssumedFrequency(double frequency) { } } + public void SetInverted(boolean inverted) { + m_isInverted = inverted; + } + @Override public void close() { if (m_ownsDutyCycle) { diff --git a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java index 044cddfab68..64fbf978be5 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java +++ b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java @@ -125,6 +125,10 @@ public static double inputModulus(double input, double minimumInput, double maxi return input; } + public static double invertInput(double input, double minimumInput, double maximumInput) { + return maximumInput - input + minimumInput; + } + /** * Wraps an angle to the range -pi to pi radians. * From 5a4d3fd199913d1a76cb5c9f50c6b52f7f0a78d1 Mon Sep 17 00:00:00 2001 From: Thad House Date: Wed, 28 Feb 2024 21:28:29 -0800 Subject: [PATCH 04/30] Fix up example --- .../edu/wpi/first/wpilibj/AnalogEncoder.java | 25 +++- .../wpi/first/wpilibj/DutyCycleEncoder.java | 109 ++++++++++++------ .../examples/dutycycleencoder/Robot.java | 46 +++++++- 3 files changed, 137 insertions(+), 43 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java index 021e8cc06d4..617c50a6b42 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java @@ -48,6 +48,25 @@ public AnalogEncoder(AnalogInput analogInput, double fullRange, double expectedZ init(fullRange, expectedZero); } + /** + * Construct a new AnalogEncoder attached to a specific AnalogIn channel. + * + * @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. + * + * @param analogInput the analog input to attach to + */ + @SuppressWarnings("this-escape") + public AnalogEncoder(AnalogInput analogInput) { + this(analogInput, 1.0, 0.0); + } + private void init(double fullRange, double expectedZero) { m_simDevice = SimDevice.create("AnalogEncoder", m_analogInput.getChannel()); @@ -76,7 +95,8 @@ private double mapSensorRange(double pos) { /** * Get the encoder value since the last reset. * - *

This is reported in rotations since the last reset. + *

+ * This is reported in rotations since the last reset. * * @return the encoder value in rotations */ @@ -115,11 +135,10 @@ public void setVoltagePercentageRange(double min, double max) { m_sensorMax = MathUtil.clamp(max, 0.0, 1.0); } - public void SetInverted(boolean inverted) { + public void setInverted(boolean inverted) { m_isInverted = inverted; } - /** * Get the channel number. * diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java index 9d2b025f856..5e321b6dc48 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java @@ -13,7 +13,8 @@ import edu.wpi.first.util.sendable.SendableRegistry; /** - * Class for supporting duty cycle/PWM encoders, such as the US Digital MA3 with PWM Output, the + * Class for supporting duty cycle/PWM encoders, such as the US Digital MA3 with + * PWM Output, the * CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag Encoder. */ public class DutyCycleEncoder implements Sendable, AutoCloseable { @@ -68,6 +69,36 @@ public DutyCycleEncoder(DigitalSource source, double fullRange, double expectedZ init(fullRange, expectedZero); } + /** + * Construct a new DutyCycleEncoder on a specific channel. + * + * @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. + * + * @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. + * + * @param source the digital source to attach to + */ + @SuppressWarnings("this-escape") + public DutyCycleEncoder(DigitalSource source) { + this(source, 1.0, 0.0); + } + private void init(double fullRange, double expectedZero) { m_simDevice = SimDevice.create("DutyCycle:DutyCycleEncoder", m_dutyCycle.getSourceChannel()); @@ -97,7 +128,8 @@ private double mapSensorRange(double pos) { /** * Get the encoder value since the last reset. * - *

This is reported in rotations since the last reset. + *

+ * This is reported in rotations since the last reset. * * @return the encoder value in rotations */ @@ -109,10 +141,10 @@ public double get() { double pos; // Compute output percentage (0-1) if (m_periodNanos == 0.0) { - pos = m_dutyCycle.getOutput(); + pos = m_dutyCycle.getOutput(); } else { - int highTime = m_dutyCycle.getHighTimeNanoseconds(); - pos = highTime / m_periodNanos; + int highTime = m_dutyCycle.getHighTimeNanoseconds(); + pos = highTime / m_periodNanos; } // Map sensor range if range isn't full @@ -131,11 +163,16 @@ public double get() { } /** - * Set the encoder duty cycle range. As the encoder needs to maintain a duty cycle, the duty cycle - * cannot go all the way to 0% or all the way to 100%. For example, an encoder with a 4096 us - * period might have a minimum duty cycle of 1 us / 4096 us and a maximum duty cycle of 4095 / - * 4096 us. Setting the range will result in an encoder duty cycle less than or equal to the - * minimum being output as 0 rotation, the duty cycle greater than or equal to the maximum being + * Set the encoder duty cycle range. As the encoder needs to maintain a duty + * cycle, the duty cycle + * cannot go all the way to 0% or all the way to 100%. For example, an encoder + * with a 4096 us + * period might have a minimum duty cycle of 1 us / 4096 us and a maximum duty + * cycle of 4095 / + * 4096 us. Setting the range will result in an encoder duty cycle less than or + * equal to the + * minimum being output as 0 rotation, the duty cycle greater than or equal to + * the maximum being * output as 1 rotation, and values in between linearly scaled from 0 to 1. * * @param min minimum duty cycle (0-1 range) @@ -158,8 +195,11 @@ public int getFrequency() { /** * Get if the sensor is connected * - *

This uses the duty cycle frequency to determine if the sensor is connected. By default, a - * value of 100 Hz is used as the threshold, and this value can be changed with {@link + *

+ * This uses the duty cycle frequency to determine if the sensor is connected. + * By default, a + * value of 100 Hz is used as the threshold, and this value can be changed with + * {@link * #setConnectedFrequencyThreshold(int)}. * * @return true if the sensor is connected @@ -172,7 +212,8 @@ public boolean isConnected() { } /** - * Change the frequency threshold for detecting connection used by {@link #isConnected()}. + * Change the frequency threshold for detecting connection used by + * {@link #isConnected()}. * * @param frequency the minimum frequency in Hz. */ @@ -184,27 +225,27 @@ public void setConnectedFrequencyThreshold(int frequency) { m_frequencyThreshold = frequency; } - /** - * Sets the assumed frequency of the connected device. - * - *

- * 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 - */ - public void setAssumedFrequency(double frequency) { - if (frequency == 0.0) { - m_periodNanos = 0.0; - } else { - m_periodNanos = 1000000000 / frequency; - } - } - - public void SetInverted(boolean inverted) { + /** + * Sets the assumed frequency of the connected device. + * + *

+ * 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 + */ + public void setAssumedFrequency(double frequency) { + if (frequency == 0.0) { + m_periodNanos = 0.0; + } else { + m_periodNanos = 1000000000 / frequency; + } + } + + public void setInverted(boolean inverted) { m_isInverted = inverted; } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java index bb99244248a..70b3c4cf5cf 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java @@ -4,19 +4,43 @@ package edu.wpi.first.wpilibj.examples.dutycycleencoder; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +/** + * This example shows how to use a duty cycle encoder for devices such as an + * arm or elevator. + */ public class Robot extends TimedRobot { private DutyCycleEncoder m_dutyCycleEncoder; + private final double m_fullRange = 1.3; + private final double m_expectedZero = 0; @Override public void robotInit() { - m_dutyCycleEncoder = new DutyCycleEncoder(0); + // 2nd parameter is the range of values. This sensor will output between + // 0 and the passed in value. + // 3rd parameter is the the physical value where you want "0" to be. How + // to measure this is fairly easy. Set the value to 0, place the mechanism + // where you want "0" to be, and observe the value on the dashboard, That + // is the value to enter for the 3rd parameter. + m_dutyCycleEncoder = new DutyCycleEncoder(0, m_fullRange, m_expectedZero); - // Set to 0.5 units per rotation - m_dutyCycleEncoder.setDistancePerRotation(0.5); + + // If you know the frequency of your sensor, uncomment the following + // method, and set the method to the frequency of your sensor. + // This will result in more stable readings from the sensor. + // Do note that occasionally the datasheet cannot be trusted + // and you should measure this value. You can do so with either + // an oscilloscope, or by observing the "Frequency" output + // on the dashboard while running this sample. If you find + // the value jumping between the 2 values, enter halfway between + // those values. This number doesn't have to be perfect, + // just having a fairly close value will make the output readings + // much more stable. + m_dutyCycleEncoder.setAssumedFrequency(967.8); } @Override @@ -30,12 +54,22 @@ public void robotPeriodic() { // Output of encoder double output = m_dutyCycleEncoder.get(); - // Output scaled by DistancePerPulse - double distance = m_dutyCycleEncoder.getDistance(); + // By default, the output will wrap around to the full range value + // when the sensor goes below 0. However, for moving mechanisms this + // is not usually ideal, as if 0 is set to a hard stop, its still + // possible for the sensor to move slightly past. If this happens + // The sensor will assume its now at the furthest away position, + // which control algorithms might not handle correctly. Therefore + // it can be a good idea to slightly shift the output so the sensor + // can go a bit negative before wrapping. Usually 10% or so is fine. + // This does not change where "0" is, so no calibration numbers need + // to be changed. + double percentOfRange = m_fullRange * 0.1; + double shiftedOutput = MathUtil.inputModulus(output, 0 - percentOfRange, m_fullRange - percentOfRange); SmartDashboard.putBoolean("Connected", connected); SmartDashboard.putNumber("Frequency", frequency); SmartDashboard.putNumber("Output", output); - SmartDashboard.putNumber("Distance", distance); + SmartDashboard.putNumber("ShiftedOutput", shiftedOutput); } } From dd2332dbafb4806fb1309abfc85db14cea88ac85 Mon Sep 17 00:00:00 2001 From: Thad House Date: Wed, 28 Feb 2024 22:09:58 -0800 Subject: [PATCH 05/30] Fixup C++ --- wpilibc/src/main/native/cpp/AnalogEncoder.cpp | 144 ++++++-------- .../src/main/native/cpp/DutyCycleEncoder.cpp | 185 +++++++++--------- .../cpp/simulation/AnalogEncoderSim.cpp | 16 +- .../cpp/simulation/DutyCycleEncoderSim.cpp | 30 +-- .../main/native/include/frc/AnalogEncoder.h | 100 +++------- .../native/include/frc/DutyCycleEncoder.h | 147 ++++++-------- .../include/frc/simulation/AnalogEncoderSim.h | 20 +- .../frc/simulation/DutyCycleEncoderSim.h | 45 +---- .../cpp/simulation/AnalogEncoderSimTest.cpp | 10 +- .../simulation/DutyCycleEncoderSimTest.cpp | 43 +--- .../examples/DutyCycleEncoder/cpp/Robot.cpp | 46 ++++- .../wpilibj/simulation/AnalogEncoderSim.java | 29 +-- .../simulation/DutyCycleEncoderSim.java | 6 +- .../simulation/AnalogEncoderSimTest.java | 10 +- .../simulation/DutyCycleEncoderSimTest.java | 52 +---- 15 files changed, 321 insertions(+), 562 deletions(-) diff --git a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp index 0c29bea5e57..c4381f0ef31 100644 --- a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp +++ b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp @@ -8,9 +8,9 @@ #include #include "frc/AnalogInput.h" -#include "frc/Counter.h" #include "frc/Errors.h" #include "frc/RobotController.h" +#include "frc/MathUtil.h" using namespace frc; @@ -18,120 +18,104 @@ AnalogEncoder::AnalogEncoder(int channel) : AnalogEncoder(std::make_shared(channel)) {} AnalogEncoder::AnalogEncoder(AnalogInput& analogInput) - : m_analogInput{&analogInput, wpi::NullDeleter{}}, - m_analogTrigger{m_analogInput.get()}, - m_counter{} { - Init(); + : m_analogInput{&analogInput, wpi::NullDeleter{}} { + Init(1.0, 0.0); } AnalogEncoder::AnalogEncoder(AnalogInput* analogInput) - : m_analogInput{analogInput, wpi::NullDeleter{}}, - m_analogTrigger{m_analogInput.get()}, - m_counter{} { - Init(); + : m_analogInput{analogInput, wpi::NullDeleter{}} { + Init(1.0, 0.0); } AnalogEncoder::AnalogEncoder(std::shared_ptr analogInput) - : m_analogInput{std::move(analogInput)}, - m_analogTrigger{m_analogInput.get()}, - m_counter{} { - Init(); + : m_analogInput{std::move(analogInput)} { + Init(1.0, 0.0); } -void AnalogEncoder::Init() { - m_simDevice = hal::SimDevice{"AnalogEncoder", m_analogInput->GetChannel()}; +AnalogEncoder::AnalogEncoder(int channel, double fullRange, double expectedZero) + : AnalogEncoder(std::make_shared(channel), fullRange, + expectedZero) {} - if (m_simDevice) { - m_simPosition = m_simDevice.CreateDouble("Position", false, 0.0); - m_simAbsolutePosition = - m_simDevice.CreateDouble("absPosition", hal::SimDevice::kInput, 0.0); - } - - m_analogTrigger.SetLimitsVoltage(1.25, 3.75); - m_counter.SetUpSource( - m_analogTrigger.CreateOutput(AnalogTriggerType::kRisingPulse)); - m_counter.SetDownSource( - m_analogTrigger.CreateOutput(AnalogTriggerType::kFallingPulse)); +AnalogEncoder::AnalogEncoder(AnalogInput& analogInput, double fullRange, + double expectedZero) + : m_analogInput{&analogInput, wpi::NullDeleter{}} { + Init(fullRange, expectedZero); +} - wpi::SendableRegistry::AddLW(this, "DutyCycle Encoder", - m_analogInput->GetChannel()); +AnalogEncoder::AnalogEncoder(AnalogInput* analogInput, double fullRange, + double expectedZero) + : m_analogInput{analogInput, wpi::NullDeleter{}} { + Init(fullRange, expectedZero); } -static bool DoubleEquals(double a, double b) { - constexpr double epsilon = 0.00001; - return std::abs(a - b) < epsilon; +AnalogEncoder::AnalogEncoder(std::shared_ptr analogInput, + double fullRange, double expectedZero) + : m_analogInput{std::move(analogInput)} { + Init(fullRange, expectedZero); } -units::turn_t AnalogEncoder::Get() const { - if (m_simPosition) { - return units::turn_t{m_simPosition.Get()}; - } +void AnalogEncoder::Init(double fullRange, double expectedZero) { + m_simDevice = hal::SimDevice{"AnalogEncoder", m_analogInput->GetChannel()}; - // 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++) { - auto counter = m_counter.Get(); - auto pos = m_analogInput->GetVoltage(); - auto counter2 = m_counter.Get(); - auto pos2 = m_analogInput->GetVoltage(); - if (counter == counter2 && DoubleEquals(pos, pos2)) { - pos = pos / frc::RobotController::GetVoltage5V(); - units::turn_t turns{counter + pos - m_positionOffset}; - m_lastPosition = turns; - return turns; - } + if (m_simDevice) { + m_simPosition = m_simDevice.CreateDouble("Position", false, 0.0); } - FRC_ReportError( - warn::Warning, - "Failed to read Analog Encoder. Potential Speed Overrun. Returning last " - "value"); - return m_lastPosition; + m_fullRange = fullRange; + m_expectedZero = expectedZero; + + wpi::SendableRegistry::AddLW(this, "Analog Encoder", + m_analogInput->GetChannel()); } -double AnalogEncoder::GetAbsolutePosition() const { - if (m_simAbsolutePosition) { - return m_simAbsolutePosition.Get(); +double AnalogEncoder::Get() const { + if (m_simPosition) { + return m_simPosition.Get(); } - return m_analogInput->GetVoltage() / frc::RobotController::GetVoltage5V(); -} + double pos = m_analogInput->GetVoltage() / RobotController::GetVoltage5V(); -double AnalogEncoder::GetPositionOffset() const { - return m_positionOffset; -} + // Map sensor range if range isn't full + pos = MapSensorRange(pos); -void AnalogEncoder::SetPositionOffset(double offset) { - m_positionOffset = std::clamp(offset, 0.0, 1.0); -} - -void AnalogEncoder::SetDistancePerRotation(double distancePerRotation) { - m_distancePerRotation = distancePerRotation; -} + // Compute full range and offset + pos = pos * m_fullRange - m_expectedZero; -double AnalogEncoder::GetDistancePerRotation() const { - return m_distancePerRotation; + // Map from 0 - Full Range + double result = InputModulus(pos, 0.0, m_fullRange); + // Invert if necessary + if (m_isInverted) { + return m_fullRange - result; + } + return result; } -double AnalogEncoder::GetDistance() const { - return Get().value() * GetDistancePerRotation(); +void AnalogEncoder::SetVoltagePercentageRange(double min, double max) { + m_sensorMin = std::clamp(min, 0.0, 1.0); + m_sensorMax = std::clamp(max, 0.0, 1.0); } -void AnalogEncoder::Reset() { - m_counter.Reset(); - m_positionOffset = - m_analogInput->GetVoltage() / frc::RobotController::GetVoltage5V(); +void AnalogEncoder::SetInverted(bool inverted) { + m_isInverted = inverted; } int AnalogEncoder::GetChannel() const { return m_analogInput->GetChannel(); } +double AnalogEncoder::MapSensorRange(double pos) const { + if (pos < m_sensorMin) { + pos = m_sensorMin; + } + if (pos > m_sensorMax) { + pos = m_sensorMax; + } + pos = (pos - m_sensorMin) / (m_sensorMax - m_sensorMin); + return pos; +} + void AnalogEncoder::InitSendable(wpi::SendableBuilder& builder) { builder.SetSmartDashboardType("AbsoluteEncoder"); builder.AddDoubleProperty( - "Distance", [this] { return this->GetDistance(); }, nullptr); - builder.AddDoubleProperty( - "Distance Per Rotation", - [this] { return this->GetDistancePerRotation(); }, nullptr); + "Position", [this] { return this->Get(); }, nullptr); } diff --git a/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp b/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp index f2ac77fb827..f127cf2ed1b 100644 --- a/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp +++ b/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp @@ -7,108 +7,137 @@ #include #include -#include "frc/Counter.h" #include "frc/DigitalInput.h" #include "frc/DigitalSource.h" #include "frc/DutyCycle.h" #include "frc/Errors.h" +#include "frc/MathUtil.h" using namespace frc; DutyCycleEncoder::DutyCycleEncoder(int channel) : m_dutyCycle{std::make_shared( std::make_shared(channel))} { - Init(); + Init(1.0, 0.0); } DutyCycleEncoder::DutyCycleEncoder(DutyCycle& dutyCycle) : m_dutyCycle{&dutyCycle, wpi::NullDeleter{}} { - Init(); + Init(1.0, 0.0); } DutyCycleEncoder::DutyCycleEncoder(DutyCycle* dutyCycle) : m_dutyCycle{dutyCycle, wpi::NullDeleter{}} { - Init(); + Init(1.0, 0.0); } DutyCycleEncoder::DutyCycleEncoder(std::shared_ptr dutyCycle) : m_dutyCycle{std::move(dutyCycle)} { - Init(); + Init(1.0, 0.0); } DutyCycleEncoder::DutyCycleEncoder(DigitalSource& digitalSource) : m_dutyCycle{std::make_shared(digitalSource)} { - Init(); + Init(1.0, 0.0); } DutyCycleEncoder::DutyCycleEncoder(DigitalSource* digitalSource) : m_dutyCycle{std::make_shared(digitalSource)} { - Init(); + Init(1.0, 0.0); } DutyCycleEncoder::DutyCycleEncoder(std::shared_ptr digitalSource) : m_dutyCycle{std::make_shared(digitalSource)} { - Init(); + Init(1.0, 0.0); } -void DutyCycleEncoder::Init() { +DutyCycleEncoder::DutyCycleEncoder(int channel, double fullRange, + double expectedZero) + : m_dutyCycle{std::make_shared( + std::make_shared(channel))} { + Init(fullRange, expectedZero); +} + +DutyCycleEncoder::DutyCycleEncoder(DutyCycle& dutyCycle, double fullRange, + double expectedZero) + : m_dutyCycle{&dutyCycle, wpi::NullDeleter{}} { + Init(fullRange, expectedZero); +} + +DutyCycleEncoder::DutyCycleEncoder(DutyCycle* dutyCycle, double fullRange, + double expectedZero) + : m_dutyCycle{dutyCycle, wpi::NullDeleter{}} { + Init(fullRange, expectedZero); +} + +DutyCycleEncoder::DutyCycleEncoder(std::shared_ptr dutyCycle, + double fullRange, double expectedZero) + : m_dutyCycle{std::move(dutyCycle)} { + Init(fullRange, expectedZero); +} + +DutyCycleEncoder::DutyCycleEncoder(DigitalSource& digitalSource, + double fullRange, double expectedZero) + : m_dutyCycle{std::make_shared(digitalSource)} { + Init(fullRange, expectedZero); +} + +DutyCycleEncoder::DutyCycleEncoder(DigitalSource* digitalSource, + double fullRange, double expectedZero) + : m_dutyCycle{std::make_shared(digitalSource)} { + Init(fullRange, expectedZero); +} + +DutyCycleEncoder::DutyCycleEncoder(std::shared_ptr digitalSource, + double fullRange, double expectedZero) + : m_dutyCycle{std::make_shared(digitalSource)} { + Init(fullRange, expectedZero); +} + +void DutyCycleEncoder::Init(double fullRange, double expectedZero) { m_simDevice = hal::SimDevice{"DutyCycle:DutyCycleEncoder", m_dutyCycle->GetSourceChannel()}; if (m_simDevice) { - m_simPosition = - m_simDevice.CreateDouble("position", hal::SimDevice::kInput, 0.0); - m_simDistancePerRotation = m_simDevice.CreateDouble( - "distance_per_rot", hal::SimDevice::kOutput, 1.0); - m_simAbsolutePosition = - m_simDevice.CreateDouble("absPosition", hal::SimDevice::kInput, 0.0); + m_simPosition = m_simDevice.CreateDouble("Position", false, 0.0); m_simIsConnected = - m_simDevice.CreateBoolean("connected", hal::SimDevice::kInput, true); - } else { - m_analogTrigger = std::make_unique(m_dutyCycle.get()); - m_analogTrigger->SetLimitsDutyCycle(0.25, 0.75); - m_counter = std::make_unique(); - m_counter->SetUpSource( - m_analogTrigger->CreateOutput(AnalogTriggerType::kRisingPulse)); - m_counter->SetDownSource( - m_analogTrigger->CreateOutput(AnalogTriggerType::kFallingPulse)); + m_simDevice.CreateBoolean("Connected", hal::SimDevice::kInput, true); } + m_fullRange = fullRange; + m_expectedZero = expectedZero; + wpi::SendableRegistry::AddLW(this, "DutyCycle Encoder", m_dutyCycle->GetSourceChannel()); } -static bool DoubleEquals(double a, double b) { - constexpr double epsilon = 0.00001; - return std::abs(a - b) < epsilon; -} - -units::turn_t DutyCycleEncoder::Get() const { +double DutyCycleEncoder::Get() const { if (m_simPosition) { - return units::turn_t{m_simPosition.Get()}; + 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++) { - auto counter = m_counter->Get(); - auto pos = m_dutyCycle->GetOutput(); - auto counter2 = m_counter->Get(); - auto pos2 = m_dutyCycle->GetOutput(); - if (counter == counter2 && DoubleEquals(pos, pos2)) { - // map sensor range - pos = MapSensorRange(pos); - units::turn_t turns{counter + pos - m_positionOffset}; - m_lastPosition = turns; - return turns; - } + double pos; + // Compute output percentage (0-1) + if (m_period.value() == 0.0) { + pos = m_dutyCycle->GetOutput(); + } else { + auto highTime = m_dutyCycle->GetHighTime(); + pos = highTime / m_period; } - FRC_ReportError( - warn::Warning, - "Failed to read DutyCycle Encoder. Potential Speed Overrun. Returning " - "last value"); - return m_lastPosition; + // 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 = InputModulus(pos, 0.0, m_fullRange); + // Invert if necessary + if (m_isInverted) { + return m_fullRange - result; + } + return result; } double DutyCycleEncoder::MapSensorRange(double pos) const { @@ -122,54 +151,15 @@ double DutyCycleEncoder::MapSensorRange(double pos) const { return pos; } -double DutyCycleEncoder::GetAbsolutePosition() const { - if (m_simAbsolutePosition) { - return m_simAbsolutePosition.Get(); - } - - return MapSensorRange(m_dutyCycle->GetOutput()); -} - -double DutyCycleEncoder::GetPositionOffset() const { - return m_positionOffset; -} - -void DutyCycleEncoder::SetPositionOffset(double offset) { - m_positionOffset = std::clamp(offset, 0.0, 1.0); -} - void DutyCycleEncoder::SetDutyCycleRange(double min, double max) { m_sensorMin = std::clamp(min, 0.0, 1.0); m_sensorMax = std::clamp(max, 0.0, 1.0); } -void DutyCycleEncoder::SetDistancePerRotation(double distancePerRotation) { - m_distancePerRotation = distancePerRotation; - m_simDistancePerRotation.Set(distancePerRotation); -} - -double DutyCycleEncoder::GetDistancePerRotation() const { - return m_distancePerRotation; -} - -double DutyCycleEncoder::GetDistance() const { - return Get().value() * GetDistancePerRotation(); -} - int DutyCycleEncoder::GetFrequency() const { return m_dutyCycle->GetFrequency(); } -void DutyCycleEncoder::Reset() { - if (m_counter) { - m_counter->Reset(); - } - if (m_simPosition) { - m_simPosition.Set(0); - } - m_positionOffset = GetAbsolutePosition(); -} - bool DutyCycleEncoder::IsConnected() const { if (m_simIsConnected) { return m_simIsConnected.Get(); @@ -184,6 +174,18 @@ void DutyCycleEncoder::SetConnectedFrequencyThreshold(int frequency) { m_frequencyThreshold = frequency; } +void DutyCycleEncoder::SetInverted(bool inverted) { + m_isInverted = inverted; +} + +void DutyCycleEncoder::SetAssumedFrequency(units::hertz_t frequency) { + if (frequency.value() == 0) { + m_period = 0_s; + } else { + m_period = 1.0 / frequency; + } +} + int DutyCycleEncoder::GetFPGAIndex() const { return m_dutyCycle->GetFPGAIndex(); } @@ -195,10 +197,7 @@ int DutyCycleEncoder::GetSourceChannel() const { void DutyCycleEncoder::InitSendable(wpi::SendableBuilder& builder) { builder.SetSmartDashboardType("AbsoluteEncoder"); builder.AddDoubleProperty( - "Distance", [this] { return this->GetDistance(); }, nullptr); - builder.AddDoubleProperty( - "Distance Per Rotation", - [this] { return this->GetDistancePerRotation(); }, nullptr); + "Position", [this] { return this->Get(); }, nullptr); builder.AddDoubleProperty( "Is Connected", [this] { return this->IsConnected(); }, nullptr); } diff --git a/wpilibc/src/main/native/cpp/simulation/AnalogEncoderSim.cpp b/wpilibc/src/main/native/cpp/simulation/AnalogEncoderSim.cpp index 435e29e378a..7bc58bd514c 100644 --- a/wpilibc/src/main/native/cpp/simulation/AnalogEncoderSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/AnalogEncoderSim.cpp @@ -14,18 +14,10 @@ AnalogEncoderSim::AnalogEncoderSim(const frc::AnalogEncoder& encoder) { m_positionSim = deviceSim.GetDouble("Position"); } -void AnalogEncoderSim::SetPosition(frc::Rotation2d angle) { - SetTurns(angle.Degrees()); +void AnalogEncoderSim::Set(double value) { + m_positionSim.Set(value); } -void AnalogEncoderSim::SetTurns(units::turn_t turns) { - m_positionSim.Set(turns.value()); -} - -units::turn_t AnalogEncoderSim::GetTurns() { - return units::turn_t{m_positionSim.Get()}; -} - -frc::Rotation2d AnalogEncoderSim::GetPosition() { - return units::radian_t{GetTurns()}; +double AnalogEncoderSim::Get() { + return m_positionSim.Get(); } diff --git a/wpilibc/src/main/native/cpp/simulation/DutyCycleEncoderSim.cpp b/wpilibc/src/main/native/cpp/simulation/DutyCycleEncoderSim.cpp index 3df775df9a3..8d5292ad8ca 100644 --- a/wpilibc/src/main/native/cpp/simulation/DutyCycleEncoderSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/DutyCycleEncoderSim.cpp @@ -14,38 +14,16 @@ DutyCycleEncoderSim::DutyCycleEncoderSim(const frc::DutyCycleEncoder& encoder) DutyCycleEncoderSim::DutyCycleEncoderSim(int channel) { frc::sim::SimDeviceSim deviceSim{"DutyCycle:DutyCycleEncoder", channel}; - m_simPosition = deviceSim.GetDouble("position"); - m_simDistancePerRotation = deviceSim.GetDouble("distance_per_rot"); - m_simAbsolutePosition = deviceSim.GetDouble("absPosition"); - m_simIsConnected = deviceSim.GetBoolean("connected"); + m_simPosition = deviceSim.GetDouble("Position"); + m_simIsConnected = deviceSim.GetBoolean("Connected"); } double DutyCycleEncoderSim::Get() { return m_simPosition.Get(); } -void DutyCycleEncoderSim::Set(units::turn_t turns) { - m_simPosition.Set(turns.value()); -} - -double DutyCycleEncoderSim::GetDistance() { - return m_simPosition.Get() * m_simDistancePerRotation.Get(); -} - -void DutyCycleEncoderSim::SetDistance(double distance) { - m_simPosition.Set(distance / m_simDistancePerRotation.Get()); -} - -double DutyCycleEncoderSim::GetAbsolutePosition() { - return m_simAbsolutePosition.Get(); -} - -void DutyCycleEncoderSim::SetAbsolutePosition(double position) { - m_simAbsolutePosition.Set(position); -} - -double DutyCycleEncoderSim::GetDistancePerRotation() { - return m_simDistancePerRotation.Get(); +void DutyCycleEncoderSim::Set(double value) { + m_simPosition.Set(value); } bool DutyCycleEncoderSim::IsConnected() { diff --git a/wpilibc/src/main/native/include/frc/AnalogEncoder.h b/wpilibc/src/main/native/include/frc/AnalogEncoder.h index 1860273ab43..b03974c0d67 100644 --- a/wpilibc/src/main/native/include/frc/AnalogEncoder.h +++ b/wpilibc/src/main/native/include/frc/AnalogEncoder.h @@ -8,13 +8,9 @@ #include #include -#include #include #include -#include "frc/AnalogTrigger.h" -#include "frc/Counter.h" - namespace frc { class AnalogInput; @@ -52,86 +48,49 @@ class AnalogEncoder : public wpi::Sendable, */ explicit AnalogEncoder(std::shared_ptr analogInput); - ~AnalogEncoder() override = default; - - AnalogEncoder(AnalogEncoder&&) = default; - AnalogEncoder& operator=(AnalogEncoder&&) = default; - - /** - * Reset the Encoder distance to zero. - */ - void Reset(); - /** - * Get the encoder value since the last reset. - * - * This is reported in rotations since the last reset. + * Construct a new AnalogEncoder attached to a specific AnalogIn channel. * - * @return the encoder value in rotations + * @param channel the analog input channel to attach to */ - units::turn_t Get() const; + AnalogEncoder(int channel, double fullRange, double expectedZero); /** - * Get the absolute position of the analog encoder. - * - *

GetAbsolutePosition() - GetPositionOffset() will give an encoder - * absolute position relative to the last reset. This could potentially be - * negative, which needs to be accounted for. - * - *

This will not account for rollovers, and will always be just the raw - * absolute position. + * Construct a new AnalogEncoder attached to a specific AnalogInput. * - * @return the absolute position + * @param analogInput the analog input to attach to */ - double GetAbsolutePosition() const; + AnalogEncoder(AnalogInput& analogInput, double fullRange, double expectedZero); /** - * Get the offset of position relative to the last reset. - * - * GetAbsolutePosition() - GetPositionOffset() will give an encoder absolute - * position relative to the last reset. This could potentially be negative, - * which needs to be accounted for. + * Construct a new AnalogEncoder attached to a specific AnalogInput. * - * @return the position offset + * @param analogInput the analog input to attach to */ - double GetPositionOffset() const; + AnalogEncoder(AnalogInput* analogInput, double fullRange, double expectedZero); /** - * Set the position offset. - * - *

This must be in the range of 0-1. + * Construct a new AnalogEncoder attached to a specific AnalogInput. * - * @param offset the offset + * @param analogInput the analog input to attach to */ - void SetPositionOffset(double offset); + AnalogEncoder(std::shared_ptr analogInput, double fullRange, double expectedZero); - /** - * 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 the 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 - */ - void SetDistancePerRotation(double distancePerRotation); + ~AnalogEncoder() override = default; - /** - * Get the distance per rotation for this encoder. - * - * @return The scale factor that will be used to convert rotation to useful - * units. - */ - double GetDistancePerRotation() const; + AnalogEncoder(AnalogEncoder&&) = default; + AnalogEncoder& operator=(AnalogEncoder&&) = default; /** - * Get the distance the sensor has driven since the last reset as scaled by - * the value from SetDistancePerRotation. + * Get the encoder value. * - * @return The distance driven since the last reset + * @return the encoder value scaled by the full range input */ - double GetDistance() const; + double Get() const; + + void SetVoltagePercentageRange(double min, double max); + + void SetInverted(bool inverted); /** * Get the channel number. @@ -143,17 +102,18 @@ class AnalogEncoder : public wpi::Sendable, void InitSendable(wpi::SendableBuilder& builder) override; private: - void Init(); + void Init(double fullRange, double expectedZero); + double MapSensorRange(double pos) const; std::shared_ptr m_analogInput; - AnalogTrigger m_analogTrigger; - Counter m_counter; - double m_positionOffset = 0; - double m_distancePerRotation = 1.0; - mutable units::turn_t m_lastPosition{0.0}; + bool m_ownsAnalogInput; + double m_fullRange; + double m_expectedZero; + double m_sensorMin{0.0}; + double m_sensorMax{1.0}; + bool m_isInverted{false}; hal::SimDevice m_simDevice; hal::SimDouble m_simPosition; - hal::SimDouble m_simAbsolutePosition; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h b/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h index 5d04ada87d1..936e401e31c 100644 --- a/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h +++ b/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h @@ -8,13 +8,11 @@ #include #include -#include +#include +#include #include #include -#include "frc/AnalogTrigger.h" -#include "frc/Counter.h" - namespace frc { class DutyCycle; class DigitalSource; @@ -76,84 +74,92 @@ class DutyCycleEncoder : public wpi::Sendable, */ explicit DutyCycleEncoder(std::shared_ptr digitalSource); - ~DutyCycleEncoder() override = default; - - DutyCycleEncoder(DutyCycleEncoder&&) = default; - DutyCycleEncoder& operator=(DutyCycleEncoder&&) = default; - /** - * Get the frequency in Hz of the duty cycle signal from the encoder. + * Construct a new DutyCycleEncoder on a specific channel. * - * @return duty cycle frequency in Hz + * @param channel the channel to attach to */ - int GetFrequency() const; + DutyCycleEncoder(int channel, double fullRange, double expectedZero); /** - * Get if the sensor is connected - * - * This uses the duty cycle frequency to determine if the sensor is connected. - * By default, a value of 100 Hz is used as the threshold, and this value can - * be changed with SetConnectedFrequencyThreshold. + * Construct a new DutyCycleEncoder attached to an existing DutyCycle object. * - * @return true if the sensor is connected + * @param dutyCycle the duty cycle to attach to */ - bool IsConnected() const; + DutyCycleEncoder(DutyCycle& dutyCycle, double fullRange, double expectedZero); /** - * Change the frequency threshold for detecting connection used by - * IsConnected. + * Construct a new DutyCycleEncoder attached to an existing DutyCycle object. * - * @param frequency the minimum frequency in Hz. + * @param dutyCycle the duty cycle to attach to */ - void SetConnectedFrequencyThreshold(int frequency); + DutyCycleEncoder(DutyCycle* dutyCycle, double fullRange, double expectedZero); /** - * Reset the Encoder distance to zero. + * Construct a new DutyCycleEncoder attached to an existing DutyCycle object. + * + * @param dutyCycle the duty cycle to attach to */ - void Reset(); + DutyCycleEncoder(std::shared_ptr dutyCycle, double fullRange, double expectedZero); /** - * Get the encoder value since the last reset. - * - * This is reported in rotations since the last reset. + * Construct a new DutyCycleEncoder attached to a DigitalSource object. * - * @return the encoder value in rotations + * @param digitalSource the digital source to attach to */ - units::turn_t Get() const; + DutyCycleEncoder(DigitalSource& digitalSource, double fullRange, double expectedZero); /** - * Get the absolute position of the duty cycle encoder encoder. + * Construct a new DutyCycleEncoder attached to a DigitalSource object. * - *

GetAbsolutePosition() - GetPositionOffset() will give an encoder - * absolute position relative to the last reset. This could potentially be - * negative, which needs to be accounted for. + * @param digitalSource the digital source to attach to + */ + DutyCycleEncoder(DigitalSource* digitalSource, double fullRange, double expectedZero); + + /** + * Construct a new DutyCycleEncoder attached to a DigitalSource object. * - *

This will not account for rollovers, and will always be just the raw - * absolute position. + * @param digitalSource the digital source to attach to + */ + DutyCycleEncoder(std::shared_ptr digitalSource, double fullRange, double expectedZero); + + ~DutyCycleEncoder() override = default; + + DutyCycleEncoder(DutyCycleEncoder&&) = default; + DutyCycleEncoder& operator=(DutyCycleEncoder&&) = default; + + /** + * Get the frequency in Hz of the duty cycle signal from the encoder. * - * @return the absolute position + * @return duty cycle frequency in Hz */ - double GetAbsolutePosition() const; + int GetFrequency() const; /** - * Get the offset of position relative to the last reset. + * Get if the sensor is connected * - * GetAbsolutePosition() - GetPositionOffset() will give an encoder absolute - * position relative to the last reset. This could potentially be negative, - * which needs to be accounted for. + * This uses the duty cycle frequency to determine if the sensor is connected. + * By default, a value of 100 Hz is used as the threshold, and this value can + * be changed with SetConnectedFrequencyThreshold. * - * @return the position offset + * @return true if the sensor is connected */ - double GetPositionOffset() const; + bool IsConnected() const; /** - * Set the position offset. + * Change the frequency threshold for detecting connection used by + * IsConnected. * - *

This must be in the range of 0-1. + * @param frequency the minimum frequency in Hz. + */ + void SetConnectedFrequencyThreshold(int frequency); + + /** + * Get the encoder value. * - * @param offset the offset + * @return the encoder value scaled by the full range input */ - void SetPositionOffset(double offset); + double Get() const; /** * Set the encoder duty cycle range. As the encoder needs to maintain a duty @@ -170,33 +176,9 @@ class DutyCycleEncoder : public wpi::Sendable, */ void SetDutyCycleRange(double min, double max); - /** - * 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 the 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 - */ - void SetDistancePerRotation(double distancePerRotation); - - /** - * Get the distance per rotation for this encoder. - * - * @return The scale factor that will be used to convert rotation to useful - * units. - */ - double GetDistancePerRotation() const; + void SetAssumedFrequency(units::hertz_t frequency); - /** - * Get the distance the sensor has driven since the last reset as scaled by - * the value from SetDistancePerRotation. - * - * @return The distance driven since the last reset - */ - double GetDistance() const; + void SetInverted(bool inverted); /** * Get the FPGA index for the DutyCycleEncoder. @@ -215,23 +197,20 @@ class DutyCycleEncoder : public wpi::Sendable, void InitSendable(wpi::SendableBuilder& builder) override; private: - void Init(); + void Init(double fullRange, double expectedZero); double MapSensorRange(double pos) const; std::shared_ptr m_dutyCycle; - std::unique_ptr m_analogTrigger; - std::unique_ptr m_counter; int m_frequencyThreshold = 100; - double m_positionOffset = 0; - double m_distancePerRotation = 1.0; - mutable units::turn_t m_lastPosition{0.0}; - double m_sensorMin = 0; - double m_sensorMax = 1; + double m_fullRange; + double m_expectedZero; + units::second_t m_period{0_s}; + double m_sensorMin{0.0}; + double m_sensorMax{1.0}; + bool m_isInverted{false}; hal::SimDevice m_simDevice; hal::SimDouble m_simPosition; - hal::SimDouble m_simAbsolutePosition; - hal::SimDouble m_simDistancePerRotation; hal::SimBoolean m_simIsConnected; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/frc/simulation/AnalogEncoderSim.h b/wpilibc/src/main/native/include/frc/simulation/AnalogEncoderSim.h index 6d763267044..00b2b876b1c 100644 --- a/wpilibc/src/main/native/include/frc/simulation/AnalogEncoderSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/AnalogEncoderSim.h @@ -28,28 +28,16 @@ class AnalogEncoderSim { explicit AnalogEncoderSim(const AnalogEncoder& encoder); /** - * Set the position using an Rotation2d. + * Set the position. * - * @param angle The angle. + * @param angle The position. */ - void SetPosition(Rotation2d angle); - - /** - * Set the position of the encoder. - * - * @param turns The position. - */ - void SetTurns(units::turn_t turns); + void Set(double value); /** * Get the simulated position. */ - units::turn_t GetTurns(); - - /** - * Get the position as a Rotation2d. - */ - Rotation2d GetPosition(); + double Get(); private: hal::SimDouble m_positionSim; diff --git a/wpilibc/src/main/native/include/frc/simulation/DutyCycleEncoderSim.h b/wpilibc/src/main/native/include/frc/simulation/DutyCycleEncoderSim.h index 18c04f0562c..06bb1df1a57 100644 --- a/wpilibc/src/main/native/include/frc/simulation/DutyCycleEncoderSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/DutyCycleEncoderSim.h @@ -33,55 +33,18 @@ class DutyCycleEncoderSim { explicit DutyCycleEncoderSim(int channel); /** - * Get the position in turns. + * Get the position. * * @return The position. */ double Get(); /** - * Set the position in turns. + * Set the position. * * @param turns The position. */ - void Set(units::turn_t turns); - - /** - * Get the distance. - * - * @return The distance. - */ - - double GetDistance(); - - /** - * Set the distance. - * - * @param distance The distance. - */ - void SetDistance(double distance); - - /** - * Get the absolute position. - * - * @return The absolute position - */ - double GetAbsolutePosition(); - - /** - * Set the absolute position. - * - * @param position The absolute position - */ - void SetAbsolutePosition(double position); - - /** - * Get the distance per rotation for this encoder. - * - * @return The scale factor that will be used to convert rotation to useful - * units. - */ - double GetDistancePerRotation(); + void Set(double value); /** * Get if the encoder is connected. @@ -99,8 +62,6 @@ class DutyCycleEncoderSim { private: hal::SimDouble m_simPosition; - hal::SimDouble m_simDistancePerRotation; - hal::SimDouble m_simAbsolutePosition; hal::SimBoolean m_simIsConnected; }; diff --git a/wpilibc/src/test/native/cpp/simulation/AnalogEncoderSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/AnalogEncoderSimTest.cpp index 3f0ea459207..fdfd5cec647 100644 --- a/wpilibc/src/test/native/cpp/simulation/AnalogEncoderSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/AnalogEncoderSimTest.cpp @@ -17,12 +17,10 @@ TEST(AnalogEncoderSimTest, Basic) { frc::AnalogInput ai(0); - frc::AnalogEncoder encoder{ai}; + frc::AnalogEncoder encoder{ai, 360, 0}; frc::sim::AnalogEncoderSim encoderSim{encoder}; - encoderSim.SetPosition(180_deg); - EXPECT_NEAR(encoder.Get().value(), 0.5, 1E-8); - EXPECT_NEAR(encoderSim.GetTurns().value(), 0.5, 1E-8); - EXPECT_NEAR(encoderSim.GetPosition().Radians().value(), std::numbers::pi, - 1E-8); + encoderSim.Set(180); + EXPECT_NEAR(encoder.Get(), 180, 1E-8); + EXPECT_NEAR(encoderSim.Get(), 180, 1E-8); } diff --git a/wpilibc/src/test/native/cpp/simulation/DutyCycleEncoderSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/DutyCycleEncoderSimTest.cpp index e81c63e3e2c..714edf655ec 100644 --- a/wpilibc/src/test/native/cpp/simulation/DutyCycleEncoderSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/DutyCycleEncoderSimTest.cpp @@ -15,42 +15,14 @@ namespace frc::sim { TEST(DutyCycleEncoderSimTest, Set) { HAL_Initialize(500, 0); - DutyCycleEncoder enc{0}; + DutyCycleEncoder enc{0, 10, 0}; DutyCycleEncoderSim sim(enc); - constexpr units::turn_t kTestValue{5.67}; + constexpr double kTestValue{5.67}; sim.Set(kTestValue); EXPECT_EQ(kTestValue, enc.Get()); } -TEST(DutyCycleEncoderSimTest, SetDistance) { - HAL_Initialize(500, 0); - - DutyCycleEncoder enc{0}; - DutyCycleEncoderSim sim(enc); - sim.SetDistance(19.1); - EXPECT_EQ(19.1, enc.GetDistance()); -} - -TEST(DutyCycleEncoderSimTest, SetDistancePerRotation) { - HAL_Initialize(500, 0); - - DutyCycleEncoder enc{0}; - DutyCycleEncoderSim sim(enc); - sim.Set(units::turn_t{1.5}); - enc.SetDistancePerRotation(42); - EXPECT_EQ(63, enc.GetDistance()); -} - -TEST(DutyCycleEncoderSimTest, SetAbsolutePosition) { - HAL_Initialize(500, 0); - - DutyCycleEncoder enc{0}; - DutyCycleEncoderSim sim(enc); - sim.SetAbsolutePosition(0.75); - EXPECT_EQ(0.75, enc.GetAbsolutePosition()); -} - TEST(DutyCycleEncoderSimTest, SetIsConnected) { HAL_Initialize(500, 0); @@ -62,15 +34,4 @@ TEST(DutyCycleEncoderSimTest, SetIsConnected) { EXPECT_FALSE(enc.IsConnected()); } -TEST(DutyCycleEncoderSimTest, Reset) { - HAL_Initialize(500, 0); - - DutyCycleEncoder enc{0}; - DutyCycleEncoderSim sim(enc); - sim.SetDistance(2.5); - EXPECT_EQ(2.5, enc.GetDistance()); - enc.Reset(); - EXPECT_EQ(0, enc.GetDistance()); -} - } // namespace frc::sim diff --git a/wpilibcExamples/src/main/cpp/examples/DutyCycleEncoder/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/DutyCycleEncoder/cpp/Robot.cpp index a9343d8aea4..73c91616753 100644 --- a/wpilibcExamples/src/main/cpp/examples/DutyCycleEncoder/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/DutyCycleEncoder/cpp/Robot.cpp @@ -5,15 +5,34 @@ #include #include #include +#include + +constexpr double fullRange = 1.3; +constexpr double expectedZero = 0.0; class Robot : public frc::TimedRobot { - // Duty cycle encoder on channel 0 - frc::DutyCycleEncoder m_dutyCycleEncoder{0}; + // 2nd parameter is the range of values. This sensor will output between + // 0 and the passed in value. + // 3rd parameter is the the physical value where you want "0" to be. How + // to measure this is fairly easy. Set the value to 0, place the mechanism + // where you want "0" to be, and observe the value on the dashboard, That + // is the value to enter for the 3rd parameter. + frc::DutyCycleEncoder m_dutyCycleEncoder{0, fullRange, expectedZero}; public: void RobotInit() override { - // Set to 0.5 units per rotation - m_dutyCycleEncoder.SetDistancePerRotation(0.5); + // If you know the frequency of your sensor, uncomment the following + // method, and set the method to the frequency of your sensor. + // This will result in more stable readings from the sensor. + // Do note that occasionally the datasheet cannot be trusted + // and you should measure this value. You can do so with either + // an oscilloscope, or by observing the "Frequency" output + // on the dashboard while running this sample. If you find + // the value jumping between the 2 values, enter halfway between + // those values. This number doesn't have to be perfect, + // just having a fairly close value will make the output readings + // much more stable. + m_dutyCycleEncoder.SetAssumedFrequency(967.8_Hz); } void RobotPeriodic() override { @@ -26,13 +45,24 @@ class Robot : public frc::TimedRobot { // Output of encoder auto output = m_dutyCycleEncoder.Get(); - // Output scaled by DistancePerPulse - auto distance = m_dutyCycleEncoder.GetDistance(); + // By default, the output will wrap around to the full range value + // when the sensor goes below 0. However, for moving mechanisms this + // is not usually ideal, as if 0 is set to a hard stop, its still + // possible for the sensor to move slightly past. If this happens + // The sensor will assume its now at the furthest away position, + // which control algorithms might not handle correctly. Therefore + // it can be a good idea to slightly shift the output so the sensor + // can go a bit negative before wrapping. Usually 10% or so is fine. + // This does not change where "0" is, so no calibration numbers need + // to be changed. + double percentOfRange = fullRange * 0.1; + double shiftedOutput = frc::InputModulus(output, 0 - percentOfRange, + fullRange - percentOfRange); frc::SmartDashboard::PutBoolean("Connected", connected); frc::SmartDashboard::PutNumber("Frequency", frequency); - frc::SmartDashboard::PutNumber("Output", output.value()); - frc::SmartDashboard::PutNumber("Distance", distance); + frc::SmartDashboard::PutNumber("Output", output); + frc::SmartDashboard::PutNumber("ShiftedOutput", shiftedOutput); } }; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java index 6b116731a5a..bead64af562 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java @@ -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 AnalogEncoderSim(AnalogEncoder encoder) { } /** - * Set the position using an {@link Rotation2d}. + * Set the position * - * @param angle The angle. + * @param angle 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 void setTurns(double turns) { * * @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); - } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java index 786c16eea17..660f09333a7 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java @@ -43,12 +43,12 @@ public double get() { } /** - * Set the position in turns. + * Set the position. * * @param turns The position. */ - public void set(double turns) { - m_simPosition.set(turns); + public void set(double value) { + m_simPosition.set(value); } /** diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSimTest.java index a04ce10bb0d..ea5edaafcf2 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSimTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSimTest.java @@ -6,7 +6,6 @@ import static org.junit.jupiter.api.Assertions.assertEquals; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.AnalogEncoder; import edu.wpi.first.wpilibj.AnalogInput; import org.junit.jupiter.api.Test; @@ -15,13 +14,12 @@ class AnalogEncoderSimTest { @Test void testBasic() { try (var analogInput = new AnalogInput(0); - var analogEncoder = new AnalogEncoder(analogInput)) { + var analogEncoder = new AnalogEncoder(analogInput, 360, 0)) { var encoderSim = new AnalogEncoderSim(analogEncoder); - encoderSim.setPosition(Rotation2d.fromDegrees(180)); - assertEquals(analogEncoder.get(), 0.5, 1E-8); - assertEquals(encoderSim.getTurns(), 0.5, 1E-8); - assertEquals(encoderSim.getPosition().getRadians(), Math.PI, 1E-8); + encoderSim.set(180); + assertEquals(analogEncoder.get(), 180, 1E-8); + assertEquals(encoderSim.get(), 180, 1E-8); } } } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSimTest.java index 42a6faa6282..3ccc05e7ac1 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSimTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSimTest.java @@ -15,7 +15,7 @@ class DutyCycleEncoderSimTest { @Test void setTest() { - try (DutyCycleEncoder encoder = new DutyCycleEncoder(0)) { + try (DutyCycleEncoder encoder = new DutyCycleEncoder(0, 5.67, 0)) { DutyCycleEncoderSim sim = new DutyCycleEncoderSim(encoder); sim.set(5.67); @@ -23,42 +23,6 @@ void setTest() { } } - @Test - void setDistanceTest() { - HAL.initialize(500, 0); - - try (DutyCycleEncoder encoder = new DutyCycleEncoder(0)) { - DutyCycleEncoderSim sim = new DutyCycleEncoderSim(encoder); - - sim.setDistance(19.1); - assertEquals(19.1, encoder.getDistance()); - } - } - - @Test - void setDistancePerRotationTest() { - HAL.initialize(500, 0); - - try (DutyCycleEncoder encoder = new DutyCycleEncoder(0)) { - DutyCycleEncoderSim sim = new DutyCycleEncoderSim(encoder); - sim.set(1.5); - encoder.setDistancePerRotation(42); - assertEquals(63.0, encoder.getDistance()); - } - } - - @Test - void setAbsolutePositionTest() { - HAL.initialize(500, 0); - - try (DutyCycleEncoder encoder = new DutyCycleEncoder(0)) { - DutyCycleEncoderSim sim = new DutyCycleEncoderSim(encoder); - - sim.setAbsolutePosition(0.75); - assertEquals(0.75, encoder.getAbsolutePosition()); - } - } - @Test void setIsConnectedTest() { HAL.initialize(500, 0); @@ -72,18 +36,4 @@ void setIsConnectedTest() { assertFalse(encoder.isConnected()); } } - - @Test - void resetTest() { - HAL.initialize(500, 0); - - try (DutyCycleEncoder encoder = new DutyCycleEncoder(0)) { - DutyCycleEncoderSim sim = new DutyCycleEncoderSim(encoder); - - sim.setDistance(2.5); - assertEquals(2.5, encoder.getDistance()); - encoder.reset(); - assertEquals(0.0, encoder.getDistance()); - } - } } From 1e02bbb9a31b60205eb4b451d719591380b937e0 Mon Sep 17 00:00:00 2001 From: Thad House Date: Sat, 2 Mar 2024 13:33:43 -0800 Subject: [PATCH 06/30] Fix unused variable --- wpilibc/src/main/native/include/frc/AnalogEncoder.h | 1 - 1 file changed, 1 deletion(-) diff --git a/wpilibc/src/main/native/include/frc/AnalogEncoder.h b/wpilibc/src/main/native/include/frc/AnalogEncoder.h index b03974c0d67..51bb27517a7 100644 --- a/wpilibc/src/main/native/include/frc/AnalogEncoder.h +++ b/wpilibc/src/main/native/include/frc/AnalogEncoder.h @@ -106,7 +106,6 @@ class AnalogEncoder : public wpi::Sendable, double MapSensorRange(double pos) const; std::shared_ptr m_analogInput; - bool m_ownsAnalogInput; double m_fullRange; double m_expectedZero; double m_sensorMin{0.0}; From d33ba3c53660a5f12f9d37b78af8dddbe8093731 Mon Sep 17 00:00:00 2001 From: Thad House Date: Sat, 2 Mar 2024 14:03:59 -0800 Subject: [PATCH 07/30] Add rollover support --- wpilibc/src/main/native/cpp/AnalogEncoder.cpp | 93 +++++++++++++++--- .../main/native/include/frc/AnalogEncoder.h | 12 +++ .../edu/wpi/first/wpilibj/AnalogEncoder.java | 95 ++++++++++++++++--- 3 files changed, 172 insertions(+), 28 deletions(-) diff --git a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp index c4381f0ef31..def9ea33a1e 100644 --- a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp +++ b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp @@ -8,6 +8,7 @@ #include #include "frc/AnalogInput.h" +#include "frc/Counter.h" #include "frc/Errors.h" #include "frc/RobotController.h" #include "frc/MathUtil.h" @@ -73,21 +74,12 @@ double AnalogEncoder::Get() const { return m_simPosition.Get(); } - double pos = m_analogInput->GetVoltage() / 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 = InputModulus(pos, 0.0, m_fullRange); - // Invert if necessary - if (m_isInverted) { - return m_fullRange - result; + if (m_rolloverCounter) { + return GetWithRollovers(); + } else { + double analog = m_analogInput->GetVoltage(); + return GetWithoutRollovers(analog); } - return result; } void AnalogEncoder::SetVoltagePercentageRange(double min, double max) { @@ -119,3 +111,76 @@ void AnalogEncoder::InitSendable(wpi::SendableBuilder& builder) { builder.AddDoubleProperty( "Position", [this] { return this->Get(); }, nullptr); } + +void AnalogEncoder::ConfigureRolloverSupport(bool enable) { + if (enable && !m_rolloverCounter) { + m_rolloverTrigger = std::make_unique(m_analogInput.get()); + m_rolloverTrigger->SetLimitsVoltage(1.25, 3.75); + m_rolloverCounter = std::make_unique(); + m_rolloverCounter->SetUpSource( + m_rolloverTrigger->CreateOutput(AnalogTriggerType::kRisingPulse)); + m_rolloverCounter->SetDownSource( + m_rolloverTrigger->CreateOutput(AnalogTriggerType::kFallingPulse)); + } else if (!enable && m_rolloverCounter) { + m_rolloverCounter = nullptr; + m_rolloverTrigger = nullptr; + } +} + +void AnalogEncoder::ResetRollovers() { + if (m_rolloverCounter) { + m_rolloverCounter->Reset(); + } +} + +double AnalogEncoder::GetWithoutRollovers(double analog) const { + 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 = InputModulus(pos, 0.0, m_fullRange); + // Invert if necessary + if (m_isInverted) { + return m_fullRange - result; + } + return result; +} + +static bool DoubleEquals(double a, double b) { + constexpr double epsilon = 0.00001; + return std::abs(a - b) < epsilon; +} + +double AnalogEncoder::GetWithRollovers() const { + // 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++) { + auto counter = m_rolloverCounter->Get(); + auto pos = m_analogInput->GetVoltage(); + auto counter2 = m_rolloverCounter->Get(); + auto pos2 = m_analogInput->GetVoltage(); + if (counter == counter2 && DoubleEquals(pos, pos2)) { + pos = GetWithoutRollovers(pos); + + if (m_isInverted) { + pos = pos - counter; + } else { + pos = pos + counter; + } + + m_lastPosition = pos; + return pos; + } + } + + FRC_ReportError( + warn::Warning, + "Failed to read Analog Encoder. Potential Speed Overrun. Returning last " + "value"); + return m_lastPosition; +} \ No newline at end of file diff --git a/wpilibc/src/main/native/include/frc/AnalogEncoder.h b/wpilibc/src/main/native/include/frc/AnalogEncoder.h index 51bb27517a7..c7831f2a852 100644 --- a/wpilibc/src/main/native/include/frc/AnalogEncoder.h +++ b/wpilibc/src/main/native/include/frc/AnalogEncoder.h @@ -13,6 +13,8 @@ namespace frc { class AnalogInput; +class Counter; +class AnalogTrigger; /** * Class for supporting continuous analog encoders, such as the US Digital MA3. @@ -101,10 +103,16 @@ class AnalogEncoder : public wpi::Sendable, void InitSendable(wpi::SendableBuilder& builder) override; + void ConfigureRolloverSupport(bool enable); + void ResetRollovers(); + private: void Init(double fullRange, double expectedZero); double MapSensorRange(double pos) const; + double GetWithoutRollovers(double analog) const; + double GetWithRollovers() const; + std::shared_ptr m_analogInput; double m_fullRange; double m_expectedZero; @@ -112,6 +120,10 @@ class AnalogEncoder : public wpi::Sendable, double m_sensorMax{1.0}; bool m_isInverted{false}; + std::unique_ptr m_rolloverTrigger{nullptr}; + std::unique_ptr m_rolloverCounter{nullptr}; + mutable double m_lastPosition{0.0}; + hal::SimDevice m_simDevice; hal::SimDouble m_simPosition; }; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java index 617c50a6b42..8465953e74c 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java @@ -11,6 +11,7 @@ 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. @@ -24,6 +25,10 @@ public class AnalogEncoder implements Sendable, AutoCloseable { private double m_sensorMax = 1.0; private boolean m_isInverted; + private AnalogTrigger m_rolloverTrigger; + private Counter m_rolloverCounter; + private double m_lastPosition; + private SimDevice m_simDevice; private SimDouble m_simPosition; @@ -105,21 +110,12 @@ public double get() { return m_simPosition.get(); } - double pos = m_analogInput.getVoltage() / 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; + if (m_rolloverCounter != null) { + return getWithRollovers(); + } else { + double analog = m_analogInput.getVoltage(); + return getWithoutRollovers(analog); } - return result; } /** @@ -158,6 +154,77 @@ public void close() { } } + public void configureRolloverSupport(boolean enable) { + if (enable && m_rolloverCounter == null) { + m_rolloverTrigger = new AnalogTrigger(m_analogInput); + m_rolloverTrigger.setLimitsVoltage(1.25, 3.75); + m_rolloverCounter = new Counter(); + m_rolloverCounter.setUpSource(m_rolloverTrigger.createOutput(AnalogTriggerType.kRisingPulse)); + m_rolloverCounter.setDownSource(m_rolloverTrigger.createOutput(AnalogTriggerType.kFallingPulse)); + } else if (!enable && m_rolloverCounter != null) { + m_rolloverTrigger.close(); + m_rolloverTrigger = null; + m_rolloverCounter.close(); + m_rolloverCounter = null; + } + } + + public void resetRollovers() { + if (m_rolloverCounter != null) { + m_rolloverCounter.reset(); + } + } + + private boolean doubleEquals(double a, double b) { + double epsilon = 0.00001d; + return Math.abs(a - b) < epsilon; + } + + private double getWithRollovers() { + // 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_rolloverCounter.get(); + double pos = m_analogInput.getVoltage(); + double counter2 = m_rolloverCounter.get(); + double pos2 = m_analogInput.getVoltage(); + if (counter == counter2 && doubleEquals(pos, pos2)) { + pos = getWithoutRollovers(pos); + + if (m_isInverted) { + pos = pos - counter; + } else { + pos = pos + counter; + } + + m_lastPosition = pos; + return pos; + } + } + + DriverStation.reportWarning( + "Failed to read Analog Encoder. Potential Speed Overrun. Returning last value", false); + return m_lastPosition; + } + + private double getWithoutRollovers(double analog) { + 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; + } + return result; + } + @Override public void initSendable(SendableBuilder builder) { builder.setSmartDashboardType("AbsoluteEncoder"); From 85adf21b1e651a595692feadcc992cf205f2a583 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sat, 2 Mar 2024 22:08:48 +0000 Subject: [PATCH 08/30] Formatting fixes --- myRobot/src/main/java/frc/robot/MyRobot.java | 4 +- wpilibc/src/main/native/cpp/AnalogEncoder.cpp | 4 +- .../main/native/include/frc/AnalogEncoder.h | 9 ++-- .../native/include/frc/DutyCycleEncoder.h | 12 ++++-- .../examples/DutyCycleEncoder/cpp/Robot.cpp | 2 +- .../edu/wpi/first/wpilibj/AnalogEncoder.java | 15 +++---- .../wpi/first/wpilibj/DutyCycleEncoder.java | 41 +++++++------------ .../examples/dutycycleencoder/Robot.java | 9 ++-- 8 files changed, 43 insertions(+), 53 deletions(-) diff --git a/myRobot/src/main/java/frc/robot/MyRobot.java b/myRobot/src/main/java/frc/robot/MyRobot.java index d63e534882b..5772b1056c5 100644 --- a/myRobot/src/main/java/frc/robot/MyRobot.java +++ b/myRobot/src/main/java/frc/robot/MyRobot.java @@ -11,6 +11,7 @@ public class MyRobot extends TimedRobot { DutyCycleEncoder pot; + /** * This function is run when the robot is first started up and should be used for any * initialization code. @@ -48,7 +49,8 @@ public void robotPeriodic() { SmartDashboard.putNumber("DC4", val); SmartDashboard.putNumber("DC3", MathUtil.invertInput(val, 1, 4)); SmartDashboard.putNumber("DC2", MathUtil.inputModulus(pot.get(), -0.1, 0.9)); - SmartDashboard.putNumber("DC", MathUtil.inputModulus(MathUtil.invertInput(pot.get(), 0, 1), -0.1, 0.9)); + SmartDashboard.putNumber( + "DC", MathUtil.inputModulus(MathUtil.invertInput(pot.get(), 0, 1), -0.1, 0.9)); SmartDashboard.putNumber("Freq", pot.getFrequency()); } } diff --git a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp index def9ea33a1e..1de52f86624 100644 --- a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp +++ b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp @@ -10,8 +10,8 @@ #include "frc/AnalogInput.h" #include "frc/Counter.h" #include "frc/Errors.h" -#include "frc/RobotController.h" #include "frc/MathUtil.h" +#include "frc/RobotController.h" using namespace frc; @@ -183,4 +183,4 @@ double AnalogEncoder::GetWithRollovers() const { "Failed to read Analog Encoder. Potential Speed Overrun. Returning last " "value"); return m_lastPosition; -} \ No newline at end of file +} diff --git a/wpilibc/src/main/native/include/frc/AnalogEncoder.h b/wpilibc/src/main/native/include/frc/AnalogEncoder.h index c7831f2a852..5162ce17eee 100644 --- a/wpilibc/src/main/native/include/frc/AnalogEncoder.h +++ b/wpilibc/src/main/native/include/frc/AnalogEncoder.h @@ -62,21 +62,24 @@ class AnalogEncoder : public wpi::Sendable, * * @param analogInput the analog input to attach to */ - AnalogEncoder(AnalogInput& analogInput, double fullRange, double expectedZero); + AnalogEncoder(AnalogInput& analogInput, double fullRange, + double expectedZero); /** * Construct a new AnalogEncoder attached to a specific AnalogInput. * * @param analogInput the analog input to attach to */ - AnalogEncoder(AnalogInput* analogInput, double fullRange, double expectedZero); + AnalogEncoder(AnalogInput* analogInput, double fullRange, + double expectedZero); /** * Construct a new AnalogEncoder attached to a specific AnalogInput. * * @param analogInput the analog input to attach to */ - AnalogEncoder(std::shared_ptr analogInput, double fullRange, double expectedZero); + AnalogEncoder(std::shared_ptr analogInput, double fullRange, + double expectedZero); ~AnalogEncoder() override = default; diff --git a/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h b/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h index 936e401e31c..10819387372 100644 --- a/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h +++ b/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h @@ -100,28 +100,32 @@ class DutyCycleEncoder : public wpi::Sendable, * * @param dutyCycle the duty cycle to attach to */ - DutyCycleEncoder(std::shared_ptr dutyCycle, double fullRange, double expectedZero); + DutyCycleEncoder(std::shared_ptr dutyCycle, double fullRange, + double expectedZero); /** * Construct a new DutyCycleEncoder attached to a DigitalSource object. * * @param digitalSource the digital source to attach to */ - DutyCycleEncoder(DigitalSource& digitalSource, double fullRange, double expectedZero); + DutyCycleEncoder(DigitalSource& digitalSource, double fullRange, + double expectedZero); /** * Construct a new DutyCycleEncoder attached to a DigitalSource object. * * @param digitalSource the digital source to attach to */ - DutyCycleEncoder(DigitalSource* digitalSource, double fullRange, double expectedZero); + DutyCycleEncoder(DigitalSource* digitalSource, double fullRange, + double expectedZero); /** * Construct a new DutyCycleEncoder attached to a DigitalSource object. * * @param digitalSource the digital source to attach to */ - DutyCycleEncoder(std::shared_ptr digitalSource, double fullRange, double expectedZero); + DutyCycleEncoder(std::shared_ptr digitalSource, + double fullRange, double expectedZero); ~DutyCycleEncoder() override = default; diff --git a/wpilibcExamples/src/main/cpp/examples/DutyCycleEncoder/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/DutyCycleEncoder/cpp/Robot.cpp index 73c91616753..3889d1c4dad 100644 --- a/wpilibcExamples/src/main/cpp/examples/DutyCycleEncoder/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/DutyCycleEncoder/cpp/Robot.cpp @@ -3,9 +3,9 @@ // the WPILib BSD license file in the root directory of this project. #include +#include #include #include -#include constexpr double fullRange = 1.3; constexpr double expectedZero = 0.0; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java index 8465953e74c..8a76d3e7c14 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java @@ -13,9 +13,7 @@ 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. - */ +/** Class for supporting continuous analog encoders, such as the US Digital MA3. */ public class AnalogEncoder implements Sendable, AutoCloseable { private final AnalogInput m_analogInput; private boolean m_ownsAnalogInput; @@ -100,8 +98,7 @@ private double mapSensorRange(double pos) { /** * Get the encoder value since the last reset. * - *

- * This is reported in rotations since the last reset. + *

This is reported in rotations since the last reset. * * @return the encoder value in rotations */ @@ -119,9 +116,8 @@ public double get() { } /** - * 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. + * 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. * * @param min minimum voltage percentage (0-1 range) * @param max maximum voltage percentage (0-1 range) @@ -160,7 +156,8 @@ public void configureRolloverSupport(boolean enable) { m_rolloverTrigger.setLimitsVoltage(1.25, 3.75); m_rolloverCounter = new Counter(); m_rolloverCounter.setUpSource(m_rolloverTrigger.createOutput(AnalogTriggerType.kRisingPulse)); - m_rolloverCounter.setDownSource(m_rolloverTrigger.createOutput(AnalogTriggerType.kFallingPulse)); + m_rolloverCounter.setDownSource( + m_rolloverTrigger.createOutput(AnalogTriggerType.kFallingPulse)); } else if (!enable && m_rolloverCounter != null) { m_rolloverTrigger.close(); m_rolloverTrigger = null; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java index 5e321b6dc48..1ff3d1beaff 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java @@ -13,8 +13,7 @@ import edu.wpi.first.util.sendable.SendableRegistry; /** - * Class for supporting duty cycle/PWM encoders, such as the US Digital MA3 with - * PWM Output, the + * Class for supporting duty cycle/PWM encoders, such as the US Digital MA3 with PWM Output, the * CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag Encoder. */ public class DutyCycleEncoder implements Sendable, AutoCloseable { @@ -128,8 +127,7 @@ private double mapSensorRange(double pos) { /** * Get the encoder value since the last reset. * - *

- * This is reported in rotations since the last reset. + *

This is reported in rotations since the last reset. * * @return the encoder value in rotations */ @@ -163,16 +161,11 @@ public double get() { } /** - * Set the encoder duty cycle range. As the encoder needs to maintain a duty - * cycle, the duty cycle - * cannot go all the way to 0% or all the way to 100%. For example, an encoder - * with a 4096 us - * period might have a minimum duty cycle of 1 us / 4096 us and a maximum duty - * cycle of 4095 / - * 4096 us. Setting the range will result in an encoder duty cycle less than or - * equal to the - * minimum being output as 0 rotation, the duty cycle greater than or equal to - * the maximum being + * Set the encoder duty cycle range. As the encoder needs to maintain a duty cycle, the duty cycle + * cannot go all the way to 0% or all the way to 100%. For example, an encoder with a 4096 us + * period might have a minimum duty cycle of 1 us / 4096 us and a maximum duty cycle of 4095 / + * 4096 us. Setting the range will result in an encoder duty cycle less than or equal to the + * minimum being output as 0 rotation, the duty cycle greater than or equal to the maximum being * output as 1 rotation, and values in between linearly scaled from 0 to 1. * * @param min minimum duty cycle (0-1 range) @@ -195,11 +188,8 @@ public int getFrequency() { /** * Get if the sensor is connected * - *

- * This uses the duty cycle frequency to determine if the sensor is connected. - * By default, a - * value of 100 Hz is used as the threshold, and this value can be changed with - * {@link + *

This uses the duty cycle frequency to determine if the sensor is connected. By default, a + * value of 100 Hz is used as the threshold, and this value can be changed with {@link * #setConnectedFrequencyThreshold(int)}. * * @return true if the sensor is connected @@ -212,8 +202,7 @@ public boolean isConnected() { } /** - * Change the frequency threshold for detecting connection used by - * {@link #isConnected()}. + * Change the frequency threshold for detecting connection used by {@link #isConnected()}. * * @param frequency the minimum frequency in Hz. */ @@ -228,12 +217,10 @@ public void setConnectedFrequencyThreshold(int frequency) { /** * Sets the assumed frequency of the connected device. * - *

- * 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. + *

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 */ diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java index 70b3c4cf5cf..75216ec2c5b 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java @@ -9,10 +9,7 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -/** - * This example shows how to use a duty cycle encoder for devices such as an - * arm or elevator. - */ +/** This example shows how to use a duty cycle encoder for devices such as an arm or elevator. */ public class Robot extends TimedRobot { private DutyCycleEncoder m_dutyCycleEncoder; private final double m_fullRange = 1.3; @@ -28,7 +25,6 @@ public void robotInit() { // is the value to enter for the 3rd parameter. m_dutyCycleEncoder = new DutyCycleEncoder(0, m_fullRange, m_expectedZero); - // If you know the frequency of your sensor, uncomment the following // method, and set the method to the frequency of your sensor. // This will result in more stable readings from the sensor. @@ -65,7 +61,8 @@ public void robotPeriodic() { // This does not change where "0" is, so no calibration numbers need // to be changed. double percentOfRange = m_fullRange * 0.1; - double shiftedOutput = MathUtil.inputModulus(output, 0 - percentOfRange, m_fullRange - percentOfRange); + double shiftedOutput = + MathUtil.inputModulus(output, 0 - percentOfRange, m_fullRange - percentOfRange); SmartDashboard.putBoolean("Connected", connected); SmartDashboard.putNumber("Frequency", frequency); From 413b9459f01cfe7fccf1bc35eed096b641745a33 Mon Sep 17 00:00:00 2001 From: Thad House Date: Sat, 2 Mar 2024 19:03:42 -0800 Subject: [PATCH 09/30] Analog Encoder Destructor --- wpilibc/src/main/native/cpp/AnalogEncoder.cpp | 2 ++ wpilibc/src/main/native/include/frc/AnalogEncoder.h | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp index 1de52f86624..49da8c0fdb8 100644 --- a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp +++ b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp @@ -15,6 +15,8 @@ using namespace frc; +AnalogEncoder::~AnalogEncoder() {} + AnalogEncoder::AnalogEncoder(int channel) : AnalogEncoder(std::make_shared(channel)) {} diff --git a/wpilibc/src/main/native/include/frc/AnalogEncoder.h b/wpilibc/src/main/native/include/frc/AnalogEncoder.h index 5162ce17eee..d31952804dc 100644 --- a/wpilibc/src/main/native/include/frc/AnalogEncoder.h +++ b/wpilibc/src/main/native/include/frc/AnalogEncoder.h @@ -81,7 +81,7 @@ class AnalogEncoder : public wpi::Sendable, AnalogEncoder(std::shared_ptr analogInput, double fullRange, double expectedZero); - ~AnalogEncoder() override = default; + ~AnalogEncoder() override; AnalogEncoder(AnalogEncoder&&) = default; AnalogEncoder& operator=(AnalogEncoder&&) = default; From bdceac9a677b91742687e7c486b96e45b7bd9bca Mon Sep 17 00:00:00 2001 From: Thad House Date: Sat, 2 Mar 2024 19:15:54 -0800 Subject: [PATCH 10/30] Docs --- .../main/native/include/frc/AnalogEncoder.h | 29 ++++++++++++++ .../native/include/frc/DutyCycleEncoder.h | 38 ++++++++++++++++++ .../edu/wpi/first/wpilibj/AnalogEncoder.java | 39 ++++++++++++++++--- .../wpi/first/wpilibj/DutyCycleEncoder.java | 19 ++++++++- 4 files changed, 119 insertions(+), 6 deletions(-) diff --git a/wpilibc/src/main/native/include/frc/AnalogEncoder.h b/wpilibc/src/main/native/include/frc/AnalogEncoder.h index d31952804dc..0c2bedbdd04 100644 --- a/wpilibc/src/main/native/include/frc/AnalogEncoder.h +++ b/wpilibc/src/main/native/include/frc/AnalogEncoder.h @@ -93,8 +93,21 @@ class AnalogEncoder : public wpi::Sendable, */ double Get() const; + /** + * 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. + * + * @param min minimum voltage percentage (0-1 range) + * @param max maximum voltage percentage (0-1 range) + */ void SetVoltagePercentageRange(double min, double max); + /** + * Set if this encoder is inverted. + * + * @param inverted true to invert the encoder, false otherwise + */ void SetInverted(bool inverted); /** @@ -106,7 +119,23 @@ class AnalogEncoder : public wpi::Sendable, void InitSendable(wpi::SendableBuilder& builder) override; + /** + * Configures if this encoder has rollover counting enabled. + * + *

By default, the encoder will not count rollovers. This + * behavior is very rarely needed, and is usually a sign you are + * using the wrong encoder type. + * + * @param enable True to enable rollover counting, false to disable. + */ void ConfigureRolloverSupport(bool enable); + + /** + * Reset the number of rollovers that have been counted. + * + *

This has no effect unless configureRolloverSupport(true) + * has been called. + */ void ResetRollovers(); private: diff --git a/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h b/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h index 10819387372..9587f57db78 100644 --- a/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h +++ b/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h @@ -27,6 +27,8 @@ class DutyCycleEncoder : public wpi::Sendable, public: /** * Construct a new DutyCycleEncoder on a specific channel. + * + *

This has a fullRange of 1 and an expectedZero of 0. * * @param channel the channel to attach to */ @@ -35,6 +37,8 @@ class DutyCycleEncoder : public wpi::Sendable, /** * Construct a new DutyCycleEncoder attached to an existing DutyCycle object. * + *

This has a fullRange of 1 and an expectedZero of 0. + * * @param dutyCycle the duty cycle to attach to */ explicit DutyCycleEncoder(DutyCycle& dutyCycle); @@ -42,6 +46,8 @@ class DutyCycleEncoder : public wpi::Sendable, /** * Construct a new DutyCycleEncoder attached to an existing DutyCycle object. * + *

This has a fullRange of 1 and an expectedZero of 0. + * * @param dutyCycle the duty cycle to attach to */ explicit DutyCycleEncoder(DutyCycle* dutyCycle); @@ -49,6 +55,8 @@ class DutyCycleEncoder : public wpi::Sendable, /** * Construct a new DutyCycleEncoder attached to an existing DutyCycle object. * + *

This has a fullRange of 1 and an expectedZero of 0. + * * @param dutyCycle the duty cycle to attach to */ explicit DutyCycleEncoder(std::shared_ptr dutyCycle); @@ -56,6 +64,8 @@ class DutyCycleEncoder : public wpi::Sendable, /** * Construct a new DutyCycleEncoder attached to a DigitalSource object. * + *

This has a fullRange of 1 and an expectedZero of 0. + * * @param digitalSource the digital source to attach to */ explicit DutyCycleEncoder(DigitalSource& digitalSource); @@ -63,6 +73,8 @@ class DutyCycleEncoder : public wpi::Sendable, /** * Construct a new DutyCycleEncoder attached to a DigitalSource object. * + *

This has a fullRange of 1 and an expectedZero of 0. + * * @param digitalSource the digital source to attach to */ explicit DutyCycleEncoder(DigitalSource* digitalSource); @@ -70,6 +82,8 @@ class DutyCycleEncoder : public wpi::Sendable, /** * Construct a new DutyCycleEncoder attached to a DigitalSource object. * + *

This has a fullRange of 1 and an expectedZero of 0. + * * @param digitalSource the digital source to attach to */ explicit DutyCycleEncoder(std::shared_ptr digitalSource); @@ -78,6 +92,8 @@ class DutyCycleEncoder : public wpi::Sendable, * 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() */ DutyCycleEncoder(int channel, double fullRange, double expectedZero); @@ -85,6 +101,8 @@ class DutyCycleEncoder : public wpi::Sendable, * 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() */ DutyCycleEncoder(DutyCycle& dutyCycle, double fullRange, double expectedZero); @@ -92,6 +110,8 @@ class DutyCycleEncoder : public wpi::Sendable, * 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() */ DutyCycleEncoder(DutyCycle* dutyCycle, double fullRange, double expectedZero); @@ -99,6 +119,8 @@ class DutyCycleEncoder : public wpi::Sendable, * 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() */ DutyCycleEncoder(std::shared_ptr dutyCycle, double fullRange, double expectedZero); @@ -107,6 +129,8 @@ class DutyCycleEncoder : public wpi::Sendable, * Construct a new DutyCycleEncoder attached to a DigitalSource object. * * @param digitalSource 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() */ DutyCycleEncoder(DigitalSource& digitalSource, double fullRange, double expectedZero); @@ -115,6 +139,8 @@ class DutyCycleEncoder : public wpi::Sendable, * Construct a new DutyCycleEncoder attached to a DigitalSource object. * * @param digitalSource 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() */ DutyCycleEncoder(DigitalSource* digitalSource, double fullRange, double expectedZero); @@ -123,6 +149,8 @@ class DutyCycleEncoder : public wpi::Sendable, * Construct a new DutyCycleEncoder attached to a DigitalSource object. * * @param digitalSource 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() */ DutyCycleEncoder(std::shared_ptr digitalSource, double fullRange, double expectedZero); @@ -180,8 +208,18 @@ class DutyCycleEncoder : public wpi::Sendable, */ void SetDutyCycleRange(double min, double max); + /** + * Set if this encoder is inverted. + * + * @param inverted true to invert the encoder, false otherwise + */ void SetAssumedFrequency(units::hertz_t frequency); + /** + * Set if this encoder is inverted. + * + * @param inverted true to invert the encoder, false otherwise + */ void SetInverted(bool inverted); /** diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java index 8a76d3e7c14..0aabde26e8b 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java @@ -34,6 +34,8 @@ public class AnalogEncoder implements Sendable, AutoCloseable { * 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, double fullRange, double expectedZero) { this(new AnalogInput(channel), fullRange, expectedZero); @@ -44,6 +46,8 @@ public AnalogEncoder(int channel, double fullRange, double expectedZero) { * 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) { @@ -53,6 +57,8 @@ public AnalogEncoder(AnalogInput analogInput, double fullRange, double expectedZ /** * Construct a new AnalogEncoder attached to a specific AnalogIn channel. + * + *

This has a fullRange of 1 and an expectedZero of 0. * * @param channel the analog input channel to attach to */ @@ -62,6 +68,8 @@ public AnalogEncoder(int channel) { /** * Construct a new AnalogEncoder attached to a specific AnalogInput. + * + *

This has a fullRange of 1 and an expectedZero of 0. * * @param analogInput the analog input to attach to */ @@ -96,11 +104,12 @@ private double mapSensorRange(double pos) { } /** - * Get the encoder value since the last reset. + * Get the encoder value. * - *

This is reported in rotations since the last reset. + *

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) { @@ -127,6 +136,11 @@ public void setVoltagePercentageRange(double min, double max) { m_sensorMax = MathUtil.clamp(max, 0.0, 1.0); } + /** + * Set if this encoder is inverted. + * + * @param inverted true to invert the encoder, false otherwise + */ public void setInverted(boolean inverted) { m_isInverted = inverted; } @@ -150,7 +164,16 @@ public void close() { } } - public void configureRolloverSupport(boolean enable) { + /** + * Configures if this encoder has rollover counting enabled. + * + *

By default, the encoder will not count rollovers. This + * behavior is very rarely needed, and is usually a sign you are + * using the wrong encoder type. + * + * @param enable True to enable rollover counting, false to disable. + */ + public void configureRolloverCounting(boolean enable) { if (enable && m_rolloverCounter == null) { m_rolloverTrigger = new AnalogTrigger(m_analogInput); m_rolloverTrigger.setLimitsVoltage(1.25, 3.75); @@ -166,7 +189,13 @@ public void configureRolloverSupport(boolean enable) { } } - public void resetRollovers() { + /** + * Reset the number of rollovers that have been counted. + * + *

This has no effect unless configureRolloverSupport(true) + * has been called. + */ + public void resetRolloverCount() { if (m_rolloverCounter != null) { m_rolloverCounter.reset(); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java index 1ff3d1beaff..76e82b54ba4 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java @@ -36,6 +36,8 @@ public class DutyCycleEncoder implements Sendable, AutoCloseable { * 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, double fullRange, double expectedZero) { @@ -49,6 +51,8 @@ public DutyCycleEncoder(int channel, double fullRange, double 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, double fullRange, double expectedZero) { @@ -60,6 +64,8 @@ public DutyCycleEncoder(DutyCycle dutyCycle, double fullRange, double expectedZe * 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) { @@ -71,6 +77,8 @@ public DutyCycleEncoder(DigitalSource source, double fullRange, double expectedZ /** * Construct a new DutyCycleEncoder on a specific channel. * + *

This has a fullRange of 1 and an expectedZero of 0. + * * @param channel the channel to attach to */ @SuppressWarnings("this-escape") @@ -81,6 +89,8 @@ public DutyCycleEncoder(int channel) { /** * Construct a new DutyCycleEncoder attached to an existing DutyCycle object. * + *

This has a fullRange of 1 and an expectedZero of 0. + * * @param dutyCycle the duty cycle to attach to */ @SuppressWarnings("this-escape") @@ -90,6 +100,8 @@ public DutyCycleEncoder(DutyCycle dutyCycle) { /** * Construct a new DutyCycleEncoder attached to a DigitalSource object. + * + *

This has a fullRange of 1 and an expectedZero of 0. * * @param source the digital source to attach to */ @@ -222,7 +234,7 @@ public void setConnectedFrequencyThreshold(int frequency) { * frequency of the sensor to this function. This will use that frequency to compute the DutyCycle * percentage, rather than the computed frequency. * - * @param frequency + * @param frequency the assumed frequency of the sensor */ public void setAssumedFrequency(double frequency) { if (frequency == 0.0) { @@ -232,6 +244,11 @@ public void setAssumedFrequency(double frequency) { } } + /** + * Set if this encoder is inverted. + * + * @param inverted true to invert the encoder, false otherwise + */ public void setInverted(boolean inverted) { m_isInverted = inverted; } From 88298eeec0c2f1fd7bd508962a221fbd25a2a562 Mon Sep 17 00:00:00 2001 From: Thad House Date: Sat, 2 Mar 2024 19:23:55 -0800 Subject: [PATCH 11/30] Docs and format --- myRobot/src/main/java/frc/robot/MyRobot.java | 20 ++----------------- .../main/native/include/frc/AnalogEncoder.h | 16 +++++++++++++++ .../native/include/frc/DutyCycleEncoder.h | 18 ++++++++--------- .../include/frc/simulation/AnalogEncoderSim.h | 2 +- .../frc/simulation/DutyCycleEncoderSim.h | 2 +- .../edu/wpi/first/wpilibj/AnalogEncoder.java | 12 +++++------ .../wpi/first/wpilibj/DutyCycleEncoder.java | 8 ++++---- 7 files changed, 39 insertions(+), 39 deletions(-) diff --git a/myRobot/src/main/java/frc/robot/MyRobot.java b/myRobot/src/main/java/frc/robot/MyRobot.java index 5772b1056c5..a5f88bd3f36 100644 --- a/myRobot/src/main/java/frc/robot/MyRobot.java +++ b/myRobot/src/main/java/frc/robot/MyRobot.java @@ -4,23 +4,15 @@ package frc.robot; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class MyRobot extends TimedRobot { - DutyCycleEncoder pot; - /** * This function is run when the robot is first started up and should be used for any * initialization code. */ @Override - public void robotInit() { - pot = new DutyCycleEncoder(0, 3, 0.5); - pot.setAssumedFrequency(967.77); - } + public void robotInit() {} /** This function is run once each time the robot enters autonomous mode. */ @Override @@ -44,13 +36,5 @@ public void testPeriodic() {} /** This function is called periodically during all modes. */ @Override - public void robotPeriodic() { - double val = pot.get() + 1; - SmartDashboard.putNumber("DC4", val); - SmartDashboard.putNumber("DC3", MathUtil.invertInput(val, 1, 4)); - SmartDashboard.putNumber("DC2", MathUtil.inputModulus(pot.get(), -0.1, 0.9)); - SmartDashboard.putNumber( - "DC", MathUtil.inputModulus(MathUtil.invertInput(pot.get(), 0, 1), -0.1, 0.9)); - SmartDashboard.putNumber("Freq", pot.getFrequency()); - } + public void robotPeriodic() {} } diff --git a/wpilibc/src/main/native/include/frc/AnalogEncoder.h b/wpilibc/src/main/native/include/frc/AnalogEncoder.h index 0c2bedbdd04..398f57ce80f 100644 --- a/wpilibc/src/main/native/include/frc/AnalogEncoder.h +++ b/wpilibc/src/main/native/include/frc/AnalogEncoder.h @@ -25,6 +25,8 @@ class AnalogEncoder : public wpi::Sendable, /** * Construct a new AnalogEncoder attached to a specific AnalogIn channel. * + *

This has a fullRange of 1 and an expectedZero of 0. + * * @param channel the analog input channel to attach to */ explicit AnalogEncoder(int channel); @@ -32,6 +34,8 @@ class AnalogEncoder : public wpi::Sendable, /** * Construct a new AnalogEncoder attached to a specific AnalogInput. * + *

This has a fullRange of 1 and an expectedZero of 0. + * * @param analogInput the analog input to attach to */ explicit AnalogEncoder(AnalogInput& analogInput); @@ -39,6 +43,8 @@ class AnalogEncoder : public wpi::Sendable, /** * Construct a new AnalogEncoder attached to a specific AnalogInput. * + *

This has a fullRange of 1 and an expectedZero of 0. + * * @param analogInput the analog input to attach to */ explicit AnalogEncoder(AnalogInput* analogInput); @@ -46,6 +52,8 @@ class AnalogEncoder : public wpi::Sendable, /** * Construct a new AnalogEncoder attached to a specific AnalogInput. * + *

This has a fullRange of 1 and an expectedZero of 0. + * * @param analogInput the analog input to attach to */ explicit AnalogEncoder(std::shared_ptr analogInput); @@ -54,6 +62,8 @@ class AnalogEncoder : public wpi::Sendable, * 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() */ AnalogEncoder(int channel, double fullRange, double expectedZero); @@ -61,6 +71,8 @@ class AnalogEncoder : public wpi::Sendable, * 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() */ AnalogEncoder(AnalogInput& analogInput, double fullRange, double expectedZero); @@ -69,6 +81,8 @@ class AnalogEncoder : public wpi::Sendable, * 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() */ AnalogEncoder(AnalogInput* analogInput, double fullRange, double expectedZero); @@ -77,6 +91,8 @@ class AnalogEncoder : public wpi::Sendable, * 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() */ AnalogEncoder(std::shared_ptr analogInput, double fullRange, double expectedZero); diff --git a/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h b/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h index 9587f57db78..1d35b411d38 100644 --- a/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h +++ b/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h @@ -27,7 +27,7 @@ class DutyCycleEncoder : public wpi::Sendable, public: /** * Construct a new DutyCycleEncoder on a specific channel. - * + * *

This has a fullRange of 1 and an expectedZero of 0. * * @param channel the channel to attach to @@ -38,7 +38,7 @@ class DutyCycleEncoder : public wpi::Sendable, * Construct a new DutyCycleEncoder attached to an existing DutyCycle object. * *

This has a fullRange of 1 and an expectedZero of 0. - * + * * @param dutyCycle the duty cycle to attach to */ explicit DutyCycleEncoder(DutyCycle& dutyCycle); @@ -47,7 +47,7 @@ class DutyCycleEncoder : public wpi::Sendable, * Construct a new DutyCycleEncoder attached to an existing DutyCycle object. * *

This has a fullRange of 1 and an expectedZero of 0. - * + * * @param dutyCycle the duty cycle to attach to */ explicit DutyCycleEncoder(DutyCycle* dutyCycle); @@ -56,7 +56,7 @@ class DutyCycleEncoder : public wpi::Sendable, * Construct a new DutyCycleEncoder attached to an existing DutyCycle object. * *

This has a fullRange of 1 and an expectedZero of 0. - * + * * @param dutyCycle the duty cycle to attach to */ explicit DutyCycleEncoder(std::shared_ptr dutyCycle); @@ -65,7 +65,7 @@ class DutyCycleEncoder : public wpi::Sendable, * Construct a new DutyCycleEncoder attached to a DigitalSource object. * *

This has a fullRange of 1 and an expectedZero of 0. - * + * * @param digitalSource the digital source to attach to */ explicit DutyCycleEncoder(DigitalSource& digitalSource); @@ -74,7 +74,7 @@ class DutyCycleEncoder : public wpi::Sendable, * Construct a new DutyCycleEncoder attached to a DigitalSource object. * *

This has a fullRange of 1 and an expectedZero of 0. - * + * * @param digitalSource the digital source to attach to */ explicit DutyCycleEncoder(DigitalSource* digitalSource); @@ -83,7 +83,7 @@ class DutyCycleEncoder : public wpi::Sendable, * Construct a new DutyCycleEncoder attached to a DigitalSource object. * *

This has a fullRange of 1 and an expectedZero of 0. - * + * * @param digitalSource the digital source to attach to */ explicit DutyCycleEncoder(std::shared_ptr digitalSource); @@ -210,14 +210,14 @@ class DutyCycleEncoder : public wpi::Sendable, /** * Set if this encoder is inverted. - * + * * @param inverted true to invert the encoder, false otherwise */ void SetAssumedFrequency(units::hertz_t frequency); /** * Set if this encoder is inverted. - * + * * @param inverted true to invert the encoder, false otherwise */ void SetInverted(bool inverted); diff --git a/wpilibc/src/main/native/include/frc/simulation/AnalogEncoderSim.h b/wpilibc/src/main/native/include/frc/simulation/AnalogEncoderSim.h index 00b2b876b1c..3cd8f223063 100644 --- a/wpilibc/src/main/native/include/frc/simulation/AnalogEncoderSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/AnalogEncoderSim.h @@ -30,7 +30,7 @@ class AnalogEncoderSim { /** * Set the position. * - * @param angle The position. + * @param value The position. */ void Set(double value); diff --git a/wpilibc/src/main/native/include/frc/simulation/DutyCycleEncoderSim.h b/wpilibc/src/main/native/include/frc/simulation/DutyCycleEncoderSim.h index 06bb1df1a57..e5d84f12fd8 100644 --- a/wpilibc/src/main/native/include/frc/simulation/DutyCycleEncoderSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/DutyCycleEncoderSim.h @@ -42,7 +42,7 @@ class DutyCycleEncoderSim { /** * Set the position. * - * @param turns The position. + * @param value The position. */ void Set(double value); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java index 0aabde26e8b..8668ee914b2 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java @@ -57,7 +57,7 @@ public AnalogEncoder(AnalogInput analogInput, double fullRange, double expectedZ /** * Construct a new AnalogEncoder attached to a specific AnalogIn channel. - * + * *

This has a fullRange of 1 and an expectedZero of 0. * * @param channel the analog input channel to attach to @@ -68,7 +68,7 @@ public AnalogEncoder(int channel) { /** * Construct a new AnalogEncoder attached to a specific AnalogInput. - * + * *

This has a fullRange of 1 and an expectedZero of 0. * * @param analogInput the analog input to attach to @@ -138,7 +138,7 @@ public void setVoltagePercentageRange(double min, double max) { /** * Set if this encoder is inverted. - * + * * @param inverted true to invert the encoder, false otherwise */ public void setInverted(boolean inverted) { @@ -166,11 +166,11 @@ public void close() { /** * Configures if this encoder has rollover counting enabled. - * + * *

By default, the encoder will not count rollovers. This * behavior is very rarely needed, and is usually a sign you are * using the wrong encoder type. - * + * * @param enable True to enable rollover counting, false to disable. */ public void configureRolloverCounting(boolean enable) { @@ -191,7 +191,7 @@ public void configureRolloverCounting(boolean enable) { /** * Reset the number of rollovers that have been counted. - * + * *

This has no effect unless configureRolloverSupport(true) * has been called. */ diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java index 76e82b54ba4..22cf1995c84 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java @@ -78,7 +78,7 @@ public DutyCycleEncoder(DigitalSource source, double fullRange, double expectedZ * Construct a new DutyCycleEncoder on a specific channel. * *

This has a fullRange of 1 and an expectedZero of 0. - * + * * @param channel the channel to attach to */ @SuppressWarnings("this-escape") @@ -90,7 +90,7 @@ public DutyCycleEncoder(int channel) { * Construct a new DutyCycleEncoder attached to an existing DutyCycle object. * *

This has a fullRange of 1 and an expectedZero of 0. - * + * * @param dutyCycle the duty cycle to attach to */ @SuppressWarnings("this-escape") @@ -100,7 +100,7 @@ public DutyCycleEncoder(DutyCycle dutyCycle) { /** * Construct a new DutyCycleEncoder attached to a DigitalSource object. - * + * *

This has a fullRange of 1 and an expectedZero of 0. * * @param source the digital source to attach to @@ -246,7 +246,7 @@ public void setAssumedFrequency(double frequency) { /** * Set if this encoder is inverted. - * + * * @param inverted true to invert the encoder, false otherwise */ public void setInverted(boolean inverted) { From cff871a5aa18167f78e4b25ec205d7148f346017 Mon Sep 17 00:00:00 2001 From: Thad House Date: Sat, 2 Mar 2024 19:46:45 -0800 Subject: [PATCH 12/30] More format --- .../src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java | 2 +- .../edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java | 4 ++-- .../edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java index 8668ee914b2..c5117996809 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java @@ -167,7 +167,7 @@ public void close() { /** * Configures if this encoder has rollover counting enabled. * - *

By default, the encoder will not count rollovers. This + *

By default, the encoder will not count rollovers. This * behavior is very rarely needed, and is usually a sign you are * using the wrong encoder type. * diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java index bead64af562..b4a85f0e8c6 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java @@ -23,9 +23,9 @@ public AnalogEncoderSim(AnalogEncoder encoder) { } /** - * Set the position + * Set the position. * - * @param angle The position. + * @param value The position. */ public void set(double value) { m_simPosition.set(value); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java index 660f09333a7..e8366188558 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java @@ -45,7 +45,7 @@ public double get() { /** * Set the position. * - * @param turns The position. + * @param value The position. */ public void set(double value) { m_simPosition.set(value); From ed0c8079816c8d5ed985af24d543a45c00db065b Mon Sep 17 00:00:00 2001 From: Thad House Date: Sat, 2 Mar 2024 19:53:09 -0800 Subject: [PATCH 13/30] Formatting --- wpilibc/src/main/native/include/frc/DutyCycleEncoder.h | 10 ++++++++-- .../first/wpilibj/examples/dutycycleencoder/Robot.java | 4 ++-- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h b/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h index 1d35b411d38..7ee04c0d9a1 100644 --- a/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h +++ b/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h @@ -209,9 +209,15 @@ class DutyCycleEncoder : public wpi::Sendable, void SetDutyCycleRange(double min, double max); /** - * Set if this encoder is inverted. + * Sets the assumed frequency of the connected device. * - * @param inverted true to invert the encoder, false otherwise + *

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 */ void SetAssumedFrequency(units::hertz_t frequency); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java index 75216ec2c5b..c3015058281 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java @@ -12,8 +12,8 @@ /** This example shows how to use a duty cycle encoder for devices such as an arm or elevator. */ public class Robot extends TimedRobot { private DutyCycleEncoder m_dutyCycleEncoder; - private final double m_fullRange = 1.3; - private final double m_expectedZero = 0; + private static final double m_fullRange = 1.3; + private static final double m_expectedZero = 0; @Override public void robotInit() { From b5e681cb25a270640986a6b1ecd6cdb35368afe8 Mon Sep 17 00:00:00 2001 From: Thad House Date: Sat, 2 Mar 2024 20:06:09 -0800 Subject: [PATCH 14/30] Format --- .../java/edu/wpi/first/wpilibj/AnalogEncoder.java | 12 +++++------- .../src/main/java/edu/wpi/first/math/MathUtil.java | 4 ---- 2 files changed, 5 insertions(+), 11 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java index c5117996809..608b530bf63 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java @@ -106,8 +106,8 @@ private double mapSensorRange(double pos) { /** * Get the encoder value. * - *

By default, this will not count rollovers. If that behavior - * is necessary, call configureRolloverCounting(true). + *

By default, this will not count rollovers. If that behavior is necessary, call + * configureRolloverCounting(true). * * @return the encoder value */ @@ -167,9 +167,8 @@ public void close() { /** * Configures if this encoder has rollover counting enabled. * - *

By default, the encoder will not count rollovers. This - * behavior is very rarely needed, and is usually a sign you are - * using the wrong encoder type. + *

By default, the encoder will not count rollovers. This behavior is very rarely needed, and + * is usually a sign you are using the wrong encoder type. * * @param enable True to enable rollover counting, false to disable. */ @@ -192,8 +191,7 @@ public void configureRolloverCounting(boolean enable) { /** * Reset the number of rollovers that have been counted. * - *

This has no effect unless configureRolloverSupport(true) - * has been called. + *

This has no effect unless configureRolloverSupport(true) has been called. */ public void resetRolloverCount() { if (m_rolloverCounter != null) { diff --git a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java index 64fbf978be5..044cddfab68 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java +++ b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java @@ -125,10 +125,6 @@ public static double inputModulus(double input, double minimumInput, double maxi return input; } - public static double invertInput(double input, double minimumInput, double maximumInput) { - return maximumInput - input + minimumInput; - } - /** * Wraps an angle to the range -pi to pi radians. * From 0c6b352b8955472fc28c6b802f929ffac9717976 Mon Sep 17 00:00:00 2001 From: Thad House Date: Thu, 7 Mar 2024 17:19:31 -0800 Subject: [PATCH 15/30] Completely remove rollover --- wpilibc/src/main/native/cpp/AnalogEncoder.cpp | 94 +++------------ .../main/native/include/frc/AnalogEncoder.h | 28 ----- .../edu/wpi/first/wpilibj/AnalogEncoder.java | 110 +++--------------- 3 files changed, 30 insertions(+), 202 deletions(-) diff --git a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp index 49da8c0fdb8..152b34f98c5 100644 --- a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp +++ b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp @@ -8,7 +8,6 @@ #include #include "frc/AnalogInput.h" -#include "frc/Counter.h" #include "frc/Errors.h" #include "frc/MathUtil.h" #include "frc/RobotController.h" @@ -76,12 +75,22 @@ double AnalogEncoder::Get() const { return m_simPosition.Get(); } - if (m_rolloverCounter) { - return GetWithRollovers(); - } else { - double analog = m_analogInput->GetVoltage(); - return GetWithoutRollovers(analog); + 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 = InputModulus(pos, 0.0, m_fullRange); + // Invert if necessary + if (m_isInverted) { + return m_fullRange - result; } + return result; } void AnalogEncoder::SetVoltagePercentageRange(double min, double max) { @@ -113,76 +122,3 @@ void AnalogEncoder::InitSendable(wpi::SendableBuilder& builder) { builder.AddDoubleProperty( "Position", [this] { return this->Get(); }, nullptr); } - -void AnalogEncoder::ConfigureRolloverSupport(bool enable) { - if (enable && !m_rolloverCounter) { - m_rolloverTrigger = std::make_unique(m_analogInput.get()); - m_rolloverTrigger->SetLimitsVoltage(1.25, 3.75); - m_rolloverCounter = std::make_unique(); - m_rolloverCounter->SetUpSource( - m_rolloverTrigger->CreateOutput(AnalogTriggerType::kRisingPulse)); - m_rolloverCounter->SetDownSource( - m_rolloverTrigger->CreateOutput(AnalogTriggerType::kFallingPulse)); - } else if (!enable && m_rolloverCounter) { - m_rolloverCounter = nullptr; - m_rolloverTrigger = nullptr; - } -} - -void AnalogEncoder::ResetRollovers() { - if (m_rolloverCounter) { - m_rolloverCounter->Reset(); - } -} - -double AnalogEncoder::GetWithoutRollovers(double analog) const { - 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 = InputModulus(pos, 0.0, m_fullRange); - // Invert if necessary - if (m_isInverted) { - return m_fullRange - result; - } - return result; -} - -static bool DoubleEquals(double a, double b) { - constexpr double epsilon = 0.00001; - return std::abs(a - b) < epsilon; -} - -double AnalogEncoder::GetWithRollovers() const { - // 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++) { - auto counter = m_rolloverCounter->Get(); - auto pos = m_analogInput->GetVoltage(); - auto counter2 = m_rolloverCounter->Get(); - auto pos2 = m_analogInput->GetVoltage(); - if (counter == counter2 && DoubleEquals(pos, pos2)) { - pos = GetWithoutRollovers(pos); - - if (m_isInverted) { - pos = pos - counter; - } else { - pos = pos + counter; - } - - m_lastPosition = pos; - return pos; - } - } - - FRC_ReportError( - warn::Warning, - "Failed to read Analog Encoder. Potential Speed Overrun. Returning last " - "value"); - return m_lastPosition; -} diff --git a/wpilibc/src/main/native/include/frc/AnalogEncoder.h b/wpilibc/src/main/native/include/frc/AnalogEncoder.h index 398f57ce80f..8baf4910137 100644 --- a/wpilibc/src/main/native/include/frc/AnalogEncoder.h +++ b/wpilibc/src/main/native/include/frc/AnalogEncoder.h @@ -13,8 +13,6 @@ namespace frc { class AnalogInput; -class Counter; -class AnalogTrigger; /** * Class for supporting continuous analog encoders, such as the US Digital MA3. @@ -135,32 +133,10 @@ class AnalogEncoder : public wpi::Sendable, void InitSendable(wpi::SendableBuilder& builder) override; - /** - * Configures if this encoder has rollover counting enabled. - * - *

By default, the encoder will not count rollovers. This - * behavior is very rarely needed, and is usually a sign you are - * using the wrong encoder type. - * - * @param enable True to enable rollover counting, false to disable. - */ - void ConfigureRolloverSupport(bool enable); - - /** - * Reset the number of rollovers that have been counted. - * - *

This has no effect unless configureRolloverSupport(true) - * has been called. - */ - void ResetRollovers(); - private: void Init(double fullRange, double expectedZero); double MapSensorRange(double pos) const; - double GetWithoutRollovers(double analog) const; - double GetWithRollovers() const; - std::shared_ptr m_analogInput; double m_fullRange; double m_expectedZero; @@ -168,10 +144,6 @@ class AnalogEncoder : public wpi::Sendable, double m_sensorMax{1.0}; bool m_isInverted{false}; - std::unique_ptr m_rolloverTrigger{nullptr}; - std::unique_ptr m_rolloverCounter{nullptr}; - mutable double m_lastPosition{0.0}; - hal::SimDevice m_simDevice; hal::SimDouble m_simPosition; }; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java index 608b530bf63..4a896126ca2 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java @@ -11,7 +11,6 @@ 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 { @@ -23,10 +22,6 @@ public class AnalogEncoder implements Sendable, AutoCloseable { private double m_sensorMax = 1.0; private boolean m_isInverted; - private AnalogTrigger m_rolloverTrigger; - private Counter m_rolloverCounter; - private double m_lastPosition; - private SimDevice m_simDevice; private SimDouble m_simPosition; @@ -116,12 +111,22 @@ public double get() { return m_simPosition.get(); } - if (m_rolloverCounter != null) { - return getWithRollovers(); - } else { - double analog = m_analogInput.getVoltage(); - return getWithoutRollovers(analog); + 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; } + return result; } /** @@ -164,91 +169,6 @@ public void close() { } } - /** - * Configures if this encoder has rollover counting enabled. - * - *

By default, the encoder will not count rollovers. This behavior is very rarely needed, and - * is usually a sign you are using the wrong encoder type. - * - * @param enable True to enable rollover counting, false to disable. - */ - public void configureRolloverCounting(boolean enable) { - if (enable && m_rolloverCounter == null) { - m_rolloverTrigger = new AnalogTrigger(m_analogInput); - m_rolloverTrigger.setLimitsVoltage(1.25, 3.75); - m_rolloverCounter = new Counter(); - m_rolloverCounter.setUpSource(m_rolloverTrigger.createOutput(AnalogTriggerType.kRisingPulse)); - m_rolloverCounter.setDownSource( - m_rolloverTrigger.createOutput(AnalogTriggerType.kFallingPulse)); - } else if (!enable && m_rolloverCounter != null) { - m_rolloverTrigger.close(); - m_rolloverTrigger = null; - m_rolloverCounter.close(); - m_rolloverCounter = null; - } - } - - /** - * Reset the number of rollovers that have been counted. - * - *

This has no effect unless configureRolloverSupport(true) has been called. - */ - public void resetRolloverCount() { - if (m_rolloverCounter != null) { - m_rolloverCounter.reset(); - } - } - - private boolean doubleEquals(double a, double b) { - double epsilon = 0.00001d; - return Math.abs(a - b) < epsilon; - } - - private double getWithRollovers() { - // 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_rolloverCounter.get(); - double pos = m_analogInput.getVoltage(); - double counter2 = m_rolloverCounter.get(); - double pos2 = m_analogInput.getVoltage(); - if (counter == counter2 && doubleEquals(pos, pos2)) { - pos = getWithoutRollovers(pos); - - if (m_isInverted) { - pos = pos - counter; - } else { - pos = pos + counter; - } - - m_lastPosition = pos; - return pos; - } - } - - DriverStation.reportWarning( - "Failed to read Analog Encoder. Potential Speed Overrun. Returning last value", false); - return m_lastPosition; - } - - private double getWithoutRollovers(double analog) { - 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; - } - return result; - } - @Override public void initSendable(SendableBuilder builder) { builder.setSmartDashboardType("AbsoluteEncoder"); From 0889fec1f61cd07b3f789ed656c88349c5ff4e72 Mon Sep 17 00:00:00 2001 From: Thad House Date: Sun, 25 Feb 2024 13:47:04 -0800 Subject: [PATCH 16/30] Deprecate AbsoluteEncoder classes, Replace DCE with DutyCyclePotentiometer --- myRobot/src/main/java/frc/robot/MyRobot.java | 13 +- .../edu/wpi/first/wpilibj/AnalogEncoder.java | 8 +- .../first/wpilibj/AnalogPotentiometer.java | 174 +++++++-- .../wpi/first/wpilibj/DutyCycleEncoder.java | 4 + .../first/wpilibj/DutyCyclePotentiometer.java | 330 ++++++++++++++++++ .../wpilibj/simulation/AnalogEncoderSim.java | 2 + .../simulation/DutyCycleEncoderSim.java | 2 + 7 files changed, 498 insertions(+), 35 deletions(-) create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCyclePotentiometer.java diff --git a/myRobot/src/main/java/frc/robot/MyRobot.java b/myRobot/src/main/java/frc/robot/MyRobot.java index a5f88bd3f36..5a9a56c493c 100644 --- a/myRobot/src/main/java/frc/robot/MyRobot.java +++ b/myRobot/src/main/java/frc/robot/MyRobot.java @@ -4,15 +4,21 @@ package frc.robot; +import edu.wpi.first.wpilibj.DutyCyclePotentiometer; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class MyRobot extends TimedRobot { + DutyCyclePotentiometer pot; /** * This function is run when the robot is first started up and should be used for any * initialization code. */ @Override - public void robotInit() {} + public void robotInit() { + pot = new DutyCyclePotentiometer(0); + pot.setAssumedFrequency(967.77); + } /** This function is run once each time the robot enters autonomous mode. */ @Override @@ -36,5 +42,8 @@ public void testPeriodic() {} /** This function is called periodically during all modes. */ @Override - public void robotPeriodic() {} + public void robotPeriodic() { + SmartDashboard.putNumber("DC", pot.get()); + SmartDashboard.putNumber("Freq", pot.getFrequency()); + } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java index 727ad2cd3de..767815bfd32 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java @@ -13,7 +13,13 @@ 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. */ +/** + * Class for supporting continuous analog encoders, such as the US Digital MA3. + * + * @deprecated Use AnalogPotentiometer instead. If rollover support is necessary, + * use enableRolloverSupport() on AnalogPotentiometer. + */ +@Deprecated(since = "2025", forRemoval = true) public class AnalogEncoder implements Sendable, AutoCloseable { private final AnalogInput m_analogInput; private AnalogTrigger m_analogTrigger; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java index 15473495528..19e5ec4b87d 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java @@ -4,13 +4,20 @@ package edu.wpi.first.wpilibj; +import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; +import edu.wpi.first.hal.SimDevice; +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.hal.SimDevice.Direction; 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 reading analog potentiometers. Analog potentiometers read in an analog voltage that - * corresponds to a position. The position is in whichever units you choose, by way of the scaling + * Class for reading analog potentiometers. Analog potentiometers read in an + * analog voltage that + * corresponds to a position. The position is in whichever units you choose, by + * way of the scaling * and offset constants passed to the constructor. */ public class AnalogPotentiometer implements Sendable, AutoCloseable { @@ -18,20 +25,42 @@ public class AnalogPotentiometer implements Sendable, AutoCloseable { private boolean m_initAnalogInput; private double m_fullRange; private double m_offset; + private AnalogTrigger m_rotationTrigger; + private Counter m_rotationCounter; + private double m_lastPosition; + + private SimDevice m_simDevice; + private SimDouble m_simPosition; + + private void init() { + m_simDevice = SimDevice.create("AnalogPotentiometer", m_analogInput.getChannel()); + + if (m_simDevice != null) { + m_simPosition = m_simDevice.createDouble("Position", Direction.kInput, 0.0); + } + } /** * AnalogPotentiometer constructor. * - *

Use the fullRange and offset values so that the output produces meaningful values. I.E: you - * have a 270 degree potentiometer, and you want the output to be degrees with the halfway point - * as 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 since the halfway - * point after scaling is 135 degrees. This will calculate the result from the fullRange times the + *

+ * Use the fullRange and offset values so that the output produces meaningful + * values. I.E: you + * have a 270 degree potentiometer, and you want the output to be degrees with + * the halfway point + * as 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 + * since the halfway + * point after scaling is 135 degrees. This will calculate the result from the + * fullRange times the * fraction of the supply voltage, plus the offset. * - * @param channel The analog input channel this potentiometer is plugged into. 0-3 are on-board - * and 4-7 are on the MXP port. - * @param fullRange The scaling to multiply the fraction by to get a meaningful unit. - * @param offset The offset to add to the scaled value for controlling the zero value + * @param channel The analog input channel this potentiometer is plugged into. + * 0-3 are on-board + * and 4-7 are on the MXP port. + * @param fullRange The scaling to multiply the fraction by to get a meaningful + * unit. + * @param offset The offset to add to the scaled value for controlling the + * zero value */ @SuppressWarnings("this-escape") public AnalogPotentiometer(final int channel, double fullRange, double offset) { @@ -43,37 +72,51 @@ public AnalogPotentiometer(final int channel, double fullRange, double offset) { /** * AnalogPotentiometer constructor. * - *

Use the fullRange and offset values so that the output produces meaningful values. I.E: you - * have a 270 degree potentiometer, and you want the output to be degrees with the halfway point - * as 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 since the halfway - * point after scaling is 135 degrees. This will calculate the result from the fullRange times the + *

+ * Use the fullRange and offset values so that the output produces meaningful + * values. I.E: you + * have a 270 degree potentiometer, and you want the output to be degrees with + * the halfway point + * as 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 + * since the halfway + * point after scaling is 135 degrees. This will calculate the result from the + * fullRange times the * fraction of the supply voltage, plus the offset. * - * @param input The {@link AnalogInput} this potentiometer is plugged into. - * @param fullRange The angular value (in desired units) representing the full 0-5V range of the - * input. - * @param offset The angular value (in desired units) representing the angular output at 0V. + * @param input The {@link AnalogInput} this potentiometer is plugged into. + * @param fullRange The angular value (in desired units) representing the full + * 0-5V range of the + * input. + * @param offset The angular value (in desired units) representing the + * angular output at 0V. */ @SuppressWarnings("this-escape") public AnalogPotentiometer(final AnalogInput input, double fullRange, double offset) { SendableRegistry.addLW(this, "AnalogPotentiometer", input.getChannel()); - m_analogInput = input; + m_analogInput = requireNonNullParam(input, "input", "AnalogPotentiometer"); m_initAnalogInput = false; m_fullRange = fullRange; m_offset = offset; + + init(); } /** * AnalogPotentiometer constructor. * - *

Use the scale value so that the output produces meaningful values. I.E: you have a 270 - * degree potentiometer, and you want the output to be degrees with the starting point as 0 + *

+ * Use the scale value so that the output produces meaningful values. I.E: you + * have a 270 + * degree potentiometer, and you want the output to be degrees with the starting + * point as 0 * degrees. The scale value is 270.0(degrees). * - * @param channel The analog input channel this potentiometer is plugged into. 0-3 are on-board - * and 4-7 are on the MXP port. - * @param scale The scaling to multiply the voltage by to get a meaningful unit. + * @param channel The analog input channel this potentiometer is plugged into. + * 0-3 are on-board + * and 4-7 are on the MXP port. + * @param scale The scaling to multiply the voltage by to get a meaningful + * unit. */ public AnalogPotentiometer(final int channel, double scale) { this(channel, scale, 0); @@ -82,8 +125,11 @@ public AnalogPotentiometer(final int channel, double scale) { /** * AnalogPotentiometer constructor. * - *

Use the fullRange and offset values so that the output produces meaningful values. I.E: you - * have a 270 degree potentiometer, and you want the output to be degrees with the starting point + *

+ * Use the fullRange and offset values so that the output produces meaningful + * values. I.E: you + * have a 270 degree potentiometer, and you want the output to be degrees with + * the starting point * as 0 degrees. The scale value is 270.0(degrees). * * @param input The {@link AnalogInput} this potentiometer is plugged into. @@ -96,10 +142,12 @@ public AnalogPotentiometer(final AnalogInput input, double scale) { /** * AnalogPotentiometer constructor. * - *

The potentiometer will return a value between 0 and 1.0. + *

+ * The potentiometer will return a value between 0 and 1.0. * - * @param channel The analog input channel this potentiometer is plugged into. 0-3 are on-board - * and 4-7 are on the MXP port. + * @param channel The analog input channel this potentiometer is plugged into. + * 0-3 are on-board + * and 4-7 are on the MXP port. */ public AnalogPotentiometer(final int channel) { this(channel, 1, 0); @@ -108,7 +156,8 @@ public AnalogPotentiometer(final int channel) { /** * AnalogPotentiometer constructor. * - *

The potentiometer will return a value between 0 and 1.0. + *

+ * The potentiometer will return a value between 0 and 1.0. * * @param input The {@link AnalogInput} this potentiometer is plugged into. */ @@ -116,19 +165,73 @@ public AnalogPotentiometer(final AnalogInput input) { this(input, 1, 0); } + private boolean doubleEquals(double a, double b) { + double epsilon = 0.00001d; + return Math.abs(a - b) < epsilon; + } + + private double getRollovers() { + 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_rotationCounter.get(); + double pos = m_analogInput.getVoltage(); + double counter2 = m_rotationCounter.get(); + double pos2 = m_analogInput.getVoltage(); + if (counter == counter2 && doubleEquals(pos, pos2)) { + pos = pos / RobotController.getVoltage5V(); + double position = (counter + pos) * m_fullRange + m_offset; + m_lastPosition = position; + return position; + } + } + + DriverStation.reportWarning( + "Failed to read Analog Encoder. Potential Speed Overrun. Returning last value", false); + return m_lastPosition; + } + /** * Get the current reading of the potentiometer. * * @return The current position of the potentiometer. */ public double get() { - if (m_analogInput == null) { - return m_offset; + if (m_simPosition != null) { + return m_simPosition.get(); + } + + if (m_rotationCounter != null) { + return getRollovers(); } + return (m_analogInput.getAverageVoltage() / RobotController.getVoltage5V()) * m_fullRange + m_offset; } + public void enableRolloverSupport() { + if (m_rotationCounter != null) { + return; + } + m_rotationTrigger = new AnalogTrigger(m_analogInput); + m_rotationCounter = new Counter(); + + // Limits need to be 25% from each end + m_rotationTrigger.setLimitsVoltage(1.25, 3.75); + m_rotationCounter.setUpSource(m_rotationTrigger, AnalogTriggerType.kRisingPulse); + m_rotationCounter.setDownSource(m_rotationTrigger, AnalogTriggerType.kFallingPulse); + } + + public void resetRollovers() { + if (m_rotationCounter != null) { + m_rotationCounter.reset(); + } + } + @Override public void initSendable(SendableBuilder builder) { if (m_analogInput != null) { @@ -140,10 +243,17 @@ public void initSendable(SendableBuilder builder) { @Override public void close() { SendableRegistry.remove(this); + if (m_rotationCounter != null) { + m_rotationCounter.close(); + m_rotationTrigger.close(); + m_rotationCounter = null; + m_rotationTrigger = null; + } if (m_initAnalogInput) { m_analogInput.close(); m_analogInput = null; m_initAnalogInput = false; } + } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java index a0858cef922..c9c7383cce3 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java @@ -16,7 +16,11 @@ /** * Class for supporting duty cycle/PWM encoders, such as the US Digital MA3 with PWM Output, the * CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag Encoder. + * + * @deprecated Use DutyCyclePotentiometer instead. Rollover support is not supported for + * duty cycle encoders, there is no reliable way to do so. */ +@Deprecated(since = "2025", forRemoval = true) public class DutyCycleEncoder implements Sendable, AutoCloseable { private final DutyCycle m_dutyCycle; private boolean m_ownsDutyCycle; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCyclePotentiometer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCyclePotentiometer.java new file mode 100644 index 00000000000..81268d12e51 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCyclePotentiometer.java @@ -0,0 +1,330 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj; + +import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; + +import edu.wpi.first.hal.SimBoolean; +import edu.wpi.first.hal.SimDevice; +import edu.wpi.first.hal.SimDevice.Direction; +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.util.sendable.SendableRegistry; + +/** + * Class for reading duty cyle potentiometers. Duty cycle potentiometers read in + * an pwm signal that + * corresponds to a position. The position is in whichever units you choose, by + * way of the scaling + * and offset constants passed to the constructor. + */ +public class DutyCyclePotentiometer implements Sendable, AutoCloseable { + private DutyCycle m_dutyCycle; + private DigitalSource m_digitalSource; + private boolean m_ownsDutyCycle; + private double m_fullRange; + private double m_offset; + private int m_frequencyThreshold = 100; + private double m_periodNanos; + + private SimDevice m_simDevice; + private SimDouble m_simPosition; + private SimBoolean m_simIsConnected; + + private final void init(double fullRange, double offset) { + SendableRegistry.addLW(this, "DutyCyclePotentiometer", m_dutyCycle.getFPGAIndex()); + m_fullRange = fullRange; + m_offset = offset; + + m_simDevice = SimDevice.create("DutyCyclePotentiometer", m_dutyCycle.getFPGAIndex()); + + if (m_simDevice != null) { + m_simPosition = m_simDevice.createDouble("Position", Direction.kInput, 0.0); + m_simIsConnected = m_simDevice.createBoolean("Connected", SimDevice.Direction.kInput, true); + } + } + + /** + * AnalogPotentiometer constructor. + * + *

+ * Use the fullRange and offset values so that the output produces meaningful + * values. I.E: you + * have a 270 degree potentiometer, and you want the output to be degrees with + * the halfway point + * as 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 + * since the halfway + * point after scaling is 135 degrees. This will calculate the result from the + * fullRange times the + * fraction of the supply voltage, plus the offset. + * + * @param channel The analog input channel this potentiometer is plugged into. + * 0-3 are on-board + * and 4-7 are on the MXP port. + * @param fullRange The scaling to multiply the fraction by to get a meaningful + * unit. + * @param offset The offset to add to the scaled value for controlling the + * zero value + */ + @SuppressWarnings("this-escape") + public DutyCyclePotentiometer(final int channel, double fullRange, double offset) { + m_digitalSource = new DigitalInput(channel); + m_dutyCycle = new DutyCycle(m_digitalSource); + m_ownsDutyCycle = true; + init(fullRange, offset); + SendableRegistry.addChild(this, m_dutyCycle); + } + + /** + * AnalogPotentiometer constructor. + * + *

+ * Use the fullRange and offset values so that the output produces meaningful + * values. I.E: you + * have a 270 degree potentiometer, and you want the output to be degrees with + * the halfway point + * as 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 + * since the halfway + * point after scaling is 135 degrees. This will calculate the result from the + * fullRange times the + * fraction of the supply voltage, plus the offset. + * + * @param input The {@link DutyCycle} this potentiometer is plugged into. + * @param fullRange The angular value (in desired units) representing the full + * 0-5V range of the + * input. + * @param offset The angular value (in desired units) representing the + * angular output at 0V. + */ + @SuppressWarnings("this-escape") + public DutyCyclePotentiometer(final DigitalSource input, double fullRange, double offset) { + m_dutyCycle = new DutyCycle(requireNonNullParam(input, "input", "DutyCyclePotentiometer")); + m_ownsDutyCycle = true; + init(fullRange, offset); + } + + /** + * AnalogPotentiometer constructor. + * + *

+ * Use the fullRange and offset values so that the output produces meaningful + * values. I.E: you + * have a 270 degree potentiometer, and you want the output to be degrees with + * the halfway point + * as 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 + * since the halfway + * point after scaling is 135 degrees. This will calculate the result from the + * fullRange times the + * fraction of the supply voltage, plus the offset. + * + * @param input The {@link DutyCycle} this potentiometer is plugged into. + * @param fullRange The angular value (in desired units) representing the full + * 0-5V range of the + * input. + * @param offset The angular value (in desired units) representing the + * angular output at 0V. + */ + @SuppressWarnings("this-escape") + public DutyCyclePotentiometer(final DutyCycle input, double fullRange, double offset) { + m_dutyCycle = requireNonNullParam(input, "input", "DutyCyclePotentiometer"); + init(fullRange, offset); + } + + /** + * AnalogPotentiometer constructor. + * + *

+ * Use the scale value so that the output produces meaningful values. I.E: you + * have a 270 + * degree potentiometer, and you want the output to be degrees with the starting + * point as 0 + * degrees. The scale value is 270.0(degrees). + * + * @param channel The analog input channel this potentiometer is plugged into. + * 0-3 are on-board + * and 4-7 are on the MXP port. + * @param scale The scaling to multiply the voltage by to get a meaningful + * unit. + */ + public DutyCyclePotentiometer(final int channel, double scale) { + this(channel, scale, 0); + } + + /** + * AnalogPotentiometer constructor. + * + *

+ * Use the fullRange and offset values so that the output produces meaningful + * values. I.E: you + * have a 270 degree potentiometer, and you want the output to be degrees with + * the starting point + * as 0 degrees. The scale value is 270.0(degrees). + * + * @param input The {@link DutyCycle} this potentiometer is plugged into. + * @param scale The scaling to multiply the voltage by to get a meaningful unit. + */ + public DutyCyclePotentiometer(final DigitalSource input, double scale) { + this(input, scale, 0); + } + + /** + * AnalogPotentiometer constructor. + * + *

+ * Use the fullRange and offset values so that the output produces meaningful + * values. I.E: you + * have a 270 degree potentiometer, and you want the output to be degrees with + * the starting point + * as 0 degrees. The scale value is 270.0(degrees). + * + * @param input The {@link DutyCycle} this potentiometer is plugged into. + * @param scale The scaling to multiply the voltage by to get a meaningful unit. + */ + public DutyCyclePotentiometer(final DutyCycle input, double scale) { + this(input, scale, 0); + } + + /** + * AnalogPotentiometer constructor. + * + *

+ * The potentiometer will return a value between 0 and 1.0. + * + * @param channel The analog input channel this potentiometer is plugged into. + * 0-3 are on-board + * and 4-7 are on the MXP port. + */ + public DutyCyclePotentiometer(final int channel) { + this(channel, 1, 0); + } + + /** + * AnalogPotentiometer constructor. + * + *

+ * The potentiometer will return a value between 0 and 1.0. + * + * @param input The {@link DutyCycle} this potentiometer is plugged into. + */ + public DutyCyclePotentiometer(final DigitalSource input) { + this(input, 1, 0); + } + + /** + * AnalogPotentiometer constructor. + * + *

+ * The potentiometer will return a value between 0 and 1.0. + * + * @param input The {@link DutyCycle} this potentiometer is plugged into. + */ + public DutyCyclePotentiometer(final DutyCycle input) { + this(input, 1, 0); + } + + /** + * Get the current reading of the potentiometer. + * + * @return The current position of the potentiometer. + */ + public double get() { + if (m_simPosition != null) { + return m_simPosition.get(); + } + double pos; + if (m_periodNanos == 0.0) { + pos = m_dutyCycle.getOutput(); + } else { + int highTime = m_dutyCycle.getHighTimeNanoseconds(); + pos = highTime / m_periodNanos; + } + + return pos * m_fullRange + m_offset; + } + + /** + * Get the frequency in Hz of the duty cycle signal from the encoder. + * + * @return duty cycle frequency in Hz + */ + public int getFrequency() { + return m_dutyCycle.getFrequency(); + } + + /** + * Get if the sensor is connected + * + *

+ * This uses the duty cycle frequency to determine if the sensor is connected. + * By default, a + * value of 100 Hz is used as the threshold, and this value can be changed with + * {@link + * #setConnectedFrequencyThreshold(int)}. + * + * @return true if the sensor is connected + */ + public boolean isConnected() { + if (m_simIsConnected != null) { + return m_simIsConnected.get(); + } + return getFrequency() > m_frequencyThreshold; + } + + /** + * Change the frequency threshold for detecting connection used by + * {@link #isConnected()}. + * + * @param frequency the minimum frequency in Hz. + */ + public void setConnectedFrequencyThreshold(int frequency) { + if (frequency < 0) { + frequency = 0; + } + + m_frequencyThreshold = frequency; + } + + /** + * Sets the assumed frequency of the connected device. + * + *

+ * 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 + */ + public void setAssumedFrequency(double frequency) { + if (frequency == 0.0) { + m_periodNanos = 0.0; + } else { + m_periodNanos = 1000000000 / frequency; + } + } + + @Override + public void initSendable(SendableBuilder builder) { + if (m_dutyCycle != null) { + builder.setSmartDashboardType("Analog Input"); + builder.addDoubleProperty("Value", this::get, null); + } + } + + @Override + public void close() { + SendableRegistry.remove(this); + if (m_ownsDutyCycle) { + m_dutyCycle.close(); + m_dutyCycle = null; + m_ownsDutyCycle = false; + } + if (m_digitalSource != null) { + m_digitalSource.close(); + } + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java index 6b116731a5a..13712439431 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj.AnalogEncoder; /** Class to control a simulated analog encoder. */ +@Deprecated() public class AnalogEncoderSim { private final SimDouble m_simPosition; @@ -17,6 +18,7 @@ public class AnalogEncoderSim { * * @param encoder AnalogEncoder to simulate */ + @SuppressWarnings("removal") public AnalogEncoderSim(AnalogEncoder encoder) { SimDeviceSim wrappedSimDevice = new SimDeviceSim("AnalogEncoder" + "[" + encoder.getChannel() + "]"); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java index 3c2c741b62d..2da8e4cd769 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj.DutyCycleEncoder; /** Class to control a simulated duty cycle encoder. */ +@Deprecated() public class DutyCycleEncoderSim { private final SimDouble m_simPosition; private final SimDouble m_simDistancePerRotation; @@ -20,6 +21,7 @@ public class DutyCycleEncoderSim { * * @param encoder DutyCycleEncoder to simulate */ + @SuppressWarnings("removal") public DutyCycleEncoderSim(DutyCycleEncoder encoder) { this(encoder.getSourceChannel()); } From 2ee62168ddfb9b1c8c3b2688eeb1b411fdd32941 Mon Sep 17 00:00:00 2001 From: Thad House Date: Sun, 25 Feb 2024 23:32:13 -0800 Subject: [PATCH 17/30] Keep with AnalogEncoder and DutyCycleEncoder --- myRobot/src/main/java/frc/robot/MyRobot.java | 6 +- .../edu/wpi/first/wpilibj/AnalogEncoder.java | 160 +++------ .../first/wpilibj/AnalogPotentiometer.java | 174 ++------- .../wpi/first/wpilibj/DutyCycleEncoder.java | 195 +++-------- .../first/wpilibj/DutyCyclePotentiometer.java | 330 ------------------ .../wpilibj/simulation/AnalogEncoderSim.java | 2 - .../simulation/DutyCycleEncoderSim.java | 55 +-- 7 files changed, 128 insertions(+), 794 deletions(-) delete mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCyclePotentiometer.java diff --git a/myRobot/src/main/java/frc/robot/MyRobot.java b/myRobot/src/main/java/frc/robot/MyRobot.java index 5a9a56c493c..da767823e35 100644 --- a/myRobot/src/main/java/frc/robot/MyRobot.java +++ b/myRobot/src/main/java/frc/robot/MyRobot.java @@ -4,19 +4,19 @@ package frc.robot; -import edu.wpi.first.wpilibj.DutyCyclePotentiometer; +import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class MyRobot extends TimedRobot { - DutyCyclePotentiometer pot; + DutyCycleEncoder pot; /** * This function is run when the robot is first started up and should be used for any * initialization code. */ @Override public void robotInit() { - pot = new DutyCyclePotentiometer(0); + pot = new DutyCycleEncoder(0, 1, 0.5); pot.setAssumedFrequency(967.77); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java index 767815bfd32..cae42a8fcd8 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java @@ -11,34 +11,29 @@ 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. - * - * @deprecated Use AnalogPotentiometer instead. If rollover support is necessary, - * use enableRolloverSupport() on AnalogPotentiometer. */ -@Deprecated(since = "2025", forRemoval = true) 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 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 */ - 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; } /** @@ -47,33 +42,34 @@ public AnalogEncoder(int channel) { * @param analogInput the analog input to attach to */ @SuppressWarnings("this-escape") - public AnalogEncoder(AnalogInput analogInput) { + public AnalogEncoder(AnalogInput analogInput, double fullRange, double expectedZero) { m_analogInput = analogInput; - init(); + init(fullRange, expectedZero); } - 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; } /** @@ -88,97 +84,31 @@ public double get() { 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 pos = m_analogInput.getVoltage() / RobotController.getVoltage5V(); - 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 analog encoder. - * - *

getAbsolutePosition() - getPositionOffset() will give an encoder absolute position relative - * to the last reset. This could potentially be negative, which needs to be accounted for. - * - *

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; - return m_analogInput.getVoltage() / RobotController.getVoltage5V(); + // Map from 0 - Full Range + return MathUtil.inputModulus(pos, 0, m_fullRange); } /** - * Get the offset of position relative to the last reset. - * - *

getAbsolutePosition() - getPositionOffset() will give an encoder absolute position relative - * to the last reset. This could potentially be negative, which needs to be accounted for. + * 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. * - * @return the position offset + * @param min minimum voltage percentage (0-1 range) + * @param max maximum voltage percentage (0-1 range) */ - public double getPositionOffset() { - return m_positionOffset; + 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); } - /** - * Set the position offset. - * - *

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(); - } /** * Get the channel number. @@ -189,16 +119,11 @@ public int getChannel() { 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(); } @@ -207,7 +132,6 @@ public void close() { @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); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java index 19e5ec4b87d..15473495528 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java @@ -4,20 +4,13 @@ package edu.wpi.first.wpilibj; -import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; -import edu.wpi.first.hal.SimDevice; -import edu.wpi.first.hal.SimDouble; -import edu.wpi.first.hal.SimDevice.Direction; 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 reading analog potentiometers. Analog potentiometers read in an - * analog voltage that - * corresponds to a position. The position is in whichever units you choose, by - * way of the scaling + * Class for reading analog potentiometers. Analog potentiometers read in an analog voltage that + * corresponds to a position. The position is in whichever units you choose, by way of the scaling * and offset constants passed to the constructor. */ public class AnalogPotentiometer implements Sendable, AutoCloseable { @@ -25,42 +18,20 @@ public class AnalogPotentiometer implements Sendable, AutoCloseable { private boolean m_initAnalogInput; private double m_fullRange; private double m_offset; - private AnalogTrigger m_rotationTrigger; - private Counter m_rotationCounter; - private double m_lastPosition; - - private SimDevice m_simDevice; - private SimDouble m_simPosition; - - private void init() { - m_simDevice = SimDevice.create("AnalogPotentiometer", m_analogInput.getChannel()); - - if (m_simDevice != null) { - m_simPosition = m_simDevice.createDouble("Position", Direction.kInput, 0.0); - } - } /** * AnalogPotentiometer constructor. * - *

- * Use the fullRange and offset values so that the output produces meaningful - * values. I.E: you - * have a 270 degree potentiometer, and you want the output to be degrees with - * the halfway point - * as 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 - * since the halfway - * point after scaling is 135 degrees. This will calculate the result from the - * fullRange times the + *

Use the fullRange and offset values so that the output produces meaningful values. I.E: you + * have a 270 degree potentiometer, and you want the output to be degrees with the halfway point + * as 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 since the halfway + * point after scaling is 135 degrees. This will calculate the result from the fullRange times the * fraction of the supply voltage, plus the offset. * - * @param channel The analog input channel this potentiometer is plugged into. - * 0-3 are on-board - * and 4-7 are on the MXP port. - * @param fullRange The scaling to multiply the fraction by to get a meaningful - * unit. - * @param offset The offset to add to the scaled value for controlling the - * zero value + * @param channel The analog input channel this potentiometer is plugged into. 0-3 are on-board + * and 4-7 are on the MXP port. + * @param fullRange The scaling to multiply the fraction by to get a meaningful unit. + * @param offset The offset to add to the scaled value for controlling the zero value */ @SuppressWarnings("this-escape") public AnalogPotentiometer(final int channel, double fullRange, double offset) { @@ -72,51 +43,37 @@ public AnalogPotentiometer(final int channel, double fullRange, double offset) { /** * AnalogPotentiometer constructor. * - *

- * Use the fullRange and offset values so that the output produces meaningful - * values. I.E: you - * have a 270 degree potentiometer, and you want the output to be degrees with - * the halfway point - * as 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 - * since the halfway - * point after scaling is 135 degrees. This will calculate the result from the - * fullRange times the + *

Use the fullRange and offset values so that the output produces meaningful values. I.E: you + * have a 270 degree potentiometer, and you want the output to be degrees with the halfway point + * as 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 since the halfway + * point after scaling is 135 degrees. This will calculate the result from the fullRange times the * fraction of the supply voltage, plus the offset. * - * @param input The {@link AnalogInput} this potentiometer is plugged into. - * @param fullRange The angular value (in desired units) representing the full - * 0-5V range of the - * input. - * @param offset The angular value (in desired units) representing the - * angular output at 0V. + * @param input The {@link AnalogInput} this potentiometer is plugged into. + * @param fullRange The angular value (in desired units) representing the full 0-5V range of the + * input. + * @param offset The angular value (in desired units) representing the angular output at 0V. */ @SuppressWarnings("this-escape") public AnalogPotentiometer(final AnalogInput input, double fullRange, double offset) { SendableRegistry.addLW(this, "AnalogPotentiometer", input.getChannel()); - m_analogInput = requireNonNullParam(input, "input", "AnalogPotentiometer"); + m_analogInput = input; m_initAnalogInput = false; m_fullRange = fullRange; m_offset = offset; - - init(); } /** * AnalogPotentiometer constructor. * - *

- * Use the scale value so that the output produces meaningful values. I.E: you - * have a 270 - * degree potentiometer, and you want the output to be degrees with the starting - * point as 0 + *

Use the scale value so that the output produces meaningful values. I.E: you have a 270 + * degree potentiometer, and you want the output to be degrees with the starting point as 0 * degrees. The scale value is 270.0(degrees). * - * @param channel The analog input channel this potentiometer is plugged into. - * 0-3 are on-board - * and 4-7 are on the MXP port. - * @param scale The scaling to multiply the voltage by to get a meaningful - * unit. + * @param channel The analog input channel this potentiometer is plugged into. 0-3 are on-board + * and 4-7 are on the MXP port. + * @param scale The scaling to multiply the voltage by to get a meaningful unit. */ public AnalogPotentiometer(final int channel, double scale) { this(channel, scale, 0); @@ -125,11 +82,8 @@ public AnalogPotentiometer(final int channel, double scale) { /** * AnalogPotentiometer constructor. * - *

- * Use the fullRange and offset values so that the output produces meaningful - * values. I.E: you - * have a 270 degree potentiometer, and you want the output to be degrees with - * the starting point + *

Use the fullRange and offset values so that the output produces meaningful values. I.E: you + * have a 270 degree potentiometer, and you want the output to be degrees with the starting point * as 0 degrees. The scale value is 270.0(degrees). * * @param input The {@link AnalogInput} this potentiometer is plugged into. @@ -142,12 +96,10 @@ public AnalogPotentiometer(final AnalogInput input, double scale) { /** * AnalogPotentiometer constructor. * - *

- * The potentiometer will return a value between 0 and 1.0. + *

The potentiometer will return a value between 0 and 1.0. * - * @param channel The analog input channel this potentiometer is plugged into. - * 0-3 are on-board - * and 4-7 are on the MXP port. + * @param channel The analog input channel this potentiometer is plugged into. 0-3 are on-board + * and 4-7 are on the MXP port. */ public AnalogPotentiometer(final int channel) { this(channel, 1, 0); @@ -156,8 +108,7 @@ public AnalogPotentiometer(final int channel) { /** * AnalogPotentiometer constructor. * - *

- * The potentiometer will return a value between 0 and 1.0. + *

The potentiometer will return a value between 0 and 1.0. * * @param input The {@link AnalogInput} this potentiometer is plugged into. */ @@ -165,73 +116,19 @@ public AnalogPotentiometer(final AnalogInput input) { this(input, 1, 0); } - private boolean doubleEquals(double a, double b) { - double epsilon = 0.00001d; - return Math.abs(a - b) < epsilon; - } - - private double getRollovers() { - 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_rotationCounter.get(); - double pos = m_analogInput.getVoltage(); - double counter2 = m_rotationCounter.get(); - double pos2 = m_analogInput.getVoltage(); - if (counter == counter2 && doubleEquals(pos, pos2)) { - pos = pos / RobotController.getVoltage5V(); - double position = (counter + pos) * m_fullRange + m_offset; - m_lastPosition = position; - return position; - } - } - - DriverStation.reportWarning( - "Failed to read Analog Encoder. Potential Speed Overrun. Returning last value", false); - return m_lastPosition; - } - /** * Get the current reading of the potentiometer. * * @return The current position of the potentiometer. */ public double get() { - if (m_simPosition != null) { - return m_simPosition.get(); - } - - if (m_rotationCounter != null) { - return getRollovers(); + if (m_analogInput == null) { + return m_offset; } - return (m_analogInput.getAverageVoltage() / RobotController.getVoltage5V()) * m_fullRange + m_offset; } - public void enableRolloverSupport() { - if (m_rotationCounter != null) { - return; - } - m_rotationTrigger = new AnalogTrigger(m_analogInput); - m_rotationCounter = new Counter(); - - // Limits need to be 25% from each end - m_rotationTrigger.setLimitsVoltage(1.25, 3.75); - m_rotationCounter.setUpSource(m_rotationTrigger, AnalogTriggerType.kRisingPulse); - m_rotationCounter.setDownSource(m_rotationTrigger, AnalogTriggerType.kFallingPulse); - } - - public void resetRollovers() { - if (m_rotationCounter != null) { - m_rotationCounter.reset(); - } - } - @Override public void initSendable(SendableBuilder builder) { if (m_analogInput != null) { @@ -243,17 +140,10 @@ public void initSendable(SendableBuilder builder) { @Override public void close() { SendableRegistry.remove(this); - if (m_rotationCounter != null) { - m_rotationCounter.close(); - m_rotationTrigger.close(); - m_rotationCounter = null; - m_rotationTrigger = null; - } if (m_initAnalogInput) { m_analogInput.close(); m_analogInput = null; m_initAnalogInput = false; } - } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java index c9c7383cce3..c3b85b4c296 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java @@ -11,33 +11,24 @@ 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 * CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag Encoder. - * - * @deprecated Use DutyCyclePotentiometer instead. Rollover support is not supported for - * duty cycle encoders, there is no reliable way to do so. */ -@Deprecated(since = "2025", forRemoval = true) 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 SimDevice m_simDevice; private SimDouble m_simPosition; - private SimDouble m_simAbsolutePosition; - private SimDouble m_simDistancePerRotation; private SimBoolean m_simIsConnected; /** @@ -46,11 +37,11 @@ public class DutyCycleEncoder implements Sendable, AutoCloseable { * @param channel the channel to attach to */ @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); } /** @@ -59,9 +50,9 @@ public DutyCycleEncoder(int channel) { * @param dutyCycle the duty cycle to attach to */ @SuppressWarnings("this-escape") - public DutyCycleEncoder(DutyCycle dutyCycle) { + public DutyCycleEncoder(DutyCycle dutyCycle, double fullRange, double expectedZero) { m_dutyCycle = dutyCycle; - init(); + init(fullRange, expectedZero); } /** @@ -70,30 +61,23 @@ public DutyCycleEncoder(DutyCycle dutyCycle) { * @param source the digital source to attach to */ @SuppressWarnings("this-escape") - public DutyCycleEncoder(DigitalSource source) { + public DutyCycleEncoder(DigitalSource source, double fullRange, double expectedZero) { m_ownsDutyCycle = true; m_dutyCycle = new DutyCycle(source); - init(); + init(fullRange, expectedZero); } - 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()); } @@ -109,11 +93,6 @@ private double mapSensorRange(double pos) { 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. * @@ -126,67 +105,23 @@ public double get() { 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; - } - } - - DriverStation.reportWarning( - "Failed to read Analog Encoder. Potential Speed Overrun. Returning last value", false); - return m_lastPosition; - } - - /** - * Get the absolute position of the duty cycle encoder. - * - *

getAbsolutePosition() - getPositionOffset() will give an encoder absolute position relative - * to the last reset. This could potentially be negative, which needs to be accounted for. - * - *

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(); + 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; } - return mapSensorRange(m_dutyCycle.getOutput()); - } + // Map sensor range if range isn't full + pos = mapSensorRange(pos); - /** - * Get the offset of position relative to the last reset. - * - *

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; - } + // Compute full range and offset + pos = pos * m_fullRange - m_expectedZero; - /** - * Set the position offset. - * - *

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); + // Map from 0 - Full Range + return MathUtil.inputModulus(pos, 0, m_fullRange); } /** @@ -205,40 +140,6 @@ public void setDutyCycleRange(double min, double max) { 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. * @@ -248,17 +149,6 @@ public int getFrequency() { 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 * @@ -288,14 +178,28 @@ public void setConnectedFrequencyThreshold(int frequency) { m_frequencyThreshold = frequency; } + /** + * Sets the assumed frequency of the connected device. + * + *

+ * 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 + */ + public void setAssumedFrequency(double frequency) { + if (frequency == 0.0) { + m_periodNanos = 0.0; + } else { + m_periodNanos = 1000000000 / frequency; + } + } + @Override public void close() { - if (m_counter != null) { - m_counter.close(); - } - if (m_analogTrigger != null) { - m_analogTrigger.close(); - } if (m_ownsDutyCycle) { m_dutyCycle.close(); } @@ -328,8 +232,7 @@ public int getSourceChannel() { @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); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCyclePotentiometer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCyclePotentiometer.java deleted file mode 100644 index 81268d12e51..00000000000 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCyclePotentiometer.java +++ /dev/null @@ -1,330 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package edu.wpi.first.wpilibj; - -import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; - -import edu.wpi.first.hal.SimBoolean; -import edu.wpi.first.hal.SimDevice; -import edu.wpi.first.hal.SimDevice.Direction; -import edu.wpi.first.hal.SimDouble; -import edu.wpi.first.util.sendable.Sendable; -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.util.sendable.SendableRegistry; - -/** - * Class for reading duty cyle potentiometers. Duty cycle potentiometers read in - * an pwm signal that - * corresponds to a position. The position is in whichever units you choose, by - * way of the scaling - * and offset constants passed to the constructor. - */ -public class DutyCyclePotentiometer implements Sendable, AutoCloseable { - private DutyCycle m_dutyCycle; - private DigitalSource m_digitalSource; - private boolean m_ownsDutyCycle; - private double m_fullRange; - private double m_offset; - private int m_frequencyThreshold = 100; - private double m_periodNanos; - - private SimDevice m_simDevice; - private SimDouble m_simPosition; - private SimBoolean m_simIsConnected; - - private final void init(double fullRange, double offset) { - SendableRegistry.addLW(this, "DutyCyclePotentiometer", m_dutyCycle.getFPGAIndex()); - m_fullRange = fullRange; - m_offset = offset; - - m_simDevice = SimDevice.create("DutyCyclePotentiometer", m_dutyCycle.getFPGAIndex()); - - if (m_simDevice != null) { - m_simPosition = m_simDevice.createDouble("Position", Direction.kInput, 0.0); - m_simIsConnected = m_simDevice.createBoolean("Connected", SimDevice.Direction.kInput, true); - } - } - - /** - * AnalogPotentiometer constructor. - * - *

- * Use the fullRange and offset values so that the output produces meaningful - * values. I.E: you - * have a 270 degree potentiometer, and you want the output to be degrees with - * the halfway point - * as 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 - * since the halfway - * point after scaling is 135 degrees. This will calculate the result from the - * fullRange times the - * fraction of the supply voltage, plus the offset. - * - * @param channel The analog input channel this potentiometer is plugged into. - * 0-3 are on-board - * and 4-7 are on the MXP port. - * @param fullRange The scaling to multiply the fraction by to get a meaningful - * unit. - * @param offset The offset to add to the scaled value for controlling the - * zero value - */ - @SuppressWarnings("this-escape") - public DutyCyclePotentiometer(final int channel, double fullRange, double offset) { - m_digitalSource = new DigitalInput(channel); - m_dutyCycle = new DutyCycle(m_digitalSource); - m_ownsDutyCycle = true; - init(fullRange, offset); - SendableRegistry.addChild(this, m_dutyCycle); - } - - /** - * AnalogPotentiometer constructor. - * - *

- * Use the fullRange and offset values so that the output produces meaningful - * values. I.E: you - * have a 270 degree potentiometer, and you want the output to be degrees with - * the halfway point - * as 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 - * since the halfway - * point after scaling is 135 degrees. This will calculate the result from the - * fullRange times the - * fraction of the supply voltage, plus the offset. - * - * @param input The {@link DutyCycle} this potentiometer is plugged into. - * @param fullRange The angular value (in desired units) representing the full - * 0-5V range of the - * input. - * @param offset The angular value (in desired units) representing the - * angular output at 0V. - */ - @SuppressWarnings("this-escape") - public DutyCyclePotentiometer(final DigitalSource input, double fullRange, double offset) { - m_dutyCycle = new DutyCycle(requireNonNullParam(input, "input", "DutyCyclePotentiometer")); - m_ownsDutyCycle = true; - init(fullRange, offset); - } - - /** - * AnalogPotentiometer constructor. - * - *

- * Use the fullRange and offset values so that the output produces meaningful - * values. I.E: you - * have a 270 degree potentiometer, and you want the output to be degrees with - * the halfway point - * as 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 - * since the halfway - * point after scaling is 135 degrees. This will calculate the result from the - * fullRange times the - * fraction of the supply voltage, plus the offset. - * - * @param input The {@link DutyCycle} this potentiometer is plugged into. - * @param fullRange The angular value (in desired units) representing the full - * 0-5V range of the - * input. - * @param offset The angular value (in desired units) representing the - * angular output at 0V. - */ - @SuppressWarnings("this-escape") - public DutyCyclePotentiometer(final DutyCycle input, double fullRange, double offset) { - m_dutyCycle = requireNonNullParam(input, "input", "DutyCyclePotentiometer"); - init(fullRange, offset); - } - - /** - * AnalogPotentiometer constructor. - * - *

- * Use the scale value so that the output produces meaningful values. I.E: you - * have a 270 - * degree potentiometer, and you want the output to be degrees with the starting - * point as 0 - * degrees. The scale value is 270.0(degrees). - * - * @param channel The analog input channel this potentiometer is plugged into. - * 0-3 are on-board - * and 4-7 are on the MXP port. - * @param scale The scaling to multiply the voltage by to get a meaningful - * unit. - */ - public DutyCyclePotentiometer(final int channel, double scale) { - this(channel, scale, 0); - } - - /** - * AnalogPotentiometer constructor. - * - *

- * Use the fullRange and offset values so that the output produces meaningful - * values. I.E: you - * have a 270 degree potentiometer, and you want the output to be degrees with - * the starting point - * as 0 degrees. The scale value is 270.0(degrees). - * - * @param input The {@link DutyCycle} this potentiometer is plugged into. - * @param scale The scaling to multiply the voltage by to get a meaningful unit. - */ - public DutyCyclePotentiometer(final DigitalSource input, double scale) { - this(input, scale, 0); - } - - /** - * AnalogPotentiometer constructor. - * - *

- * Use the fullRange and offset values so that the output produces meaningful - * values. I.E: you - * have a 270 degree potentiometer, and you want the output to be degrees with - * the starting point - * as 0 degrees. The scale value is 270.0(degrees). - * - * @param input The {@link DutyCycle} this potentiometer is plugged into. - * @param scale The scaling to multiply the voltage by to get a meaningful unit. - */ - public DutyCyclePotentiometer(final DutyCycle input, double scale) { - this(input, scale, 0); - } - - /** - * AnalogPotentiometer constructor. - * - *

- * The potentiometer will return a value between 0 and 1.0. - * - * @param channel The analog input channel this potentiometer is plugged into. - * 0-3 are on-board - * and 4-7 are on the MXP port. - */ - public DutyCyclePotentiometer(final int channel) { - this(channel, 1, 0); - } - - /** - * AnalogPotentiometer constructor. - * - *

- * The potentiometer will return a value between 0 and 1.0. - * - * @param input The {@link DutyCycle} this potentiometer is plugged into. - */ - public DutyCyclePotentiometer(final DigitalSource input) { - this(input, 1, 0); - } - - /** - * AnalogPotentiometer constructor. - * - *

- * The potentiometer will return a value between 0 and 1.0. - * - * @param input The {@link DutyCycle} this potentiometer is plugged into. - */ - public DutyCyclePotentiometer(final DutyCycle input) { - this(input, 1, 0); - } - - /** - * Get the current reading of the potentiometer. - * - * @return The current position of the potentiometer. - */ - public double get() { - if (m_simPosition != null) { - return m_simPosition.get(); - } - double pos; - if (m_periodNanos == 0.0) { - pos = m_dutyCycle.getOutput(); - } else { - int highTime = m_dutyCycle.getHighTimeNanoseconds(); - pos = highTime / m_periodNanos; - } - - return pos * m_fullRange + m_offset; - } - - /** - * Get the frequency in Hz of the duty cycle signal from the encoder. - * - * @return duty cycle frequency in Hz - */ - public int getFrequency() { - return m_dutyCycle.getFrequency(); - } - - /** - * Get if the sensor is connected - * - *

- * This uses the duty cycle frequency to determine if the sensor is connected. - * By default, a - * value of 100 Hz is used as the threshold, and this value can be changed with - * {@link - * #setConnectedFrequencyThreshold(int)}. - * - * @return true if the sensor is connected - */ - public boolean isConnected() { - if (m_simIsConnected != null) { - return m_simIsConnected.get(); - } - return getFrequency() > m_frequencyThreshold; - } - - /** - * Change the frequency threshold for detecting connection used by - * {@link #isConnected()}. - * - * @param frequency the minimum frequency in Hz. - */ - public void setConnectedFrequencyThreshold(int frequency) { - if (frequency < 0) { - frequency = 0; - } - - m_frequencyThreshold = frequency; - } - - /** - * Sets the assumed frequency of the connected device. - * - *

- * 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 - */ - public void setAssumedFrequency(double frequency) { - if (frequency == 0.0) { - m_periodNanos = 0.0; - } else { - m_periodNanos = 1000000000 / frequency; - } - } - - @Override - public void initSendable(SendableBuilder builder) { - if (m_dutyCycle != null) { - builder.setSmartDashboardType("Analog Input"); - builder.addDoubleProperty("Value", this::get, null); - } - } - - @Override - public void close() { - SendableRegistry.remove(this); - if (m_ownsDutyCycle) { - m_dutyCycle.close(); - m_dutyCycle = null; - m_ownsDutyCycle = false; - } - if (m_digitalSource != null) { - m_digitalSource.close(); - } - } -} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java index 13712439431..6b116731a5a 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java @@ -9,7 +9,6 @@ import edu.wpi.first.wpilibj.AnalogEncoder; /** Class to control a simulated analog encoder. */ -@Deprecated() public class AnalogEncoderSim { private final SimDouble m_simPosition; @@ -18,7 +17,6 @@ public class AnalogEncoderSim { * * @param encoder AnalogEncoder to simulate */ - @SuppressWarnings("removal") public AnalogEncoderSim(AnalogEncoder encoder) { SimDeviceSim wrappedSimDevice = new SimDeviceSim("AnalogEncoder" + "[" + encoder.getChannel() + "]"); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java index 2da8e4cd769..786c16eea17 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java @@ -9,11 +9,8 @@ import edu.wpi.first.wpilibj.DutyCycleEncoder; /** Class to control a simulated duty cycle encoder. */ -@Deprecated() public class DutyCycleEncoderSim { private final SimDouble m_simPosition; - private final SimDouble m_simDistancePerRotation; - private final SimDouble m_simAbsolutePosition; private final SimBoolean m_simIsConnected; /** @@ -21,7 +18,6 @@ public class DutyCycleEncoderSim { * * @param encoder DutyCycleEncoder to simulate */ - @SuppressWarnings("removal") public DutyCycleEncoderSim(DutyCycleEncoder encoder) { this(encoder.getSourceChannel()); } @@ -33,10 +29,8 @@ public DutyCycleEncoderSim(DutyCycleEncoder encoder) { */ 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"); } /** @@ -57,51 +51,6 @@ 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(); - } - /** * Get if the encoder is connected. * From 0c4b9c7bb76c8e1739241bcc278ca6c39fa298fd Mon Sep 17 00:00:00 2001 From: Thad House Date: Sun, 25 Feb 2024 23:59:48 -0800 Subject: [PATCH 18/30] Add inversion --- myRobot/src/main/java/frc/robot/MyRobot.java | 9 +++++++-- .../java/edu/wpi/first/wpilibj/AnalogEncoder.java | 12 +++++++++++- .../java/edu/wpi/first/wpilibj/DutyCycleEncoder.java | 12 +++++++++++- .../src/main/java/edu/wpi/first/math/MathUtil.java | 4 ++++ 4 files changed, 33 insertions(+), 4 deletions(-) diff --git a/myRobot/src/main/java/frc/robot/MyRobot.java b/myRobot/src/main/java/frc/robot/MyRobot.java index da767823e35..d63e534882b 100644 --- a/myRobot/src/main/java/frc/robot/MyRobot.java +++ b/myRobot/src/main/java/frc/robot/MyRobot.java @@ -4,6 +4,7 @@ package frc.robot; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -16,7 +17,7 @@ public class MyRobot extends TimedRobot { */ @Override public void robotInit() { - pot = new DutyCycleEncoder(0, 1, 0.5); + pot = new DutyCycleEncoder(0, 3, 0.5); pot.setAssumedFrequency(967.77); } @@ -43,7 +44,11 @@ public void testPeriodic() {} /** This function is called periodically during all modes. */ @Override public void robotPeriodic() { - SmartDashboard.putNumber("DC", pot.get()); + double val = pot.get() + 1; + SmartDashboard.putNumber("DC4", val); + SmartDashboard.putNumber("DC3", MathUtil.invertInput(val, 1, 4)); + SmartDashboard.putNumber("DC2", MathUtil.inputModulus(pot.get(), -0.1, 0.9)); + SmartDashboard.putNumber("DC", MathUtil.inputModulus(MathUtil.invertInput(pot.get(), 0, 1), -0.1, 0.9)); SmartDashboard.putNumber("Freq", pot.getFrequency()); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java index cae42a8fcd8..021e8cc06d4 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java @@ -22,6 +22,7 @@ public class AnalogEncoder implements Sendable, AutoCloseable { 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; @@ -93,7 +94,12 @@ public double get() { pos = pos * m_fullRange - m_expectedZero; // Map from 0 - Full Range - return MathUtil.inputModulus(pos, 0, m_fullRange); + double result = MathUtil.inputModulus(pos, 0, m_fullRange); + // Invert if necessary + if (m_isInverted) { + return m_fullRange - result; + } + return result; } /** @@ -109,6 +115,10 @@ public void setVoltagePercentageRange(double min, double max) { m_sensorMax = MathUtil.clamp(max, 0.0, 1.0); } + public void SetInverted(boolean inverted) { + m_isInverted = inverted; + } + /** * Get the channel number. diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java index c3b85b4c296..9d2b025f856 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java @@ -26,6 +26,7 @@ public class DutyCycleEncoder implements Sendable, AutoCloseable { 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; @@ -121,7 +122,12 @@ public double get() { pos = pos * m_fullRange - m_expectedZero; // Map from 0 - Full Range - return MathUtil.inputModulus(pos, 0, m_fullRange); + double result = MathUtil.inputModulus(pos, 0, m_fullRange); + // Invert if necessary + if (m_isInverted) { + return m_fullRange - result; + } + return result; } /** @@ -198,6 +204,10 @@ public void setAssumedFrequency(double frequency) { } } + public void SetInverted(boolean inverted) { + m_isInverted = inverted; + } + @Override public void close() { if (m_ownsDutyCycle) { diff --git a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java index 044cddfab68..64fbf978be5 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java +++ b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java @@ -125,6 +125,10 @@ public static double inputModulus(double input, double minimumInput, double maxi return input; } + public static double invertInput(double input, double minimumInput, double maximumInput) { + return maximumInput - input + minimumInput; + } + /** * Wraps an angle to the range -pi to pi radians. * From 85469db7579a6ffa7d5620aa2d7371fc25174bef Mon Sep 17 00:00:00 2001 From: Thad House Date: Wed, 28 Feb 2024 21:28:29 -0800 Subject: [PATCH 19/30] Fix up example --- .../edu/wpi/first/wpilibj/AnalogEncoder.java | 25 +++- .../wpi/first/wpilibj/DutyCycleEncoder.java | 109 ++++++++++++------ .../examples/dutycycleencoder/Robot.java | 46 +++++++- 3 files changed, 137 insertions(+), 43 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java index 021e8cc06d4..617c50a6b42 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java @@ -48,6 +48,25 @@ public AnalogEncoder(AnalogInput analogInput, double fullRange, double expectedZ init(fullRange, expectedZero); } + /** + * Construct a new AnalogEncoder attached to a specific AnalogIn channel. + * + * @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. + * + * @param analogInput the analog input to attach to + */ + @SuppressWarnings("this-escape") + public AnalogEncoder(AnalogInput analogInput) { + this(analogInput, 1.0, 0.0); + } + private void init(double fullRange, double expectedZero) { m_simDevice = SimDevice.create("AnalogEncoder", m_analogInput.getChannel()); @@ -76,7 +95,8 @@ private double mapSensorRange(double pos) { /** * Get the encoder value since the last reset. * - *

This is reported in rotations since the last reset. + *

+ * This is reported in rotations since the last reset. * * @return the encoder value in rotations */ @@ -115,11 +135,10 @@ public void setVoltagePercentageRange(double min, double max) { m_sensorMax = MathUtil.clamp(max, 0.0, 1.0); } - public void SetInverted(boolean inverted) { + public void setInverted(boolean inverted) { m_isInverted = inverted; } - /** * Get the channel number. * diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java index 9d2b025f856..5e321b6dc48 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java @@ -13,7 +13,8 @@ import edu.wpi.first.util.sendable.SendableRegistry; /** - * Class for supporting duty cycle/PWM encoders, such as the US Digital MA3 with PWM Output, the + * Class for supporting duty cycle/PWM encoders, such as the US Digital MA3 with + * PWM Output, the * CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag Encoder. */ public class DutyCycleEncoder implements Sendable, AutoCloseable { @@ -68,6 +69,36 @@ public DutyCycleEncoder(DigitalSource source, double fullRange, double expectedZ init(fullRange, expectedZero); } + /** + * Construct a new DutyCycleEncoder on a specific channel. + * + * @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. + * + * @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. + * + * @param source the digital source to attach to + */ + @SuppressWarnings("this-escape") + public DutyCycleEncoder(DigitalSource source) { + this(source, 1.0, 0.0); + } + private void init(double fullRange, double expectedZero) { m_simDevice = SimDevice.create("DutyCycle:DutyCycleEncoder", m_dutyCycle.getSourceChannel()); @@ -97,7 +128,8 @@ private double mapSensorRange(double pos) { /** * Get the encoder value since the last reset. * - *

This is reported in rotations since the last reset. + *

+ * This is reported in rotations since the last reset. * * @return the encoder value in rotations */ @@ -109,10 +141,10 @@ public double get() { double pos; // Compute output percentage (0-1) if (m_periodNanos == 0.0) { - pos = m_dutyCycle.getOutput(); + pos = m_dutyCycle.getOutput(); } else { - int highTime = m_dutyCycle.getHighTimeNanoseconds(); - pos = highTime / m_periodNanos; + int highTime = m_dutyCycle.getHighTimeNanoseconds(); + pos = highTime / m_periodNanos; } // Map sensor range if range isn't full @@ -131,11 +163,16 @@ public double get() { } /** - * Set the encoder duty cycle range. As the encoder needs to maintain a duty cycle, the duty cycle - * cannot go all the way to 0% or all the way to 100%. For example, an encoder with a 4096 us - * period might have a minimum duty cycle of 1 us / 4096 us and a maximum duty cycle of 4095 / - * 4096 us. Setting the range will result in an encoder duty cycle less than or equal to the - * minimum being output as 0 rotation, the duty cycle greater than or equal to the maximum being + * Set the encoder duty cycle range. As the encoder needs to maintain a duty + * cycle, the duty cycle + * cannot go all the way to 0% or all the way to 100%. For example, an encoder + * with a 4096 us + * period might have a minimum duty cycle of 1 us / 4096 us and a maximum duty + * cycle of 4095 / + * 4096 us. Setting the range will result in an encoder duty cycle less than or + * equal to the + * minimum being output as 0 rotation, the duty cycle greater than or equal to + * the maximum being * output as 1 rotation, and values in between linearly scaled from 0 to 1. * * @param min minimum duty cycle (0-1 range) @@ -158,8 +195,11 @@ public int getFrequency() { /** * Get if the sensor is connected * - *

This uses the duty cycle frequency to determine if the sensor is connected. By default, a - * value of 100 Hz is used as the threshold, and this value can be changed with {@link + *

+ * This uses the duty cycle frequency to determine if the sensor is connected. + * By default, a + * value of 100 Hz is used as the threshold, and this value can be changed with + * {@link * #setConnectedFrequencyThreshold(int)}. * * @return true if the sensor is connected @@ -172,7 +212,8 @@ public boolean isConnected() { } /** - * Change the frequency threshold for detecting connection used by {@link #isConnected()}. + * Change the frequency threshold for detecting connection used by + * {@link #isConnected()}. * * @param frequency the minimum frequency in Hz. */ @@ -184,27 +225,27 @@ public void setConnectedFrequencyThreshold(int frequency) { m_frequencyThreshold = frequency; } - /** - * Sets the assumed frequency of the connected device. - * - *

- * 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 - */ - public void setAssumedFrequency(double frequency) { - if (frequency == 0.0) { - m_periodNanos = 0.0; - } else { - m_periodNanos = 1000000000 / frequency; - } - } - - public void SetInverted(boolean inverted) { + /** + * Sets the assumed frequency of the connected device. + * + *

+ * 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 + */ + public void setAssumedFrequency(double frequency) { + if (frequency == 0.0) { + m_periodNanos = 0.0; + } else { + m_periodNanos = 1000000000 / frequency; + } + } + + public void setInverted(boolean inverted) { m_isInverted = inverted; } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java index bb99244248a..70b3c4cf5cf 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java @@ -4,19 +4,43 @@ package edu.wpi.first.wpilibj.examples.dutycycleencoder; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +/** + * This example shows how to use a duty cycle encoder for devices such as an + * arm or elevator. + */ public class Robot extends TimedRobot { private DutyCycleEncoder m_dutyCycleEncoder; + private final double m_fullRange = 1.3; + private final double m_expectedZero = 0; @Override public void robotInit() { - m_dutyCycleEncoder = new DutyCycleEncoder(0); + // 2nd parameter is the range of values. This sensor will output between + // 0 and the passed in value. + // 3rd parameter is the the physical value where you want "0" to be. How + // to measure this is fairly easy. Set the value to 0, place the mechanism + // where you want "0" to be, and observe the value on the dashboard, That + // is the value to enter for the 3rd parameter. + m_dutyCycleEncoder = new DutyCycleEncoder(0, m_fullRange, m_expectedZero); - // Set to 0.5 units per rotation - m_dutyCycleEncoder.setDistancePerRotation(0.5); + + // If you know the frequency of your sensor, uncomment the following + // method, and set the method to the frequency of your sensor. + // This will result in more stable readings from the sensor. + // Do note that occasionally the datasheet cannot be trusted + // and you should measure this value. You can do so with either + // an oscilloscope, or by observing the "Frequency" output + // on the dashboard while running this sample. If you find + // the value jumping between the 2 values, enter halfway between + // those values. This number doesn't have to be perfect, + // just having a fairly close value will make the output readings + // much more stable. + m_dutyCycleEncoder.setAssumedFrequency(967.8); } @Override @@ -30,12 +54,22 @@ public void robotPeriodic() { // Output of encoder double output = m_dutyCycleEncoder.get(); - // Output scaled by DistancePerPulse - double distance = m_dutyCycleEncoder.getDistance(); + // By default, the output will wrap around to the full range value + // when the sensor goes below 0. However, for moving mechanisms this + // is not usually ideal, as if 0 is set to a hard stop, its still + // possible for the sensor to move slightly past. If this happens + // The sensor will assume its now at the furthest away position, + // which control algorithms might not handle correctly. Therefore + // it can be a good idea to slightly shift the output so the sensor + // can go a bit negative before wrapping. Usually 10% or so is fine. + // This does not change where "0" is, so no calibration numbers need + // to be changed. + double percentOfRange = m_fullRange * 0.1; + double shiftedOutput = MathUtil.inputModulus(output, 0 - percentOfRange, m_fullRange - percentOfRange); SmartDashboard.putBoolean("Connected", connected); SmartDashboard.putNumber("Frequency", frequency); SmartDashboard.putNumber("Output", output); - SmartDashboard.putNumber("Distance", distance); + SmartDashboard.putNumber("ShiftedOutput", shiftedOutput); } } From 071b62c4add4fc4ed979bc26e4018f493a0413fa Mon Sep 17 00:00:00 2001 From: Thad House Date: Wed, 28 Feb 2024 22:09:58 -0800 Subject: [PATCH 20/30] Fixup C++ --- wpilibc/src/main/native/cpp/AnalogEncoder.cpp | 144 ++++++-------- .../src/main/native/cpp/DutyCycleEncoder.cpp | 185 +++++++++--------- .../cpp/simulation/AnalogEncoderSim.cpp | 16 +- .../cpp/simulation/DutyCycleEncoderSim.cpp | 30 +-- .../main/native/include/frc/AnalogEncoder.h | 100 +++------- .../native/include/frc/DutyCycleEncoder.h | 147 ++++++-------- .../include/frc/simulation/AnalogEncoderSim.h | 20 +- .../frc/simulation/DutyCycleEncoderSim.h | 45 +---- .../cpp/simulation/AnalogEncoderSimTest.cpp | 10 +- .../simulation/DutyCycleEncoderSimTest.cpp | 43 +--- .../examples/DutyCycleEncoder/cpp/Robot.cpp | 46 ++++- .../wpilibj/simulation/AnalogEncoderSim.java | 29 +-- .../simulation/DutyCycleEncoderSim.java | 6 +- .../simulation/AnalogEncoderSimTest.java | 10 +- .../simulation/DutyCycleEncoderSimTest.java | 52 +---- 15 files changed, 321 insertions(+), 562 deletions(-) diff --git a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp index 0c29bea5e57..c4381f0ef31 100644 --- a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp +++ b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp @@ -8,9 +8,9 @@ #include #include "frc/AnalogInput.h" -#include "frc/Counter.h" #include "frc/Errors.h" #include "frc/RobotController.h" +#include "frc/MathUtil.h" using namespace frc; @@ -18,120 +18,104 @@ AnalogEncoder::AnalogEncoder(int channel) : AnalogEncoder(std::make_shared(channel)) {} AnalogEncoder::AnalogEncoder(AnalogInput& analogInput) - : m_analogInput{&analogInput, wpi::NullDeleter{}}, - m_analogTrigger{m_analogInput.get()}, - m_counter{} { - Init(); + : m_analogInput{&analogInput, wpi::NullDeleter{}} { + Init(1.0, 0.0); } AnalogEncoder::AnalogEncoder(AnalogInput* analogInput) - : m_analogInput{analogInput, wpi::NullDeleter{}}, - m_analogTrigger{m_analogInput.get()}, - m_counter{} { - Init(); + : m_analogInput{analogInput, wpi::NullDeleter{}} { + Init(1.0, 0.0); } AnalogEncoder::AnalogEncoder(std::shared_ptr analogInput) - : m_analogInput{std::move(analogInput)}, - m_analogTrigger{m_analogInput.get()}, - m_counter{} { - Init(); + : m_analogInput{std::move(analogInput)} { + Init(1.0, 0.0); } -void AnalogEncoder::Init() { - m_simDevice = hal::SimDevice{"AnalogEncoder", m_analogInput->GetChannel()}; +AnalogEncoder::AnalogEncoder(int channel, double fullRange, double expectedZero) + : AnalogEncoder(std::make_shared(channel), fullRange, + expectedZero) {} - if (m_simDevice) { - m_simPosition = m_simDevice.CreateDouble("Position", false, 0.0); - m_simAbsolutePosition = - m_simDevice.CreateDouble("absPosition", hal::SimDevice::kInput, 0.0); - } - - m_analogTrigger.SetLimitsVoltage(1.25, 3.75); - m_counter.SetUpSource( - m_analogTrigger.CreateOutput(AnalogTriggerType::kRisingPulse)); - m_counter.SetDownSource( - m_analogTrigger.CreateOutput(AnalogTriggerType::kFallingPulse)); +AnalogEncoder::AnalogEncoder(AnalogInput& analogInput, double fullRange, + double expectedZero) + : m_analogInput{&analogInput, wpi::NullDeleter{}} { + Init(fullRange, expectedZero); +} - wpi::SendableRegistry::AddLW(this, "DutyCycle Encoder", - m_analogInput->GetChannel()); +AnalogEncoder::AnalogEncoder(AnalogInput* analogInput, double fullRange, + double expectedZero) + : m_analogInput{analogInput, wpi::NullDeleter{}} { + Init(fullRange, expectedZero); } -static bool DoubleEquals(double a, double b) { - constexpr double epsilon = 0.00001; - return std::abs(a - b) < epsilon; +AnalogEncoder::AnalogEncoder(std::shared_ptr analogInput, + double fullRange, double expectedZero) + : m_analogInput{std::move(analogInput)} { + Init(fullRange, expectedZero); } -units::turn_t AnalogEncoder::Get() const { - if (m_simPosition) { - return units::turn_t{m_simPosition.Get()}; - } +void AnalogEncoder::Init(double fullRange, double expectedZero) { + m_simDevice = hal::SimDevice{"AnalogEncoder", m_analogInput->GetChannel()}; - // 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++) { - auto counter = m_counter.Get(); - auto pos = m_analogInput->GetVoltage(); - auto counter2 = m_counter.Get(); - auto pos2 = m_analogInput->GetVoltage(); - if (counter == counter2 && DoubleEquals(pos, pos2)) { - pos = pos / frc::RobotController::GetVoltage5V(); - units::turn_t turns{counter + pos - m_positionOffset}; - m_lastPosition = turns; - return turns; - } + if (m_simDevice) { + m_simPosition = m_simDevice.CreateDouble("Position", false, 0.0); } - FRC_ReportError( - warn::Warning, - "Failed to read Analog Encoder. Potential Speed Overrun. Returning last " - "value"); - return m_lastPosition; + m_fullRange = fullRange; + m_expectedZero = expectedZero; + + wpi::SendableRegistry::AddLW(this, "Analog Encoder", + m_analogInput->GetChannel()); } -double AnalogEncoder::GetAbsolutePosition() const { - if (m_simAbsolutePosition) { - return m_simAbsolutePosition.Get(); +double AnalogEncoder::Get() const { + if (m_simPosition) { + return m_simPosition.Get(); } - return m_analogInput->GetVoltage() / frc::RobotController::GetVoltage5V(); -} + double pos = m_analogInput->GetVoltage() / RobotController::GetVoltage5V(); -double AnalogEncoder::GetPositionOffset() const { - return m_positionOffset; -} + // Map sensor range if range isn't full + pos = MapSensorRange(pos); -void AnalogEncoder::SetPositionOffset(double offset) { - m_positionOffset = std::clamp(offset, 0.0, 1.0); -} - -void AnalogEncoder::SetDistancePerRotation(double distancePerRotation) { - m_distancePerRotation = distancePerRotation; -} + // Compute full range and offset + pos = pos * m_fullRange - m_expectedZero; -double AnalogEncoder::GetDistancePerRotation() const { - return m_distancePerRotation; + // Map from 0 - Full Range + double result = InputModulus(pos, 0.0, m_fullRange); + // Invert if necessary + if (m_isInverted) { + return m_fullRange - result; + } + return result; } -double AnalogEncoder::GetDistance() const { - return Get().value() * GetDistancePerRotation(); +void AnalogEncoder::SetVoltagePercentageRange(double min, double max) { + m_sensorMin = std::clamp(min, 0.0, 1.0); + m_sensorMax = std::clamp(max, 0.0, 1.0); } -void AnalogEncoder::Reset() { - m_counter.Reset(); - m_positionOffset = - m_analogInput->GetVoltage() / frc::RobotController::GetVoltage5V(); +void AnalogEncoder::SetInverted(bool inverted) { + m_isInverted = inverted; } int AnalogEncoder::GetChannel() const { return m_analogInput->GetChannel(); } +double AnalogEncoder::MapSensorRange(double pos) const { + if (pos < m_sensorMin) { + pos = m_sensorMin; + } + if (pos > m_sensorMax) { + pos = m_sensorMax; + } + pos = (pos - m_sensorMin) / (m_sensorMax - m_sensorMin); + return pos; +} + void AnalogEncoder::InitSendable(wpi::SendableBuilder& builder) { builder.SetSmartDashboardType("AbsoluteEncoder"); builder.AddDoubleProperty( - "Distance", [this] { return this->GetDistance(); }, nullptr); - builder.AddDoubleProperty( - "Distance Per Rotation", - [this] { return this->GetDistancePerRotation(); }, nullptr); + "Position", [this] { return this->Get(); }, nullptr); } diff --git a/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp b/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp index f2ac77fb827..f127cf2ed1b 100644 --- a/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp +++ b/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp @@ -7,108 +7,137 @@ #include #include -#include "frc/Counter.h" #include "frc/DigitalInput.h" #include "frc/DigitalSource.h" #include "frc/DutyCycle.h" #include "frc/Errors.h" +#include "frc/MathUtil.h" using namespace frc; DutyCycleEncoder::DutyCycleEncoder(int channel) : m_dutyCycle{std::make_shared( std::make_shared(channel))} { - Init(); + Init(1.0, 0.0); } DutyCycleEncoder::DutyCycleEncoder(DutyCycle& dutyCycle) : m_dutyCycle{&dutyCycle, wpi::NullDeleter{}} { - Init(); + Init(1.0, 0.0); } DutyCycleEncoder::DutyCycleEncoder(DutyCycle* dutyCycle) : m_dutyCycle{dutyCycle, wpi::NullDeleter{}} { - Init(); + Init(1.0, 0.0); } DutyCycleEncoder::DutyCycleEncoder(std::shared_ptr dutyCycle) : m_dutyCycle{std::move(dutyCycle)} { - Init(); + Init(1.0, 0.0); } DutyCycleEncoder::DutyCycleEncoder(DigitalSource& digitalSource) : m_dutyCycle{std::make_shared(digitalSource)} { - Init(); + Init(1.0, 0.0); } DutyCycleEncoder::DutyCycleEncoder(DigitalSource* digitalSource) : m_dutyCycle{std::make_shared(digitalSource)} { - Init(); + Init(1.0, 0.0); } DutyCycleEncoder::DutyCycleEncoder(std::shared_ptr digitalSource) : m_dutyCycle{std::make_shared(digitalSource)} { - Init(); + Init(1.0, 0.0); } -void DutyCycleEncoder::Init() { +DutyCycleEncoder::DutyCycleEncoder(int channel, double fullRange, + double expectedZero) + : m_dutyCycle{std::make_shared( + std::make_shared(channel))} { + Init(fullRange, expectedZero); +} + +DutyCycleEncoder::DutyCycleEncoder(DutyCycle& dutyCycle, double fullRange, + double expectedZero) + : m_dutyCycle{&dutyCycle, wpi::NullDeleter{}} { + Init(fullRange, expectedZero); +} + +DutyCycleEncoder::DutyCycleEncoder(DutyCycle* dutyCycle, double fullRange, + double expectedZero) + : m_dutyCycle{dutyCycle, wpi::NullDeleter{}} { + Init(fullRange, expectedZero); +} + +DutyCycleEncoder::DutyCycleEncoder(std::shared_ptr dutyCycle, + double fullRange, double expectedZero) + : m_dutyCycle{std::move(dutyCycle)} { + Init(fullRange, expectedZero); +} + +DutyCycleEncoder::DutyCycleEncoder(DigitalSource& digitalSource, + double fullRange, double expectedZero) + : m_dutyCycle{std::make_shared(digitalSource)} { + Init(fullRange, expectedZero); +} + +DutyCycleEncoder::DutyCycleEncoder(DigitalSource* digitalSource, + double fullRange, double expectedZero) + : m_dutyCycle{std::make_shared(digitalSource)} { + Init(fullRange, expectedZero); +} + +DutyCycleEncoder::DutyCycleEncoder(std::shared_ptr digitalSource, + double fullRange, double expectedZero) + : m_dutyCycle{std::make_shared(digitalSource)} { + Init(fullRange, expectedZero); +} + +void DutyCycleEncoder::Init(double fullRange, double expectedZero) { m_simDevice = hal::SimDevice{"DutyCycle:DutyCycleEncoder", m_dutyCycle->GetSourceChannel()}; if (m_simDevice) { - m_simPosition = - m_simDevice.CreateDouble("position", hal::SimDevice::kInput, 0.0); - m_simDistancePerRotation = m_simDevice.CreateDouble( - "distance_per_rot", hal::SimDevice::kOutput, 1.0); - m_simAbsolutePosition = - m_simDevice.CreateDouble("absPosition", hal::SimDevice::kInput, 0.0); + m_simPosition = m_simDevice.CreateDouble("Position", false, 0.0); m_simIsConnected = - m_simDevice.CreateBoolean("connected", hal::SimDevice::kInput, true); - } else { - m_analogTrigger = std::make_unique(m_dutyCycle.get()); - m_analogTrigger->SetLimitsDutyCycle(0.25, 0.75); - m_counter = std::make_unique(); - m_counter->SetUpSource( - m_analogTrigger->CreateOutput(AnalogTriggerType::kRisingPulse)); - m_counter->SetDownSource( - m_analogTrigger->CreateOutput(AnalogTriggerType::kFallingPulse)); + m_simDevice.CreateBoolean("Connected", hal::SimDevice::kInput, true); } + m_fullRange = fullRange; + m_expectedZero = expectedZero; + wpi::SendableRegistry::AddLW(this, "DutyCycle Encoder", m_dutyCycle->GetSourceChannel()); } -static bool DoubleEquals(double a, double b) { - constexpr double epsilon = 0.00001; - return std::abs(a - b) < epsilon; -} - -units::turn_t DutyCycleEncoder::Get() const { +double DutyCycleEncoder::Get() const { if (m_simPosition) { - return units::turn_t{m_simPosition.Get()}; + 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++) { - auto counter = m_counter->Get(); - auto pos = m_dutyCycle->GetOutput(); - auto counter2 = m_counter->Get(); - auto pos2 = m_dutyCycle->GetOutput(); - if (counter == counter2 && DoubleEquals(pos, pos2)) { - // map sensor range - pos = MapSensorRange(pos); - units::turn_t turns{counter + pos - m_positionOffset}; - m_lastPosition = turns; - return turns; - } + double pos; + // Compute output percentage (0-1) + if (m_period.value() == 0.0) { + pos = m_dutyCycle->GetOutput(); + } else { + auto highTime = m_dutyCycle->GetHighTime(); + pos = highTime / m_period; } - FRC_ReportError( - warn::Warning, - "Failed to read DutyCycle Encoder. Potential Speed Overrun. Returning " - "last value"); - return m_lastPosition; + // 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 = InputModulus(pos, 0.0, m_fullRange); + // Invert if necessary + if (m_isInverted) { + return m_fullRange - result; + } + return result; } double DutyCycleEncoder::MapSensorRange(double pos) const { @@ -122,54 +151,15 @@ double DutyCycleEncoder::MapSensorRange(double pos) const { return pos; } -double DutyCycleEncoder::GetAbsolutePosition() const { - if (m_simAbsolutePosition) { - return m_simAbsolutePosition.Get(); - } - - return MapSensorRange(m_dutyCycle->GetOutput()); -} - -double DutyCycleEncoder::GetPositionOffset() const { - return m_positionOffset; -} - -void DutyCycleEncoder::SetPositionOffset(double offset) { - m_positionOffset = std::clamp(offset, 0.0, 1.0); -} - void DutyCycleEncoder::SetDutyCycleRange(double min, double max) { m_sensorMin = std::clamp(min, 0.0, 1.0); m_sensorMax = std::clamp(max, 0.0, 1.0); } -void DutyCycleEncoder::SetDistancePerRotation(double distancePerRotation) { - m_distancePerRotation = distancePerRotation; - m_simDistancePerRotation.Set(distancePerRotation); -} - -double DutyCycleEncoder::GetDistancePerRotation() const { - return m_distancePerRotation; -} - -double DutyCycleEncoder::GetDistance() const { - return Get().value() * GetDistancePerRotation(); -} - int DutyCycleEncoder::GetFrequency() const { return m_dutyCycle->GetFrequency(); } -void DutyCycleEncoder::Reset() { - if (m_counter) { - m_counter->Reset(); - } - if (m_simPosition) { - m_simPosition.Set(0); - } - m_positionOffset = GetAbsolutePosition(); -} - bool DutyCycleEncoder::IsConnected() const { if (m_simIsConnected) { return m_simIsConnected.Get(); @@ -184,6 +174,18 @@ void DutyCycleEncoder::SetConnectedFrequencyThreshold(int frequency) { m_frequencyThreshold = frequency; } +void DutyCycleEncoder::SetInverted(bool inverted) { + m_isInverted = inverted; +} + +void DutyCycleEncoder::SetAssumedFrequency(units::hertz_t frequency) { + if (frequency.value() == 0) { + m_period = 0_s; + } else { + m_period = 1.0 / frequency; + } +} + int DutyCycleEncoder::GetFPGAIndex() const { return m_dutyCycle->GetFPGAIndex(); } @@ -195,10 +197,7 @@ int DutyCycleEncoder::GetSourceChannel() const { void DutyCycleEncoder::InitSendable(wpi::SendableBuilder& builder) { builder.SetSmartDashboardType("AbsoluteEncoder"); builder.AddDoubleProperty( - "Distance", [this] { return this->GetDistance(); }, nullptr); - builder.AddDoubleProperty( - "Distance Per Rotation", - [this] { return this->GetDistancePerRotation(); }, nullptr); + "Position", [this] { return this->Get(); }, nullptr); builder.AddDoubleProperty( "Is Connected", [this] { return this->IsConnected(); }, nullptr); } diff --git a/wpilibc/src/main/native/cpp/simulation/AnalogEncoderSim.cpp b/wpilibc/src/main/native/cpp/simulation/AnalogEncoderSim.cpp index 435e29e378a..7bc58bd514c 100644 --- a/wpilibc/src/main/native/cpp/simulation/AnalogEncoderSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/AnalogEncoderSim.cpp @@ -14,18 +14,10 @@ AnalogEncoderSim::AnalogEncoderSim(const frc::AnalogEncoder& encoder) { m_positionSim = deviceSim.GetDouble("Position"); } -void AnalogEncoderSim::SetPosition(frc::Rotation2d angle) { - SetTurns(angle.Degrees()); +void AnalogEncoderSim::Set(double value) { + m_positionSim.Set(value); } -void AnalogEncoderSim::SetTurns(units::turn_t turns) { - m_positionSim.Set(turns.value()); -} - -units::turn_t AnalogEncoderSim::GetTurns() { - return units::turn_t{m_positionSim.Get()}; -} - -frc::Rotation2d AnalogEncoderSim::GetPosition() { - return units::radian_t{GetTurns()}; +double AnalogEncoderSim::Get() { + return m_positionSim.Get(); } diff --git a/wpilibc/src/main/native/cpp/simulation/DutyCycleEncoderSim.cpp b/wpilibc/src/main/native/cpp/simulation/DutyCycleEncoderSim.cpp index 3df775df9a3..8d5292ad8ca 100644 --- a/wpilibc/src/main/native/cpp/simulation/DutyCycleEncoderSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/DutyCycleEncoderSim.cpp @@ -14,38 +14,16 @@ DutyCycleEncoderSim::DutyCycleEncoderSim(const frc::DutyCycleEncoder& encoder) DutyCycleEncoderSim::DutyCycleEncoderSim(int channel) { frc::sim::SimDeviceSim deviceSim{"DutyCycle:DutyCycleEncoder", channel}; - m_simPosition = deviceSim.GetDouble("position"); - m_simDistancePerRotation = deviceSim.GetDouble("distance_per_rot"); - m_simAbsolutePosition = deviceSim.GetDouble("absPosition"); - m_simIsConnected = deviceSim.GetBoolean("connected"); + m_simPosition = deviceSim.GetDouble("Position"); + m_simIsConnected = deviceSim.GetBoolean("Connected"); } double DutyCycleEncoderSim::Get() { return m_simPosition.Get(); } -void DutyCycleEncoderSim::Set(units::turn_t turns) { - m_simPosition.Set(turns.value()); -} - -double DutyCycleEncoderSim::GetDistance() { - return m_simPosition.Get() * m_simDistancePerRotation.Get(); -} - -void DutyCycleEncoderSim::SetDistance(double distance) { - m_simPosition.Set(distance / m_simDistancePerRotation.Get()); -} - -double DutyCycleEncoderSim::GetAbsolutePosition() { - return m_simAbsolutePosition.Get(); -} - -void DutyCycleEncoderSim::SetAbsolutePosition(double position) { - m_simAbsolutePosition.Set(position); -} - -double DutyCycleEncoderSim::GetDistancePerRotation() { - return m_simDistancePerRotation.Get(); +void DutyCycleEncoderSim::Set(double value) { + m_simPosition.Set(value); } bool DutyCycleEncoderSim::IsConnected() { diff --git a/wpilibc/src/main/native/include/frc/AnalogEncoder.h b/wpilibc/src/main/native/include/frc/AnalogEncoder.h index 1860273ab43..b03974c0d67 100644 --- a/wpilibc/src/main/native/include/frc/AnalogEncoder.h +++ b/wpilibc/src/main/native/include/frc/AnalogEncoder.h @@ -8,13 +8,9 @@ #include #include -#include #include #include -#include "frc/AnalogTrigger.h" -#include "frc/Counter.h" - namespace frc { class AnalogInput; @@ -52,86 +48,49 @@ class AnalogEncoder : public wpi::Sendable, */ explicit AnalogEncoder(std::shared_ptr analogInput); - ~AnalogEncoder() override = default; - - AnalogEncoder(AnalogEncoder&&) = default; - AnalogEncoder& operator=(AnalogEncoder&&) = default; - - /** - * Reset the Encoder distance to zero. - */ - void Reset(); - /** - * Get the encoder value since the last reset. - * - * This is reported in rotations since the last reset. + * Construct a new AnalogEncoder attached to a specific AnalogIn channel. * - * @return the encoder value in rotations + * @param channel the analog input channel to attach to */ - units::turn_t Get() const; + AnalogEncoder(int channel, double fullRange, double expectedZero); /** - * Get the absolute position of the analog encoder. - * - *

GetAbsolutePosition() - GetPositionOffset() will give an encoder - * absolute position relative to the last reset. This could potentially be - * negative, which needs to be accounted for. - * - *

This will not account for rollovers, and will always be just the raw - * absolute position. + * Construct a new AnalogEncoder attached to a specific AnalogInput. * - * @return the absolute position + * @param analogInput the analog input to attach to */ - double GetAbsolutePosition() const; + AnalogEncoder(AnalogInput& analogInput, double fullRange, double expectedZero); /** - * Get the offset of position relative to the last reset. - * - * GetAbsolutePosition() - GetPositionOffset() will give an encoder absolute - * position relative to the last reset. This could potentially be negative, - * which needs to be accounted for. + * Construct a new AnalogEncoder attached to a specific AnalogInput. * - * @return the position offset + * @param analogInput the analog input to attach to */ - double GetPositionOffset() const; + AnalogEncoder(AnalogInput* analogInput, double fullRange, double expectedZero); /** - * Set the position offset. - * - *

This must be in the range of 0-1. + * Construct a new AnalogEncoder attached to a specific AnalogInput. * - * @param offset the offset + * @param analogInput the analog input to attach to */ - void SetPositionOffset(double offset); + AnalogEncoder(std::shared_ptr analogInput, double fullRange, double expectedZero); - /** - * 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 the 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 - */ - void SetDistancePerRotation(double distancePerRotation); + ~AnalogEncoder() override = default; - /** - * Get the distance per rotation for this encoder. - * - * @return The scale factor that will be used to convert rotation to useful - * units. - */ - double GetDistancePerRotation() const; + AnalogEncoder(AnalogEncoder&&) = default; + AnalogEncoder& operator=(AnalogEncoder&&) = default; /** - * Get the distance the sensor has driven since the last reset as scaled by - * the value from SetDistancePerRotation. + * Get the encoder value. * - * @return The distance driven since the last reset + * @return the encoder value scaled by the full range input */ - double GetDistance() const; + double Get() const; + + void SetVoltagePercentageRange(double min, double max); + + void SetInverted(bool inverted); /** * Get the channel number. @@ -143,17 +102,18 @@ class AnalogEncoder : public wpi::Sendable, void InitSendable(wpi::SendableBuilder& builder) override; private: - void Init(); + void Init(double fullRange, double expectedZero); + double MapSensorRange(double pos) const; std::shared_ptr m_analogInput; - AnalogTrigger m_analogTrigger; - Counter m_counter; - double m_positionOffset = 0; - double m_distancePerRotation = 1.0; - mutable units::turn_t m_lastPosition{0.0}; + bool m_ownsAnalogInput; + double m_fullRange; + double m_expectedZero; + double m_sensorMin{0.0}; + double m_sensorMax{1.0}; + bool m_isInverted{false}; hal::SimDevice m_simDevice; hal::SimDouble m_simPosition; - hal::SimDouble m_simAbsolutePosition; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h b/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h index 5d04ada87d1..936e401e31c 100644 --- a/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h +++ b/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h @@ -8,13 +8,11 @@ #include #include -#include +#include +#include #include #include -#include "frc/AnalogTrigger.h" -#include "frc/Counter.h" - namespace frc { class DutyCycle; class DigitalSource; @@ -76,84 +74,92 @@ class DutyCycleEncoder : public wpi::Sendable, */ explicit DutyCycleEncoder(std::shared_ptr digitalSource); - ~DutyCycleEncoder() override = default; - - DutyCycleEncoder(DutyCycleEncoder&&) = default; - DutyCycleEncoder& operator=(DutyCycleEncoder&&) = default; - /** - * Get the frequency in Hz of the duty cycle signal from the encoder. + * Construct a new DutyCycleEncoder on a specific channel. * - * @return duty cycle frequency in Hz + * @param channel the channel to attach to */ - int GetFrequency() const; + DutyCycleEncoder(int channel, double fullRange, double expectedZero); /** - * Get if the sensor is connected - * - * This uses the duty cycle frequency to determine if the sensor is connected. - * By default, a value of 100 Hz is used as the threshold, and this value can - * be changed with SetConnectedFrequencyThreshold. + * Construct a new DutyCycleEncoder attached to an existing DutyCycle object. * - * @return true if the sensor is connected + * @param dutyCycle the duty cycle to attach to */ - bool IsConnected() const; + DutyCycleEncoder(DutyCycle& dutyCycle, double fullRange, double expectedZero); /** - * Change the frequency threshold for detecting connection used by - * IsConnected. + * Construct a new DutyCycleEncoder attached to an existing DutyCycle object. * - * @param frequency the minimum frequency in Hz. + * @param dutyCycle the duty cycle to attach to */ - void SetConnectedFrequencyThreshold(int frequency); + DutyCycleEncoder(DutyCycle* dutyCycle, double fullRange, double expectedZero); /** - * Reset the Encoder distance to zero. + * Construct a new DutyCycleEncoder attached to an existing DutyCycle object. + * + * @param dutyCycle the duty cycle to attach to */ - void Reset(); + DutyCycleEncoder(std::shared_ptr dutyCycle, double fullRange, double expectedZero); /** - * Get the encoder value since the last reset. - * - * This is reported in rotations since the last reset. + * Construct a new DutyCycleEncoder attached to a DigitalSource object. * - * @return the encoder value in rotations + * @param digitalSource the digital source to attach to */ - units::turn_t Get() const; + DutyCycleEncoder(DigitalSource& digitalSource, double fullRange, double expectedZero); /** - * Get the absolute position of the duty cycle encoder encoder. + * Construct a new DutyCycleEncoder attached to a DigitalSource object. * - *

GetAbsolutePosition() - GetPositionOffset() will give an encoder - * absolute position relative to the last reset. This could potentially be - * negative, which needs to be accounted for. + * @param digitalSource the digital source to attach to + */ + DutyCycleEncoder(DigitalSource* digitalSource, double fullRange, double expectedZero); + + /** + * Construct a new DutyCycleEncoder attached to a DigitalSource object. * - *

This will not account for rollovers, and will always be just the raw - * absolute position. + * @param digitalSource the digital source to attach to + */ + DutyCycleEncoder(std::shared_ptr digitalSource, double fullRange, double expectedZero); + + ~DutyCycleEncoder() override = default; + + DutyCycleEncoder(DutyCycleEncoder&&) = default; + DutyCycleEncoder& operator=(DutyCycleEncoder&&) = default; + + /** + * Get the frequency in Hz of the duty cycle signal from the encoder. * - * @return the absolute position + * @return duty cycle frequency in Hz */ - double GetAbsolutePosition() const; + int GetFrequency() const; /** - * Get the offset of position relative to the last reset. + * Get if the sensor is connected * - * GetAbsolutePosition() - GetPositionOffset() will give an encoder absolute - * position relative to the last reset. This could potentially be negative, - * which needs to be accounted for. + * This uses the duty cycle frequency to determine if the sensor is connected. + * By default, a value of 100 Hz is used as the threshold, and this value can + * be changed with SetConnectedFrequencyThreshold. * - * @return the position offset + * @return true if the sensor is connected */ - double GetPositionOffset() const; + bool IsConnected() const; /** - * Set the position offset. + * Change the frequency threshold for detecting connection used by + * IsConnected. * - *

This must be in the range of 0-1. + * @param frequency the minimum frequency in Hz. + */ + void SetConnectedFrequencyThreshold(int frequency); + + /** + * Get the encoder value. * - * @param offset the offset + * @return the encoder value scaled by the full range input */ - void SetPositionOffset(double offset); + double Get() const; /** * Set the encoder duty cycle range. As the encoder needs to maintain a duty @@ -170,33 +176,9 @@ class DutyCycleEncoder : public wpi::Sendable, */ void SetDutyCycleRange(double min, double max); - /** - * 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 the 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 - */ - void SetDistancePerRotation(double distancePerRotation); - - /** - * Get the distance per rotation for this encoder. - * - * @return The scale factor that will be used to convert rotation to useful - * units. - */ - double GetDistancePerRotation() const; + void SetAssumedFrequency(units::hertz_t frequency); - /** - * Get the distance the sensor has driven since the last reset as scaled by - * the value from SetDistancePerRotation. - * - * @return The distance driven since the last reset - */ - double GetDistance() const; + void SetInverted(bool inverted); /** * Get the FPGA index for the DutyCycleEncoder. @@ -215,23 +197,20 @@ class DutyCycleEncoder : public wpi::Sendable, void InitSendable(wpi::SendableBuilder& builder) override; private: - void Init(); + void Init(double fullRange, double expectedZero); double MapSensorRange(double pos) const; std::shared_ptr m_dutyCycle; - std::unique_ptr m_analogTrigger; - std::unique_ptr m_counter; int m_frequencyThreshold = 100; - double m_positionOffset = 0; - double m_distancePerRotation = 1.0; - mutable units::turn_t m_lastPosition{0.0}; - double m_sensorMin = 0; - double m_sensorMax = 1; + double m_fullRange; + double m_expectedZero; + units::second_t m_period{0_s}; + double m_sensorMin{0.0}; + double m_sensorMax{1.0}; + bool m_isInverted{false}; hal::SimDevice m_simDevice; hal::SimDouble m_simPosition; - hal::SimDouble m_simAbsolutePosition; - hal::SimDouble m_simDistancePerRotation; hal::SimBoolean m_simIsConnected; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/frc/simulation/AnalogEncoderSim.h b/wpilibc/src/main/native/include/frc/simulation/AnalogEncoderSim.h index 6d763267044..00b2b876b1c 100644 --- a/wpilibc/src/main/native/include/frc/simulation/AnalogEncoderSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/AnalogEncoderSim.h @@ -28,28 +28,16 @@ class AnalogEncoderSim { explicit AnalogEncoderSim(const AnalogEncoder& encoder); /** - * Set the position using an Rotation2d. + * Set the position. * - * @param angle The angle. + * @param angle The position. */ - void SetPosition(Rotation2d angle); - - /** - * Set the position of the encoder. - * - * @param turns The position. - */ - void SetTurns(units::turn_t turns); + void Set(double value); /** * Get the simulated position. */ - units::turn_t GetTurns(); - - /** - * Get the position as a Rotation2d. - */ - Rotation2d GetPosition(); + double Get(); private: hal::SimDouble m_positionSim; diff --git a/wpilibc/src/main/native/include/frc/simulation/DutyCycleEncoderSim.h b/wpilibc/src/main/native/include/frc/simulation/DutyCycleEncoderSim.h index 18c04f0562c..06bb1df1a57 100644 --- a/wpilibc/src/main/native/include/frc/simulation/DutyCycleEncoderSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/DutyCycleEncoderSim.h @@ -33,55 +33,18 @@ class DutyCycleEncoderSim { explicit DutyCycleEncoderSim(int channel); /** - * Get the position in turns. + * Get the position. * * @return The position. */ double Get(); /** - * Set the position in turns. + * Set the position. * * @param turns The position. */ - void Set(units::turn_t turns); - - /** - * Get the distance. - * - * @return The distance. - */ - - double GetDistance(); - - /** - * Set the distance. - * - * @param distance The distance. - */ - void SetDistance(double distance); - - /** - * Get the absolute position. - * - * @return The absolute position - */ - double GetAbsolutePosition(); - - /** - * Set the absolute position. - * - * @param position The absolute position - */ - void SetAbsolutePosition(double position); - - /** - * Get the distance per rotation for this encoder. - * - * @return The scale factor that will be used to convert rotation to useful - * units. - */ - double GetDistancePerRotation(); + void Set(double value); /** * Get if the encoder is connected. @@ -99,8 +62,6 @@ class DutyCycleEncoderSim { private: hal::SimDouble m_simPosition; - hal::SimDouble m_simDistancePerRotation; - hal::SimDouble m_simAbsolutePosition; hal::SimBoolean m_simIsConnected; }; diff --git a/wpilibc/src/test/native/cpp/simulation/AnalogEncoderSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/AnalogEncoderSimTest.cpp index 3f0ea459207..fdfd5cec647 100644 --- a/wpilibc/src/test/native/cpp/simulation/AnalogEncoderSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/AnalogEncoderSimTest.cpp @@ -17,12 +17,10 @@ TEST(AnalogEncoderSimTest, Basic) { frc::AnalogInput ai(0); - frc::AnalogEncoder encoder{ai}; + frc::AnalogEncoder encoder{ai, 360, 0}; frc::sim::AnalogEncoderSim encoderSim{encoder}; - encoderSim.SetPosition(180_deg); - EXPECT_NEAR(encoder.Get().value(), 0.5, 1E-8); - EXPECT_NEAR(encoderSim.GetTurns().value(), 0.5, 1E-8); - EXPECT_NEAR(encoderSim.GetPosition().Radians().value(), std::numbers::pi, - 1E-8); + encoderSim.Set(180); + EXPECT_NEAR(encoder.Get(), 180, 1E-8); + EXPECT_NEAR(encoderSim.Get(), 180, 1E-8); } diff --git a/wpilibc/src/test/native/cpp/simulation/DutyCycleEncoderSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/DutyCycleEncoderSimTest.cpp index e81c63e3e2c..714edf655ec 100644 --- a/wpilibc/src/test/native/cpp/simulation/DutyCycleEncoderSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/DutyCycleEncoderSimTest.cpp @@ -15,42 +15,14 @@ namespace frc::sim { TEST(DutyCycleEncoderSimTest, Set) { HAL_Initialize(500, 0); - DutyCycleEncoder enc{0}; + DutyCycleEncoder enc{0, 10, 0}; DutyCycleEncoderSim sim(enc); - constexpr units::turn_t kTestValue{5.67}; + constexpr double kTestValue{5.67}; sim.Set(kTestValue); EXPECT_EQ(kTestValue, enc.Get()); } -TEST(DutyCycleEncoderSimTest, SetDistance) { - HAL_Initialize(500, 0); - - DutyCycleEncoder enc{0}; - DutyCycleEncoderSim sim(enc); - sim.SetDistance(19.1); - EXPECT_EQ(19.1, enc.GetDistance()); -} - -TEST(DutyCycleEncoderSimTest, SetDistancePerRotation) { - HAL_Initialize(500, 0); - - DutyCycleEncoder enc{0}; - DutyCycleEncoderSim sim(enc); - sim.Set(units::turn_t{1.5}); - enc.SetDistancePerRotation(42); - EXPECT_EQ(63, enc.GetDistance()); -} - -TEST(DutyCycleEncoderSimTest, SetAbsolutePosition) { - HAL_Initialize(500, 0); - - DutyCycleEncoder enc{0}; - DutyCycleEncoderSim sim(enc); - sim.SetAbsolutePosition(0.75); - EXPECT_EQ(0.75, enc.GetAbsolutePosition()); -} - TEST(DutyCycleEncoderSimTest, SetIsConnected) { HAL_Initialize(500, 0); @@ -62,15 +34,4 @@ TEST(DutyCycleEncoderSimTest, SetIsConnected) { EXPECT_FALSE(enc.IsConnected()); } -TEST(DutyCycleEncoderSimTest, Reset) { - HAL_Initialize(500, 0); - - DutyCycleEncoder enc{0}; - DutyCycleEncoderSim sim(enc); - sim.SetDistance(2.5); - EXPECT_EQ(2.5, enc.GetDistance()); - enc.Reset(); - EXPECT_EQ(0, enc.GetDistance()); -} - } // namespace frc::sim diff --git a/wpilibcExamples/src/main/cpp/examples/DutyCycleEncoder/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/DutyCycleEncoder/cpp/Robot.cpp index a9343d8aea4..73c91616753 100644 --- a/wpilibcExamples/src/main/cpp/examples/DutyCycleEncoder/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/DutyCycleEncoder/cpp/Robot.cpp @@ -5,15 +5,34 @@ #include #include #include +#include + +constexpr double fullRange = 1.3; +constexpr double expectedZero = 0.0; class Robot : public frc::TimedRobot { - // Duty cycle encoder on channel 0 - frc::DutyCycleEncoder m_dutyCycleEncoder{0}; + // 2nd parameter is the range of values. This sensor will output between + // 0 and the passed in value. + // 3rd parameter is the the physical value where you want "0" to be. How + // to measure this is fairly easy. Set the value to 0, place the mechanism + // where you want "0" to be, and observe the value on the dashboard, That + // is the value to enter for the 3rd parameter. + frc::DutyCycleEncoder m_dutyCycleEncoder{0, fullRange, expectedZero}; public: void RobotInit() override { - // Set to 0.5 units per rotation - m_dutyCycleEncoder.SetDistancePerRotation(0.5); + // If you know the frequency of your sensor, uncomment the following + // method, and set the method to the frequency of your sensor. + // This will result in more stable readings from the sensor. + // Do note that occasionally the datasheet cannot be trusted + // and you should measure this value. You can do so with either + // an oscilloscope, or by observing the "Frequency" output + // on the dashboard while running this sample. If you find + // the value jumping between the 2 values, enter halfway between + // those values. This number doesn't have to be perfect, + // just having a fairly close value will make the output readings + // much more stable. + m_dutyCycleEncoder.SetAssumedFrequency(967.8_Hz); } void RobotPeriodic() override { @@ -26,13 +45,24 @@ class Robot : public frc::TimedRobot { // Output of encoder auto output = m_dutyCycleEncoder.Get(); - // Output scaled by DistancePerPulse - auto distance = m_dutyCycleEncoder.GetDistance(); + // By default, the output will wrap around to the full range value + // when the sensor goes below 0. However, for moving mechanisms this + // is not usually ideal, as if 0 is set to a hard stop, its still + // possible for the sensor to move slightly past. If this happens + // The sensor will assume its now at the furthest away position, + // which control algorithms might not handle correctly. Therefore + // it can be a good idea to slightly shift the output so the sensor + // can go a bit negative before wrapping. Usually 10% or so is fine. + // This does not change where "0" is, so no calibration numbers need + // to be changed. + double percentOfRange = fullRange * 0.1; + double shiftedOutput = frc::InputModulus(output, 0 - percentOfRange, + fullRange - percentOfRange); frc::SmartDashboard::PutBoolean("Connected", connected); frc::SmartDashboard::PutNumber("Frequency", frequency); - frc::SmartDashboard::PutNumber("Output", output.value()); - frc::SmartDashboard::PutNumber("Distance", distance); + frc::SmartDashboard::PutNumber("Output", output); + frc::SmartDashboard::PutNumber("ShiftedOutput", shiftedOutput); } }; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java index 6b116731a5a..bead64af562 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java @@ -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 AnalogEncoderSim(AnalogEncoder encoder) { } /** - * Set the position using an {@link Rotation2d}. + * Set the position * - * @param angle The angle. + * @param angle 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 void setTurns(double turns) { * * @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); - } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java index 786c16eea17..660f09333a7 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java @@ -43,12 +43,12 @@ public double get() { } /** - * Set the position in turns. + * Set the position. * * @param turns The position. */ - public void set(double turns) { - m_simPosition.set(turns); + public void set(double value) { + m_simPosition.set(value); } /** diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSimTest.java index a04ce10bb0d..ea5edaafcf2 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSimTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSimTest.java @@ -6,7 +6,6 @@ import static org.junit.jupiter.api.Assertions.assertEquals; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.AnalogEncoder; import edu.wpi.first.wpilibj.AnalogInput; import org.junit.jupiter.api.Test; @@ -15,13 +14,12 @@ class AnalogEncoderSimTest { @Test void testBasic() { try (var analogInput = new AnalogInput(0); - var analogEncoder = new AnalogEncoder(analogInput)) { + var analogEncoder = new AnalogEncoder(analogInput, 360, 0)) { var encoderSim = new AnalogEncoderSim(analogEncoder); - encoderSim.setPosition(Rotation2d.fromDegrees(180)); - assertEquals(analogEncoder.get(), 0.5, 1E-8); - assertEquals(encoderSim.getTurns(), 0.5, 1E-8); - assertEquals(encoderSim.getPosition().getRadians(), Math.PI, 1E-8); + encoderSim.set(180); + assertEquals(analogEncoder.get(), 180, 1E-8); + assertEquals(encoderSim.get(), 180, 1E-8); } } } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSimTest.java index 42a6faa6282..3ccc05e7ac1 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSimTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSimTest.java @@ -15,7 +15,7 @@ class DutyCycleEncoderSimTest { @Test void setTest() { - try (DutyCycleEncoder encoder = new DutyCycleEncoder(0)) { + try (DutyCycleEncoder encoder = new DutyCycleEncoder(0, 5.67, 0)) { DutyCycleEncoderSim sim = new DutyCycleEncoderSim(encoder); sim.set(5.67); @@ -23,42 +23,6 @@ void setTest() { } } - @Test - void setDistanceTest() { - HAL.initialize(500, 0); - - try (DutyCycleEncoder encoder = new DutyCycleEncoder(0)) { - DutyCycleEncoderSim sim = new DutyCycleEncoderSim(encoder); - - sim.setDistance(19.1); - assertEquals(19.1, encoder.getDistance()); - } - } - - @Test - void setDistancePerRotationTest() { - HAL.initialize(500, 0); - - try (DutyCycleEncoder encoder = new DutyCycleEncoder(0)) { - DutyCycleEncoderSim sim = new DutyCycleEncoderSim(encoder); - sim.set(1.5); - encoder.setDistancePerRotation(42); - assertEquals(63.0, encoder.getDistance()); - } - } - - @Test - void setAbsolutePositionTest() { - HAL.initialize(500, 0); - - try (DutyCycleEncoder encoder = new DutyCycleEncoder(0)) { - DutyCycleEncoderSim sim = new DutyCycleEncoderSim(encoder); - - sim.setAbsolutePosition(0.75); - assertEquals(0.75, encoder.getAbsolutePosition()); - } - } - @Test void setIsConnectedTest() { HAL.initialize(500, 0); @@ -72,18 +36,4 @@ void setIsConnectedTest() { assertFalse(encoder.isConnected()); } } - - @Test - void resetTest() { - HAL.initialize(500, 0); - - try (DutyCycleEncoder encoder = new DutyCycleEncoder(0)) { - DutyCycleEncoderSim sim = new DutyCycleEncoderSim(encoder); - - sim.setDistance(2.5); - assertEquals(2.5, encoder.getDistance()); - encoder.reset(); - assertEquals(0.0, encoder.getDistance()); - } - } } From f48c09392484845aa1497c03c3e1350703616e78 Mon Sep 17 00:00:00 2001 From: Thad House Date: Sat, 2 Mar 2024 13:33:43 -0800 Subject: [PATCH 21/30] Fix unused variable --- wpilibc/src/main/native/include/frc/AnalogEncoder.h | 1 - 1 file changed, 1 deletion(-) diff --git a/wpilibc/src/main/native/include/frc/AnalogEncoder.h b/wpilibc/src/main/native/include/frc/AnalogEncoder.h index b03974c0d67..51bb27517a7 100644 --- a/wpilibc/src/main/native/include/frc/AnalogEncoder.h +++ b/wpilibc/src/main/native/include/frc/AnalogEncoder.h @@ -106,7 +106,6 @@ class AnalogEncoder : public wpi::Sendable, double MapSensorRange(double pos) const; std::shared_ptr m_analogInput; - bool m_ownsAnalogInput; double m_fullRange; double m_expectedZero; double m_sensorMin{0.0}; From a9a353f12cd2fa7877bd33e04ea79ca308c4108f Mon Sep 17 00:00:00 2001 From: Thad House Date: Sat, 2 Mar 2024 14:03:59 -0800 Subject: [PATCH 22/30] Add rollover support --- wpilibc/src/main/native/cpp/AnalogEncoder.cpp | 93 +++++++++++++++--- .../main/native/include/frc/AnalogEncoder.h | 12 +++ .../edu/wpi/first/wpilibj/AnalogEncoder.java | 95 ++++++++++++++++--- 3 files changed, 172 insertions(+), 28 deletions(-) diff --git a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp index c4381f0ef31..def9ea33a1e 100644 --- a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp +++ b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp @@ -8,6 +8,7 @@ #include #include "frc/AnalogInput.h" +#include "frc/Counter.h" #include "frc/Errors.h" #include "frc/RobotController.h" #include "frc/MathUtil.h" @@ -73,21 +74,12 @@ double AnalogEncoder::Get() const { return m_simPosition.Get(); } - double pos = m_analogInput->GetVoltage() / 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 = InputModulus(pos, 0.0, m_fullRange); - // Invert if necessary - if (m_isInverted) { - return m_fullRange - result; + if (m_rolloverCounter) { + return GetWithRollovers(); + } else { + double analog = m_analogInput->GetVoltage(); + return GetWithoutRollovers(analog); } - return result; } void AnalogEncoder::SetVoltagePercentageRange(double min, double max) { @@ -119,3 +111,76 @@ void AnalogEncoder::InitSendable(wpi::SendableBuilder& builder) { builder.AddDoubleProperty( "Position", [this] { return this->Get(); }, nullptr); } + +void AnalogEncoder::ConfigureRolloverSupport(bool enable) { + if (enable && !m_rolloverCounter) { + m_rolloverTrigger = std::make_unique(m_analogInput.get()); + m_rolloverTrigger->SetLimitsVoltage(1.25, 3.75); + m_rolloverCounter = std::make_unique(); + m_rolloverCounter->SetUpSource( + m_rolloverTrigger->CreateOutput(AnalogTriggerType::kRisingPulse)); + m_rolloverCounter->SetDownSource( + m_rolloverTrigger->CreateOutput(AnalogTriggerType::kFallingPulse)); + } else if (!enable && m_rolloverCounter) { + m_rolloverCounter = nullptr; + m_rolloverTrigger = nullptr; + } +} + +void AnalogEncoder::ResetRollovers() { + if (m_rolloverCounter) { + m_rolloverCounter->Reset(); + } +} + +double AnalogEncoder::GetWithoutRollovers(double analog) const { + 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 = InputModulus(pos, 0.0, m_fullRange); + // Invert if necessary + if (m_isInverted) { + return m_fullRange - result; + } + return result; +} + +static bool DoubleEquals(double a, double b) { + constexpr double epsilon = 0.00001; + return std::abs(a - b) < epsilon; +} + +double AnalogEncoder::GetWithRollovers() const { + // 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++) { + auto counter = m_rolloverCounter->Get(); + auto pos = m_analogInput->GetVoltage(); + auto counter2 = m_rolloverCounter->Get(); + auto pos2 = m_analogInput->GetVoltage(); + if (counter == counter2 && DoubleEquals(pos, pos2)) { + pos = GetWithoutRollovers(pos); + + if (m_isInverted) { + pos = pos - counter; + } else { + pos = pos + counter; + } + + m_lastPosition = pos; + return pos; + } + } + + FRC_ReportError( + warn::Warning, + "Failed to read Analog Encoder. Potential Speed Overrun. Returning last " + "value"); + return m_lastPosition; +} \ No newline at end of file diff --git a/wpilibc/src/main/native/include/frc/AnalogEncoder.h b/wpilibc/src/main/native/include/frc/AnalogEncoder.h index 51bb27517a7..c7831f2a852 100644 --- a/wpilibc/src/main/native/include/frc/AnalogEncoder.h +++ b/wpilibc/src/main/native/include/frc/AnalogEncoder.h @@ -13,6 +13,8 @@ namespace frc { class AnalogInput; +class Counter; +class AnalogTrigger; /** * Class for supporting continuous analog encoders, such as the US Digital MA3. @@ -101,10 +103,16 @@ class AnalogEncoder : public wpi::Sendable, void InitSendable(wpi::SendableBuilder& builder) override; + void ConfigureRolloverSupport(bool enable); + void ResetRollovers(); + private: void Init(double fullRange, double expectedZero); double MapSensorRange(double pos) const; + double GetWithoutRollovers(double analog) const; + double GetWithRollovers() const; + std::shared_ptr m_analogInput; double m_fullRange; double m_expectedZero; @@ -112,6 +120,10 @@ class AnalogEncoder : public wpi::Sendable, double m_sensorMax{1.0}; bool m_isInverted{false}; + std::unique_ptr m_rolloverTrigger{nullptr}; + std::unique_ptr m_rolloverCounter{nullptr}; + mutable double m_lastPosition{0.0}; + hal::SimDevice m_simDevice; hal::SimDouble m_simPosition; }; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java index 617c50a6b42..8465953e74c 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java @@ -11,6 +11,7 @@ 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. @@ -24,6 +25,10 @@ public class AnalogEncoder implements Sendable, AutoCloseable { private double m_sensorMax = 1.0; private boolean m_isInverted; + private AnalogTrigger m_rolloverTrigger; + private Counter m_rolloverCounter; + private double m_lastPosition; + private SimDevice m_simDevice; private SimDouble m_simPosition; @@ -105,21 +110,12 @@ public double get() { return m_simPosition.get(); } - double pos = m_analogInput.getVoltage() / 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; + if (m_rolloverCounter != null) { + return getWithRollovers(); + } else { + double analog = m_analogInput.getVoltage(); + return getWithoutRollovers(analog); } - return result; } /** @@ -158,6 +154,77 @@ public void close() { } } + public void configureRolloverSupport(boolean enable) { + if (enable && m_rolloverCounter == null) { + m_rolloverTrigger = new AnalogTrigger(m_analogInput); + m_rolloverTrigger.setLimitsVoltage(1.25, 3.75); + m_rolloverCounter = new Counter(); + m_rolloverCounter.setUpSource(m_rolloverTrigger.createOutput(AnalogTriggerType.kRisingPulse)); + m_rolloverCounter.setDownSource(m_rolloverTrigger.createOutput(AnalogTriggerType.kFallingPulse)); + } else if (!enable && m_rolloverCounter != null) { + m_rolloverTrigger.close(); + m_rolloverTrigger = null; + m_rolloverCounter.close(); + m_rolloverCounter = null; + } + } + + public void resetRollovers() { + if (m_rolloverCounter != null) { + m_rolloverCounter.reset(); + } + } + + private boolean doubleEquals(double a, double b) { + double epsilon = 0.00001d; + return Math.abs(a - b) < epsilon; + } + + private double getWithRollovers() { + // 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_rolloverCounter.get(); + double pos = m_analogInput.getVoltage(); + double counter2 = m_rolloverCounter.get(); + double pos2 = m_analogInput.getVoltage(); + if (counter == counter2 && doubleEquals(pos, pos2)) { + pos = getWithoutRollovers(pos); + + if (m_isInverted) { + pos = pos - counter; + } else { + pos = pos + counter; + } + + m_lastPosition = pos; + return pos; + } + } + + DriverStation.reportWarning( + "Failed to read Analog Encoder. Potential Speed Overrun. Returning last value", false); + return m_lastPosition; + } + + private double getWithoutRollovers(double analog) { + 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; + } + return result; + } + @Override public void initSendable(SendableBuilder builder) { builder.setSmartDashboardType("AbsoluteEncoder"); From 1d7177609d34d6d52db2ea66be04c93943d5e941 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sat, 2 Mar 2024 22:08:48 +0000 Subject: [PATCH 23/30] Formatting fixes --- myRobot/src/main/java/frc/robot/MyRobot.java | 4 +- wpilibc/src/main/native/cpp/AnalogEncoder.cpp | 4 +- .../main/native/include/frc/AnalogEncoder.h | 9 ++-- .../native/include/frc/DutyCycleEncoder.h | 12 ++++-- .../examples/DutyCycleEncoder/cpp/Robot.cpp | 2 +- .../edu/wpi/first/wpilibj/AnalogEncoder.java | 15 +++---- .../wpi/first/wpilibj/DutyCycleEncoder.java | 41 +++++++------------ .../examples/dutycycleencoder/Robot.java | 9 ++-- 8 files changed, 43 insertions(+), 53 deletions(-) diff --git a/myRobot/src/main/java/frc/robot/MyRobot.java b/myRobot/src/main/java/frc/robot/MyRobot.java index d63e534882b..5772b1056c5 100644 --- a/myRobot/src/main/java/frc/robot/MyRobot.java +++ b/myRobot/src/main/java/frc/robot/MyRobot.java @@ -11,6 +11,7 @@ public class MyRobot extends TimedRobot { DutyCycleEncoder pot; + /** * This function is run when the robot is first started up and should be used for any * initialization code. @@ -48,7 +49,8 @@ public void robotPeriodic() { SmartDashboard.putNumber("DC4", val); SmartDashboard.putNumber("DC3", MathUtil.invertInput(val, 1, 4)); SmartDashboard.putNumber("DC2", MathUtil.inputModulus(pot.get(), -0.1, 0.9)); - SmartDashboard.putNumber("DC", MathUtil.inputModulus(MathUtil.invertInput(pot.get(), 0, 1), -0.1, 0.9)); + SmartDashboard.putNumber( + "DC", MathUtil.inputModulus(MathUtil.invertInput(pot.get(), 0, 1), -0.1, 0.9)); SmartDashboard.putNumber("Freq", pot.getFrequency()); } } diff --git a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp index def9ea33a1e..1de52f86624 100644 --- a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp +++ b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp @@ -10,8 +10,8 @@ #include "frc/AnalogInput.h" #include "frc/Counter.h" #include "frc/Errors.h" -#include "frc/RobotController.h" #include "frc/MathUtil.h" +#include "frc/RobotController.h" using namespace frc; @@ -183,4 +183,4 @@ double AnalogEncoder::GetWithRollovers() const { "Failed to read Analog Encoder. Potential Speed Overrun. Returning last " "value"); return m_lastPosition; -} \ No newline at end of file +} diff --git a/wpilibc/src/main/native/include/frc/AnalogEncoder.h b/wpilibc/src/main/native/include/frc/AnalogEncoder.h index c7831f2a852..5162ce17eee 100644 --- a/wpilibc/src/main/native/include/frc/AnalogEncoder.h +++ b/wpilibc/src/main/native/include/frc/AnalogEncoder.h @@ -62,21 +62,24 @@ class AnalogEncoder : public wpi::Sendable, * * @param analogInput the analog input to attach to */ - AnalogEncoder(AnalogInput& analogInput, double fullRange, double expectedZero); + AnalogEncoder(AnalogInput& analogInput, double fullRange, + double expectedZero); /** * Construct a new AnalogEncoder attached to a specific AnalogInput. * * @param analogInput the analog input to attach to */ - AnalogEncoder(AnalogInput* analogInput, double fullRange, double expectedZero); + AnalogEncoder(AnalogInput* analogInput, double fullRange, + double expectedZero); /** * Construct a new AnalogEncoder attached to a specific AnalogInput. * * @param analogInput the analog input to attach to */ - AnalogEncoder(std::shared_ptr analogInput, double fullRange, double expectedZero); + AnalogEncoder(std::shared_ptr analogInput, double fullRange, + double expectedZero); ~AnalogEncoder() override = default; diff --git a/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h b/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h index 936e401e31c..10819387372 100644 --- a/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h +++ b/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h @@ -100,28 +100,32 @@ class DutyCycleEncoder : public wpi::Sendable, * * @param dutyCycle the duty cycle to attach to */ - DutyCycleEncoder(std::shared_ptr dutyCycle, double fullRange, double expectedZero); + DutyCycleEncoder(std::shared_ptr dutyCycle, double fullRange, + double expectedZero); /** * Construct a new DutyCycleEncoder attached to a DigitalSource object. * * @param digitalSource the digital source to attach to */ - DutyCycleEncoder(DigitalSource& digitalSource, double fullRange, double expectedZero); + DutyCycleEncoder(DigitalSource& digitalSource, double fullRange, + double expectedZero); /** * Construct a new DutyCycleEncoder attached to a DigitalSource object. * * @param digitalSource the digital source to attach to */ - DutyCycleEncoder(DigitalSource* digitalSource, double fullRange, double expectedZero); + DutyCycleEncoder(DigitalSource* digitalSource, double fullRange, + double expectedZero); /** * Construct a new DutyCycleEncoder attached to a DigitalSource object. * * @param digitalSource the digital source to attach to */ - DutyCycleEncoder(std::shared_ptr digitalSource, double fullRange, double expectedZero); + DutyCycleEncoder(std::shared_ptr digitalSource, + double fullRange, double expectedZero); ~DutyCycleEncoder() override = default; diff --git a/wpilibcExamples/src/main/cpp/examples/DutyCycleEncoder/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/DutyCycleEncoder/cpp/Robot.cpp index 73c91616753..3889d1c4dad 100644 --- a/wpilibcExamples/src/main/cpp/examples/DutyCycleEncoder/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/DutyCycleEncoder/cpp/Robot.cpp @@ -3,9 +3,9 @@ // the WPILib BSD license file in the root directory of this project. #include +#include #include #include -#include constexpr double fullRange = 1.3; constexpr double expectedZero = 0.0; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java index 8465953e74c..8a76d3e7c14 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java @@ -13,9 +13,7 @@ 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. - */ +/** Class for supporting continuous analog encoders, such as the US Digital MA3. */ public class AnalogEncoder implements Sendable, AutoCloseable { private final AnalogInput m_analogInput; private boolean m_ownsAnalogInput; @@ -100,8 +98,7 @@ private double mapSensorRange(double pos) { /** * Get the encoder value since the last reset. * - *

- * This is reported in rotations since the last reset. + *

This is reported in rotations since the last reset. * * @return the encoder value in rotations */ @@ -119,9 +116,8 @@ public double get() { } /** - * 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. + * 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. * * @param min minimum voltage percentage (0-1 range) * @param max maximum voltage percentage (0-1 range) @@ -160,7 +156,8 @@ public void configureRolloverSupport(boolean enable) { m_rolloverTrigger.setLimitsVoltage(1.25, 3.75); m_rolloverCounter = new Counter(); m_rolloverCounter.setUpSource(m_rolloverTrigger.createOutput(AnalogTriggerType.kRisingPulse)); - m_rolloverCounter.setDownSource(m_rolloverTrigger.createOutput(AnalogTriggerType.kFallingPulse)); + m_rolloverCounter.setDownSource( + m_rolloverTrigger.createOutput(AnalogTriggerType.kFallingPulse)); } else if (!enable && m_rolloverCounter != null) { m_rolloverTrigger.close(); m_rolloverTrigger = null; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java index 5e321b6dc48..1ff3d1beaff 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java @@ -13,8 +13,7 @@ import edu.wpi.first.util.sendable.SendableRegistry; /** - * Class for supporting duty cycle/PWM encoders, such as the US Digital MA3 with - * PWM Output, the + * Class for supporting duty cycle/PWM encoders, such as the US Digital MA3 with PWM Output, the * CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag Encoder. */ public class DutyCycleEncoder implements Sendable, AutoCloseable { @@ -128,8 +127,7 @@ private double mapSensorRange(double pos) { /** * Get the encoder value since the last reset. * - *

- * This is reported in rotations since the last reset. + *

This is reported in rotations since the last reset. * * @return the encoder value in rotations */ @@ -163,16 +161,11 @@ public double get() { } /** - * Set the encoder duty cycle range. As the encoder needs to maintain a duty - * cycle, the duty cycle - * cannot go all the way to 0% or all the way to 100%. For example, an encoder - * with a 4096 us - * period might have a minimum duty cycle of 1 us / 4096 us and a maximum duty - * cycle of 4095 / - * 4096 us. Setting the range will result in an encoder duty cycle less than or - * equal to the - * minimum being output as 0 rotation, the duty cycle greater than or equal to - * the maximum being + * Set the encoder duty cycle range. As the encoder needs to maintain a duty cycle, the duty cycle + * cannot go all the way to 0% or all the way to 100%. For example, an encoder with a 4096 us + * period might have a minimum duty cycle of 1 us / 4096 us and a maximum duty cycle of 4095 / + * 4096 us. Setting the range will result in an encoder duty cycle less than or equal to the + * minimum being output as 0 rotation, the duty cycle greater than or equal to the maximum being * output as 1 rotation, and values in between linearly scaled from 0 to 1. * * @param min minimum duty cycle (0-1 range) @@ -195,11 +188,8 @@ public int getFrequency() { /** * Get if the sensor is connected * - *

- * This uses the duty cycle frequency to determine if the sensor is connected. - * By default, a - * value of 100 Hz is used as the threshold, and this value can be changed with - * {@link + *

This uses the duty cycle frequency to determine if the sensor is connected. By default, a + * value of 100 Hz is used as the threshold, and this value can be changed with {@link * #setConnectedFrequencyThreshold(int)}. * * @return true if the sensor is connected @@ -212,8 +202,7 @@ public boolean isConnected() { } /** - * Change the frequency threshold for detecting connection used by - * {@link #isConnected()}. + * Change the frequency threshold for detecting connection used by {@link #isConnected()}. * * @param frequency the minimum frequency in Hz. */ @@ -228,12 +217,10 @@ public void setConnectedFrequencyThreshold(int frequency) { /** * Sets the assumed frequency of the connected device. * - *

- * 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. + *

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 */ diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java index 70b3c4cf5cf..75216ec2c5b 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java @@ -9,10 +9,7 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -/** - * This example shows how to use a duty cycle encoder for devices such as an - * arm or elevator. - */ +/** This example shows how to use a duty cycle encoder for devices such as an arm or elevator. */ public class Robot extends TimedRobot { private DutyCycleEncoder m_dutyCycleEncoder; private final double m_fullRange = 1.3; @@ -28,7 +25,6 @@ public void robotInit() { // is the value to enter for the 3rd parameter. m_dutyCycleEncoder = new DutyCycleEncoder(0, m_fullRange, m_expectedZero); - // If you know the frequency of your sensor, uncomment the following // method, and set the method to the frequency of your sensor. // This will result in more stable readings from the sensor. @@ -65,7 +61,8 @@ public void robotPeriodic() { // This does not change where "0" is, so no calibration numbers need // to be changed. double percentOfRange = m_fullRange * 0.1; - double shiftedOutput = MathUtil.inputModulus(output, 0 - percentOfRange, m_fullRange - percentOfRange); + double shiftedOutput = + MathUtil.inputModulus(output, 0 - percentOfRange, m_fullRange - percentOfRange); SmartDashboard.putBoolean("Connected", connected); SmartDashboard.putNumber("Frequency", frequency); From e9a0339b8da46deeac8d9152aa111694b1395fc1 Mon Sep 17 00:00:00 2001 From: Thad House Date: Sat, 2 Mar 2024 19:03:42 -0800 Subject: [PATCH 24/30] Analog Encoder Destructor --- wpilibc/src/main/native/cpp/AnalogEncoder.cpp | 2 ++ wpilibc/src/main/native/include/frc/AnalogEncoder.h | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp index 1de52f86624..49da8c0fdb8 100644 --- a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp +++ b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp @@ -15,6 +15,8 @@ using namespace frc; +AnalogEncoder::~AnalogEncoder() {} + AnalogEncoder::AnalogEncoder(int channel) : AnalogEncoder(std::make_shared(channel)) {} diff --git a/wpilibc/src/main/native/include/frc/AnalogEncoder.h b/wpilibc/src/main/native/include/frc/AnalogEncoder.h index 5162ce17eee..d31952804dc 100644 --- a/wpilibc/src/main/native/include/frc/AnalogEncoder.h +++ b/wpilibc/src/main/native/include/frc/AnalogEncoder.h @@ -81,7 +81,7 @@ class AnalogEncoder : public wpi::Sendable, AnalogEncoder(std::shared_ptr analogInput, double fullRange, double expectedZero); - ~AnalogEncoder() override = default; + ~AnalogEncoder() override; AnalogEncoder(AnalogEncoder&&) = default; AnalogEncoder& operator=(AnalogEncoder&&) = default; From 45da00de75e959f1ced39161e03ce20bf9c9d4a7 Mon Sep 17 00:00:00 2001 From: Thad House Date: Sat, 2 Mar 2024 19:15:54 -0800 Subject: [PATCH 25/30] Docs --- .../main/native/include/frc/AnalogEncoder.h | 29 ++++++++++++++ .../native/include/frc/DutyCycleEncoder.h | 38 ++++++++++++++++++ .../edu/wpi/first/wpilibj/AnalogEncoder.java | 39 ++++++++++++++++--- .../wpi/first/wpilibj/DutyCycleEncoder.java | 19 ++++++++- 4 files changed, 119 insertions(+), 6 deletions(-) diff --git a/wpilibc/src/main/native/include/frc/AnalogEncoder.h b/wpilibc/src/main/native/include/frc/AnalogEncoder.h index d31952804dc..0c2bedbdd04 100644 --- a/wpilibc/src/main/native/include/frc/AnalogEncoder.h +++ b/wpilibc/src/main/native/include/frc/AnalogEncoder.h @@ -93,8 +93,21 @@ class AnalogEncoder : public wpi::Sendable, */ double Get() const; + /** + * 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. + * + * @param min minimum voltage percentage (0-1 range) + * @param max maximum voltage percentage (0-1 range) + */ void SetVoltagePercentageRange(double min, double max); + /** + * Set if this encoder is inverted. + * + * @param inverted true to invert the encoder, false otherwise + */ void SetInverted(bool inverted); /** @@ -106,7 +119,23 @@ class AnalogEncoder : public wpi::Sendable, void InitSendable(wpi::SendableBuilder& builder) override; + /** + * Configures if this encoder has rollover counting enabled. + * + *

By default, the encoder will not count rollovers. This + * behavior is very rarely needed, and is usually a sign you are + * using the wrong encoder type. + * + * @param enable True to enable rollover counting, false to disable. + */ void ConfigureRolloverSupport(bool enable); + + /** + * Reset the number of rollovers that have been counted. + * + *

This has no effect unless configureRolloverSupport(true) + * has been called. + */ void ResetRollovers(); private: diff --git a/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h b/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h index 10819387372..9587f57db78 100644 --- a/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h +++ b/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h @@ -27,6 +27,8 @@ class DutyCycleEncoder : public wpi::Sendable, public: /** * Construct a new DutyCycleEncoder on a specific channel. + * + *

This has a fullRange of 1 and an expectedZero of 0. * * @param channel the channel to attach to */ @@ -35,6 +37,8 @@ class DutyCycleEncoder : public wpi::Sendable, /** * Construct a new DutyCycleEncoder attached to an existing DutyCycle object. * + *

This has a fullRange of 1 and an expectedZero of 0. + * * @param dutyCycle the duty cycle to attach to */ explicit DutyCycleEncoder(DutyCycle& dutyCycle); @@ -42,6 +46,8 @@ class DutyCycleEncoder : public wpi::Sendable, /** * Construct a new DutyCycleEncoder attached to an existing DutyCycle object. * + *

This has a fullRange of 1 and an expectedZero of 0. + * * @param dutyCycle the duty cycle to attach to */ explicit DutyCycleEncoder(DutyCycle* dutyCycle); @@ -49,6 +55,8 @@ class DutyCycleEncoder : public wpi::Sendable, /** * Construct a new DutyCycleEncoder attached to an existing DutyCycle object. * + *

This has a fullRange of 1 and an expectedZero of 0. + * * @param dutyCycle the duty cycle to attach to */ explicit DutyCycleEncoder(std::shared_ptr dutyCycle); @@ -56,6 +64,8 @@ class DutyCycleEncoder : public wpi::Sendable, /** * Construct a new DutyCycleEncoder attached to a DigitalSource object. * + *

This has a fullRange of 1 and an expectedZero of 0. + * * @param digitalSource the digital source to attach to */ explicit DutyCycleEncoder(DigitalSource& digitalSource); @@ -63,6 +73,8 @@ class DutyCycleEncoder : public wpi::Sendable, /** * Construct a new DutyCycleEncoder attached to a DigitalSource object. * + *

This has a fullRange of 1 and an expectedZero of 0. + * * @param digitalSource the digital source to attach to */ explicit DutyCycleEncoder(DigitalSource* digitalSource); @@ -70,6 +82,8 @@ class DutyCycleEncoder : public wpi::Sendable, /** * Construct a new DutyCycleEncoder attached to a DigitalSource object. * + *

This has a fullRange of 1 and an expectedZero of 0. + * * @param digitalSource the digital source to attach to */ explicit DutyCycleEncoder(std::shared_ptr digitalSource); @@ -78,6 +92,8 @@ class DutyCycleEncoder : public wpi::Sendable, * 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() */ DutyCycleEncoder(int channel, double fullRange, double expectedZero); @@ -85,6 +101,8 @@ class DutyCycleEncoder : public wpi::Sendable, * 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() */ DutyCycleEncoder(DutyCycle& dutyCycle, double fullRange, double expectedZero); @@ -92,6 +110,8 @@ class DutyCycleEncoder : public wpi::Sendable, * 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() */ DutyCycleEncoder(DutyCycle* dutyCycle, double fullRange, double expectedZero); @@ -99,6 +119,8 @@ class DutyCycleEncoder : public wpi::Sendable, * 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() */ DutyCycleEncoder(std::shared_ptr dutyCycle, double fullRange, double expectedZero); @@ -107,6 +129,8 @@ class DutyCycleEncoder : public wpi::Sendable, * Construct a new DutyCycleEncoder attached to a DigitalSource object. * * @param digitalSource 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() */ DutyCycleEncoder(DigitalSource& digitalSource, double fullRange, double expectedZero); @@ -115,6 +139,8 @@ class DutyCycleEncoder : public wpi::Sendable, * Construct a new DutyCycleEncoder attached to a DigitalSource object. * * @param digitalSource 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() */ DutyCycleEncoder(DigitalSource* digitalSource, double fullRange, double expectedZero); @@ -123,6 +149,8 @@ class DutyCycleEncoder : public wpi::Sendable, * Construct a new DutyCycleEncoder attached to a DigitalSource object. * * @param digitalSource 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() */ DutyCycleEncoder(std::shared_ptr digitalSource, double fullRange, double expectedZero); @@ -180,8 +208,18 @@ class DutyCycleEncoder : public wpi::Sendable, */ void SetDutyCycleRange(double min, double max); + /** + * Set if this encoder is inverted. + * + * @param inverted true to invert the encoder, false otherwise + */ void SetAssumedFrequency(units::hertz_t frequency); + /** + * Set if this encoder is inverted. + * + * @param inverted true to invert the encoder, false otherwise + */ void SetInverted(bool inverted); /** diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java index 8a76d3e7c14..0aabde26e8b 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java @@ -34,6 +34,8 @@ public class AnalogEncoder implements Sendable, AutoCloseable { * 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, double fullRange, double expectedZero) { this(new AnalogInput(channel), fullRange, expectedZero); @@ -44,6 +46,8 @@ public AnalogEncoder(int channel, double fullRange, double expectedZero) { * 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) { @@ -53,6 +57,8 @@ public AnalogEncoder(AnalogInput analogInput, double fullRange, double expectedZ /** * Construct a new AnalogEncoder attached to a specific AnalogIn channel. + * + *

This has a fullRange of 1 and an expectedZero of 0. * * @param channel the analog input channel to attach to */ @@ -62,6 +68,8 @@ public AnalogEncoder(int channel) { /** * Construct a new AnalogEncoder attached to a specific AnalogInput. + * + *

This has a fullRange of 1 and an expectedZero of 0. * * @param analogInput the analog input to attach to */ @@ -96,11 +104,12 @@ private double mapSensorRange(double pos) { } /** - * Get the encoder value since the last reset. + * Get the encoder value. * - *

This is reported in rotations since the last reset. + *

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) { @@ -127,6 +136,11 @@ public void setVoltagePercentageRange(double min, double max) { m_sensorMax = MathUtil.clamp(max, 0.0, 1.0); } + /** + * Set if this encoder is inverted. + * + * @param inverted true to invert the encoder, false otherwise + */ public void setInverted(boolean inverted) { m_isInverted = inverted; } @@ -150,7 +164,16 @@ public void close() { } } - public void configureRolloverSupport(boolean enable) { + /** + * Configures if this encoder has rollover counting enabled. + * + *

By default, the encoder will not count rollovers. This + * behavior is very rarely needed, and is usually a sign you are + * using the wrong encoder type. + * + * @param enable True to enable rollover counting, false to disable. + */ + public void configureRolloverCounting(boolean enable) { if (enable && m_rolloverCounter == null) { m_rolloverTrigger = new AnalogTrigger(m_analogInput); m_rolloverTrigger.setLimitsVoltage(1.25, 3.75); @@ -166,7 +189,13 @@ public void configureRolloverSupport(boolean enable) { } } - public void resetRollovers() { + /** + * Reset the number of rollovers that have been counted. + * + *

This has no effect unless configureRolloverSupport(true) + * has been called. + */ + public void resetRolloverCount() { if (m_rolloverCounter != null) { m_rolloverCounter.reset(); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java index 1ff3d1beaff..76e82b54ba4 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java @@ -36,6 +36,8 @@ public class DutyCycleEncoder implements Sendable, AutoCloseable { * 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, double fullRange, double expectedZero) { @@ -49,6 +51,8 @@ public DutyCycleEncoder(int channel, double fullRange, double 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, double fullRange, double expectedZero) { @@ -60,6 +64,8 @@ public DutyCycleEncoder(DutyCycle dutyCycle, double fullRange, double expectedZe * 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) { @@ -71,6 +77,8 @@ public DutyCycleEncoder(DigitalSource source, double fullRange, double expectedZ /** * Construct a new DutyCycleEncoder on a specific channel. * + *

This has a fullRange of 1 and an expectedZero of 0. + * * @param channel the channel to attach to */ @SuppressWarnings("this-escape") @@ -81,6 +89,8 @@ public DutyCycleEncoder(int channel) { /** * Construct a new DutyCycleEncoder attached to an existing DutyCycle object. * + *

This has a fullRange of 1 and an expectedZero of 0. + * * @param dutyCycle the duty cycle to attach to */ @SuppressWarnings("this-escape") @@ -90,6 +100,8 @@ public DutyCycleEncoder(DutyCycle dutyCycle) { /** * Construct a new DutyCycleEncoder attached to a DigitalSource object. + * + *

This has a fullRange of 1 and an expectedZero of 0. * * @param source the digital source to attach to */ @@ -222,7 +234,7 @@ public void setConnectedFrequencyThreshold(int frequency) { * frequency of the sensor to this function. This will use that frequency to compute the DutyCycle * percentage, rather than the computed frequency. * - * @param frequency + * @param frequency the assumed frequency of the sensor */ public void setAssumedFrequency(double frequency) { if (frequency == 0.0) { @@ -232,6 +244,11 @@ public void setAssumedFrequency(double frequency) { } } + /** + * Set if this encoder is inverted. + * + * @param inverted true to invert the encoder, false otherwise + */ public void setInverted(boolean inverted) { m_isInverted = inverted; } From 202a9e8d705588b8c6549cadf1b420044eb85a05 Mon Sep 17 00:00:00 2001 From: Thad House Date: Sat, 2 Mar 2024 19:23:55 -0800 Subject: [PATCH 26/30] Docs and format --- myRobot/src/main/java/frc/robot/MyRobot.java | 20 ++----------------- .../main/native/include/frc/AnalogEncoder.h | 16 +++++++++++++++ .../native/include/frc/DutyCycleEncoder.h | 18 ++++++++--------- .../include/frc/simulation/AnalogEncoderSim.h | 2 +- .../frc/simulation/DutyCycleEncoderSim.h | 2 +- .../edu/wpi/first/wpilibj/AnalogEncoder.java | 12 +++++------ .../wpi/first/wpilibj/DutyCycleEncoder.java | 8 ++++---- 7 files changed, 39 insertions(+), 39 deletions(-) diff --git a/myRobot/src/main/java/frc/robot/MyRobot.java b/myRobot/src/main/java/frc/robot/MyRobot.java index 5772b1056c5..a5f88bd3f36 100644 --- a/myRobot/src/main/java/frc/robot/MyRobot.java +++ b/myRobot/src/main/java/frc/robot/MyRobot.java @@ -4,23 +4,15 @@ package frc.robot; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class MyRobot extends TimedRobot { - DutyCycleEncoder pot; - /** * This function is run when the robot is first started up and should be used for any * initialization code. */ @Override - public void robotInit() { - pot = new DutyCycleEncoder(0, 3, 0.5); - pot.setAssumedFrequency(967.77); - } + public void robotInit() {} /** This function is run once each time the robot enters autonomous mode. */ @Override @@ -44,13 +36,5 @@ public void testPeriodic() {} /** This function is called periodically during all modes. */ @Override - public void robotPeriodic() { - double val = pot.get() + 1; - SmartDashboard.putNumber("DC4", val); - SmartDashboard.putNumber("DC3", MathUtil.invertInput(val, 1, 4)); - SmartDashboard.putNumber("DC2", MathUtil.inputModulus(pot.get(), -0.1, 0.9)); - SmartDashboard.putNumber( - "DC", MathUtil.inputModulus(MathUtil.invertInput(pot.get(), 0, 1), -0.1, 0.9)); - SmartDashboard.putNumber("Freq", pot.getFrequency()); - } + public void robotPeriodic() {} } diff --git a/wpilibc/src/main/native/include/frc/AnalogEncoder.h b/wpilibc/src/main/native/include/frc/AnalogEncoder.h index 0c2bedbdd04..398f57ce80f 100644 --- a/wpilibc/src/main/native/include/frc/AnalogEncoder.h +++ b/wpilibc/src/main/native/include/frc/AnalogEncoder.h @@ -25,6 +25,8 @@ class AnalogEncoder : public wpi::Sendable, /** * Construct a new AnalogEncoder attached to a specific AnalogIn channel. * + *

This has a fullRange of 1 and an expectedZero of 0. + * * @param channel the analog input channel to attach to */ explicit AnalogEncoder(int channel); @@ -32,6 +34,8 @@ class AnalogEncoder : public wpi::Sendable, /** * Construct a new AnalogEncoder attached to a specific AnalogInput. * + *

This has a fullRange of 1 and an expectedZero of 0. + * * @param analogInput the analog input to attach to */ explicit AnalogEncoder(AnalogInput& analogInput); @@ -39,6 +43,8 @@ class AnalogEncoder : public wpi::Sendable, /** * Construct a new AnalogEncoder attached to a specific AnalogInput. * + *

This has a fullRange of 1 and an expectedZero of 0. + * * @param analogInput the analog input to attach to */ explicit AnalogEncoder(AnalogInput* analogInput); @@ -46,6 +52,8 @@ class AnalogEncoder : public wpi::Sendable, /** * Construct a new AnalogEncoder attached to a specific AnalogInput. * + *

This has a fullRange of 1 and an expectedZero of 0. + * * @param analogInput the analog input to attach to */ explicit AnalogEncoder(std::shared_ptr analogInput); @@ -54,6 +62,8 @@ class AnalogEncoder : public wpi::Sendable, * 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() */ AnalogEncoder(int channel, double fullRange, double expectedZero); @@ -61,6 +71,8 @@ class AnalogEncoder : public wpi::Sendable, * 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() */ AnalogEncoder(AnalogInput& analogInput, double fullRange, double expectedZero); @@ -69,6 +81,8 @@ class AnalogEncoder : public wpi::Sendable, * 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() */ AnalogEncoder(AnalogInput* analogInput, double fullRange, double expectedZero); @@ -77,6 +91,8 @@ class AnalogEncoder : public wpi::Sendable, * 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() */ AnalogEncoder(std::shared_ptr analogInput, double fullRange, double expectedZero); diff --git a/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h b/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h index 9587f57db78..1d35b411d38 100644 --- a/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h +++ b/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h @@ -27,7 +27,7 @@ class DutyCycleEncoder : public wpi::Sendable, public: /** * Construct a new DutyCycleEncoder on a specific channel. - * + * *

This has a fullRange of 1 and an expectedZero of 0. * * @param channel the channel to attach to @@ -38,7 +38,7 @@ class DutyCycleEncoder : public wpi::Sendable, * Construct a new DutyCycleEncoder attached to an existing DutyCycle object. * *

This has a fullRange of 1 and an expectedZero of 0. - * + * * @param dutyCycle the duty cycle to attach to */ explicit DutyCycleEncoder(DutyCycle& dutyCycle); @@ -47,7 +47,7 @@ class DutyCycleEncoder : public wpi::Sendable, * Construct a new DutyCycleEncoder attached to an existing DutyCycle object. * *

This has a fullRange of 1 and an expectedZero of 0. - * + * * @param dutyCycle the duty cycle to attach to */ explicit DutyCycleEncoder(DutyCycle* dutyCycle); @@ -56,7 +56,7 @@ class DutyCycleEncoder : public wpi::Sendable, * Construct a new DutyCycleEncoder attached to an existing DutyCycle object. * *

This has a fullRange of 1 and an expectedZero of 0. - * + * * @param dutyCycle the duty cycle to attach to */ explicit DutyCycleEncoder(std::shared_ptr dutyCycle); @@ -65,7 +65,7 @@ class DutyCycleEncoder : public wpi::Sendable, * Construct a new DutyCycleEncoder attached to a DigitalSource object. * *

This has a fullRange of 1 and an expectedZero of 0. - * + * * @param digitalSource the digital source to attach to */ explicit DutyCycleEncoder(DigitalSource& digitalSource); @@ -74,7 +74,7 @@ class DutyCycleEncoder : public wpi::Sendable, * Construct a new DutyCycleEncoder attached to a DigitalSource object. * *

This has a fullRange of 1 and an expectedZero of 0. - * + * * @param digitalSource the digital source to attach to */ explicit DutyCycleEncoder(DigitalSource* digitalSource); @@ -83,7 +83,7 @@ class DutyCycleEncoder : public wpi::Sendable, * Construct a new DutyCycleEncoder attached to a DigitalSource object. * *

This has a fullRange of 1 and an expectedZero of 0. - * + * * @param digitalSource the digital source to attach to */ explicit DutyCycleEncoder(std::shared_ptr digitalSource); @@ -210,14 +210,14 @@ class DutyCycleEncoder : public wpi::Sendable, /** * Set if this encoder is inverted. - * + * * @param inverted true to invert the encoder, false otherwise */ void SetAssumedFrequency(units::hertz_t frequency); /** * Set if this encoder is inverted. - * + * * @param inverted true to invert the encoder, false otherwise */ void SetInverted(bool inverted); diff --git a/wpilibc/src/main/native/include/frc/simulation/AnalogEncoderSim.h b/wpilibc/src/main/native/include/frc/simulation/AnalogEncoderSim.h index 00b2b876b1c..3cd8f223063 100644 --- a/wpilibc/src/main/native/include/frc/simulation/AnalogEncoderSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/AnalogEncoderSim.h @@ -30,7 +30,7 @@ class AnalogEncoderSim { /** * Set the position. * - * @param angle The position. + * @param value The position. */ void Set(double value); diff --git a/wpilibc/src/main/native/include/frc/simulation/DutyCycleEncoderSim.h b/wpilibc/src/main/native/include/frc/simulation/DutyCycleEncoderSim.h index 06bb1df1a57..e5d84f12fd8 100644 --- a/wpilibc/src/main/native/include/frc/simulation/DutyCycleEncoderSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/DutyCycleEncoderSim.h @@ -42,7 +42,7 @@ class DutyCycleEncoderSim { /** * Set the position. * - * @param turns The position. + * @param value The position. */ void Set(double value); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java index 0aabde26e8b..8668ee914b2 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java @@ -57,7 +57,7 @@ public AnalogEncoder(AnalogInput analogInput, double fullRange, double expectedZ /** * Construct a new AnalogEncoder attached to a specific AnalogIn channel. - * + * *

This has a fullRange of 1 and an expectedZero of 0. * * @param channel the analog input channel to attach to @@ -68,7 +68,7 @@ public AnalogEncoder(int channel) { /** * Construct a new AnalogEncoder attached to a specific AnalogInput. - * + * *

This has a fullRange of 1 and an expectedZero of 0. * * @param analogInput the analog input to attach to @@ -138,7 +138,7 @@ public void setVoltagePercentageRange(double min, double max) { /** * Set if this encoder is inverted. - * + * * @param inverted true to invert the encoder, false otherwise */ public void setInverted(boolean inverted) { @@ -166,11 +166,11 @@ public void close() { /** * Configures if this encoder has rollover counting enabled. - * + * *

By default, the encoder will not count rollovers. This * behavior is very rarely needed, and is usually a sign you are * using the wrong encoder type. - * + * * @param enable True to enable rollover counting, false to disable. */ public void configureRolloverCounting(boolean enable) { @@ -191,7 +191,7 @@ public void configureRolloverCounting(boolean enable) { /** * Reset the number of rollovers that have been counted. - * + * *

This has no effect unless configureRolloverSupport(true) * has been called. */ diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java index 76e82b54ba4..22cf1995c84 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java @@ -78,7 +78,7 @@ public DutyCycleEncoder(DigitalSource source, double fullRange, double expectedZ * Construct a new DutyCycleEncoder on a specific channel. * *

This has a fullRange of 1 and an expectedZero of 0. - * + * * @param channel the channel to attach to */ @SuppressWarnings("this-escape") @@ -90,7 +90,7 @@ public DutyCycleEncoder(int channel) { * Construct a new DutyCycleEncoder attached to an existing DutyCycle object. * *

This has a fullRange of 1 and an expectedZero of 0. - * + * * @param dutyCycle the duty cycle to attach to */ @SuppressWarnings("this-escape") @@ -100,7 +100,7 @@ public DutyCycleEncoder(DutyCycle dutyCycle) { /** * Construct a new DutyCycleEncoder attached to a DigitalSource object. - * + * *

This has a fullRange of 1 and an expectedZero of 0. * * @param source the digital source to attach to @@ -246,7 +246,7 @@ public void setAssumedFrequency(double frequency) { /** * Set if this encoder is inverted. - * + * * @param inverted true to invert the encoder, false otherwise */ public void setInverted(boolean inverted) { From 8d9c4275eb7856605c11de81a3f3839aeda1363d Mon Sep 17 00:00:00 2001 From: Thad House Date: Sat, 2 Mar 2024 19:46:45 -0800 Subject: [PATCH 27/30] More format --- .../src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java | 2 +- .../edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java | 4 ++-- .../edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java index 8668ee914b2..c5117996809 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java @@ -167,7 +167,7 @@ public void close() { /** * Configures if this encoder has rollover counting enabled. * - *

By default, the encoder will not count rollovers. This + *

By default, the encoder will not count rollovers. This * behavior is very rarely needed, and is usually a sign you are * using the wrong encoder type. * diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java index bead64af562..b4a85f0e8c6 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java @@ -23,9 +23,9 @@ public AnalogEncoderSim(AnalogEncoder encoder) { } /** - * Set the position + * Set the position. * - * @param angle The position. + * @param value The position. */ public void set(double value) { m_simPosition.set(value); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java index 660f09333a7..e8366188558 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java @@ -45,7 +45,7 @@ public double get() { /** * Set the position. * - * @param turns The position. + * @param value The position. */ public void set(double value) { m_simPosition.set(value); From af23048f7c53bace00246a78047393c9cae12fc1 Mon Sep 17 00:00:00 2001 From: Thad House Date: Sat, 2 Mar 2024 19:53:09 -0800 Subject: [PATCH 28/30] Formatting --- wpilibc/src/main/native/include/frc/DutyCycleEncoder.h | 10 ++++++++-- .../first/wpilibj/examples/dutycycleencoder/Robot.java | 4 ++-- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h b/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h index 1d35b411d38..7ee04c0d9a1 100644 --- a/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h +++ b/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h @@ -209,9 +209,15 @@ class DutyCycleEncoder : public wpi::Sendable, void SetDutyCycleRange(double min, double max); /** - * Set if this encoder is inverted. + * Sets the assumed frequency of the connected device. * - * @param inverted true to invert the encoder, false otherwise + *

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 */ void SetAssumedFrequency(units::hertz_t frequency); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java index 75216ec2c5b..c3015058281 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java @@ -12,8 +12,8 @@ /** This example shows how to use a duty cycle encoder for devices such as an arm or elevator. */ public class Robot extends TimedRobot { private DutyCycleEncoder m_dutyCycleEncoder; - private final double m_fullRange = 1.3; - private final double m_expectedZero = 0; + private static final double m_fullRange = 1.3; + private static final double m_expectedZero = 0; @Override public void robotInit() { From 3622c0121166ab1a2ad0cba379df0b868fd761cd Mon Sep 17 00:00:00 2001 From: Thad House Date: Sat, 2 Mar 2024 20:06:09 -0800 Subject: [PATCH 29/30] Format --- .../java/edu/wpi/first/wpilibj/AnalogEncoder.java | 12 +++++------- .../src/main/java/edu/wpi/first/math/MathUtil.java | 4 ---- 2 files changed, 5 insertions(+), 11 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java index c5117996809..608b530bf63 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java @@ -106,8 +106,8 @@ private double mapSensorRange(double pos) { /** * Get the encoder value. * - *

By default, this will not count rollovers. If that behavior - * is necessary, call configureRolloverCounting(true). + *

By default, this will not count rollovers. If that behavior is necessary, call + * configureRolloverCounting(true). * * @return the encoder value */ @@ -167,9 +167,8 @@ public void close() { /** * Configures if this encoder has rollover counting enabled. * - *

By default, the encoder will not count rollovers. This - * behavior is very rarely needed, and is usually a sign you are - * using the wrong encoder type. + *

By default, the encoder will not count rollovers. This behavior is very rarely needed, and + * is usually a sign you are using the wrong encoder type. * * @param enable True to enable rollover counting, false to disable. */ @@ -192,8 +191,7 @@ public void configureRolloverCounting(boolean enable) { /** * Reset the number of rollovers that have been counted. * - *

This has no effect unless configureRolloverSupport(true) - * has been called. + *

This has no effect unless configureRolloverSupport(true) has been called. */ public void resetRolloverCount() { if (m_rolloverCounter != null) { diff --git a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java index 64fbf978be5..044cddfab68 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java +++ b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java @@ -125,10 +125,6 @@ public static double inputModulus(double input, double minimumInput, double maxi return input; } - public static double invertInput(double input, double minimumInput, double maximumInput) { - return maximumInput - input + minimumInput; - } - /** * Wraps an angle to the range -pi to pi radians. * From 0b1e8a8db6576f8965f8330d236c4c8de6c7898d Mon Sep 17 00:00:00 2001 From: Thad House Date: Thu, 7 Mar 2024 17:19:31 -0800 Subject: [PATCH 30/30] Completely remove rollover --- wpilibc/src/main/native/cpp/AnalogEncoder.cpp | 94 +++------------ .../main/native/include/frc/AnalogEncoder.h | 28 ----- .../edu/wpi/first/wpilibj/AnalogEncoder.java | 110 +++--------------- 3 files changed, 30 insertions(+), 202 deletions(-) diff --git a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp index 49da8c0fdb8..152b34f98c5 100644 --- a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp +++ b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp @@ -8,7 +8,6 @@ #include #include "frc/AnalogInput.h" -#include "frc/Counter.h" #include "frc/Errors.h" #include "frc/MathUtil.h" #include "frc/RobotController.h" @@ -76,12 +75,22 @@ double AnalogEncoder::Get() const { return m_simPosition.Get(); } - if (m_rolloverCounter) { - return GetWithRollovers(); - } else { - double analog = m_analogInput->GetVoltage(); - return GetWithoutRollovers(analog); + 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 = InputModulus(pos, 0.0, m_fullRange); + // Invert if necessary + if (m_isInverted) { + return m_fullRange - result; } + return result; } void AnalogEncoder::SetVoltagePercentageRange(double min, double max) { @@ -113,76 +122,3 @@ void AnalogEncoder::InitSendable(wpi::SendableBuilder& builder) { builder.AddDoubleProperty( "Position", [this] { return this->Get(); }, nullptr); } - -void AnalogEncoder::ConfigureRolloverSupport(bool enable) { - if (enable && !m_rolloverCounter) { - m_rolloverTrigger = std::make_unique(m_analogInput.get()); - m_rolloverTrigger->SetLimitsVoltage(1.25, 3.75); - m_rolloverCounter = std::make_unique(); - m_rolloverCounter->SetUpSource( - m_rolloverTrigger->CreateOutput(AnalogTriggerType::kRisingPulse)); - m_rolloverCounter->SetDownSource( - m_rolloverTrigger->CreateOutput(AnalogTriggerType::kFallingPulse)); - } else if (!enable && m_rolloverCounter) { - m_rolloverCounter = nullptr; - m_rolloverTrigger = nullptr; - } -} - -void AnalogEncoder::ResetRollovers() { - if (m_rolloverCounter) { - m_rolloverCounter->Reset(); - } -} - -double AnalogEncoder::GetWithoutRollovers(double analog) const { - 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 = InputModulus(pos, 0.0, m_fullRange); - // Invert if necessary - if (m_isInverted) { - return m_fullRange - result; - } - return result; -} - -static bool DoubleEquals(double a, double b) { - constexpr double epsilon = 0.00001; - return std::abs(a - b) < epsilon; -} - -double AnalogEncoder::GetWithRollovers() const { - // 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++) { - auto counter = m_rolloverCounter->Get(); - auto pos = m_analogInput->GetVoltage(); - auto counter2 = m_rolloverCounter->Get(); - auto pos2 = m_analogInput->GetVoltage(); - if (counter == counter2 && DoubleEquals(pos, pos2)) { - pos = GetWithoutRollovers(pos); - - if (m_isInverted) { - pos = pos - counter; - } else { - pos = pos + counter; - } - - m_lastPosition = pos; - return pos; - } - } - - FRC_ReportError( - warn::Warning, - "Failed to read Analog Encoder. Potential Speed Overrun. Returning last " - "value"); - return m_lastPosition; -} diff --git a/wpilibc/src/main/native/include/frc/AnalogEncoder.h b/wpilibc/src/main/native/include/frc/AnalogEncoder.h index 398f57ce80f..8baf4910137 100644 --- a/wpilibc/src/main/native/include/frc/AnalogEncoder.h +++ b/wpilibc/src/main/native/include/frc/AnalogEncoder.h @@ -13,8 +13,6 @@ namespace frc { class AnalogInput; -class Counter; -class AnalogTrigger; /** * Class for supporting continuous analog encoders, such as the US Digital MA3. @@ -135,32 +133,10 @@ class AnalogEncoder : public wpi::Sendable, void InitSendable(wpi::SendableBuilder& builder) override; - /** - * Configures if this encoder has rollover counting enabled. - * - *

By default, the encoder will not count rollovers. This - * behavior is very rarely needed, and is usually a sign you are - * using the wrong encoder type. - * - * @param enable True to enable rollover counting, false to disable. - */ - void ConfigureRolloverSupport(bool enable); - - /** - * Reset the number of rollovers that have been counted. - * - *

This has no effect unless configureRolloverSupport(true) - * has been called. - */ - void ResetRollovers(); - private: void Init(double fullRange, double expectedZero); double MapSensorRange(double pos) const; - double GetWithoutRollovers(double analog) const; - double GetWithRollovers() const; - std::shared_ptr m_analogInput; double m_fullRange; double m_expectedZero; @@ -168,10 +144,6 @@ class AnalogEncoder : public wpi::Sendable, double m_sensorMax{1.0}; bool m_isInverted{false}; - std::unique_ptr m_rolloverTrigger{nullptr}; - std::unique_ptr m_rolloverCounter{nullptr}; - mutable double m_lastPosition{0.0}; - hal::SimDevice m_simDevice; hal::SimDouble m_simPosition; }; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java index 608b530bf63..4a896126ca2 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java @@ -11,7 +11,6 @@ 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 { @@ -23,10 +22,6 @@ public class AnalogEncoder implements Sendable, AutoCloseable { private double m_sensorMax = 1.0; private boolean m_isInverted; - private AnalogTrigger m_rolloverTrigger; - private Counter m_rolloverCounter; - private double m_lastPosition; - private SimDevice m_simDevice; private SimDouble m_simPosition; @@ -116,12 +111,22 @@ public double get() { return m_simPosition.get(); } - if (m_rolloverCounter != null) { - return getWithRollovers(); - } else { - double analog = m_analogInput.getVoltage(); - return getWithoutRollovers(analog); + 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; } + return result; } /** @@ -164,91 +169,6 @@ public void close() { } } - /** - * Configures if this encoder has rollover counting enabled. - * - *

By default, the encoder will not count rollovers. This behavior is very rarely needed, and - * is usually a sign you are using the wrong encoder type. - * - * @param enable True to enable rollover counting, false to disable. - */ - public void configureRolloverCounting(boolean enable) { - if (enable && m_rolloverCounter == null) { - m_rolloverTrigger = new AnalogTrigger(m_analogInput); - m_rolloverTrigger.setLimitsVoltage(1.25, 3.75); - m_rolloverCounter = new Counter(); - m_rolloverCounter.setUpSource(m_rolloverTrigger.createOutput(AnalogTriggerType.kRisingPulse)); - m_rolloverCounter.setDownSource( - m_rolloverTrigger.createOutput(AnalogTriggerType.kFallingPulse)); - } else if (!enable && m_rolloverCounter != null) { - m_rolloverTrigger.close(); - m_rolloverTrigger = null; - m_rolloverCounter.close(); - m_rolloverCounter = null; - } - } - - /** - * Reset the number of rollovers that have been counted. - * - *

This has no effect unless configureRolloverSupport(true) has been called. - */ - public void resetRolloverCount() { - if (m_rolloverCounter != null) { - m_rolloverCounter.reset(); - } - } - - private boolean doubleEquals(double a, double b) { - double epsilon = 0.00001d; - return Math.abs(a - b) < epsilon; - } - - private double getWithRollovers() { - // 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_rolloverCounter.get(); - double pos = m_analogInput.getVoltage(); - double counter2 = m_rolloverCounter.get(); - double pos2 = m_analogInput.getVoltage(); - if (counter == counter2 && doubleEquals(pos, pos2)) { - pos = getWithoutRollovers(pos); - - if (m_isInverted) { - pos = pos - counter; - } else { - pos = pos + counter; - } - - m_lastPosition = pos; - return pos; - } - } - - DriverStation.reportWarning( - "Failed to read Analog Encoder. Potential Speed Overrun. Returning last value", false); - return m_lastPosition; - } - - private double getWithoutRollovers(double analog) { - 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; - } - return result; - } - @Override public void initSendable(SendableBuilder builder) { builder.setSmartDashboardType("AbsoluteEncoder");