diff --git a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp index 5f26e87fb9..9a1adb4687 100644 --- a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp +++ b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp @@ -10,6 +10,7 @@ #include "frc/AnalogInput.h" #include "frc/Counter.h" #include "frc/Errors.h" +#include "frc/RobotController.h" using namespace frc; @@ -42,6 +43,8 @@ void AnalogEncoder::Init() { 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); @@ -54,6 +57,11 @@ void AnalogEncoder::Init() { m_analogInput->GetChannel()); } +static bool DoubleEquals(double a, double b) { + constexpr double epsilon = 0.00001; + return std::abs(a - b) < epsilon; +} + units::turn_t AnalogEncoder::Get() const { if (m_simPosition) { return units::turn_t{m_simPosition.Get()}; @@ -66,7 +74,8 @@ units::turn_t AnalogEncoder::Get() const { auto pos = m_analogInput->GetVoltage(); auto counter2 = m_counter.Get(); auto pos2 = m_analogInput->GetVoltage(); - if (counter == counter2 && pos == pos2) { + if (counter == counter2 && DoubleEquals(pos, pos2)) { + pos = pos / frc::RobotController::GetVoltage5V(); units::turn_t turns{counter + pos - m_positionOffset}; m_lastPosition = turns; return turns; @@ -80,10 +89,22 @@ units::turn_t AnalogEncoder::Get() const { return m_lastPosition; } +double AnalogEncoder::GetAbsolutePosition() const { + if (m_simAbsolutePosition) { + return m_simAbsolutePosition.Get(); + } + + return m_analogInput->GetVoltage() / frc::RobotController::GetVoltage5V(); +} + double AnalogEncoder::GetPositionOffset() const { return m_positionOffset; } +void AnalogEncoder::SetPositionOffset(double offset) { + m_positionOffset = std::clamp(offset, 0.0, 1.0); +} + void AnalogEncoder::SetDistancePerRotation(double distancePerRotation) { m_distancePerRotation = distancePerRotation; } @@ -98,7 +119,8 @@ double AnalogEncoder::GetDistance() const { void AnalogEncoder::Reset() { m_counter.Reset(); - m_positionOffset = m_analogInput->GetVoltage(); + m_positionOffset = + m_analogInput->GetVoltage() / frc::RobotController::GetVoltage5V(); } int AnalogEncoder::GetChannel() const { diff --git a/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp b/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp index 452f5cd729..87afe9e652 100644 --- a/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp +++ b/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp @@ -60,6 +60,8 @@ void DutyCycleEncoder::Init() { 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_simIsConnected = m_simDevice.CreateBoolean("connected", hal::SimDevice::kInput, true); } else { @@ -76,6 +78,11 @@ void DutyCycleEncoder::Init() { 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 { if (m_simPosition) { return units::turn_t{m_simPosition.Get()}; @@ -88,15 +95,9 @@ units::turn_t DutyCycleEncoder::Get() const { auto pos = m_dutyCycle->GetOutput(); auto counter2 = m_counter->Get(); auto pos2 = m_dutyCycle->GetOutput(); - if (counter == counter2 && pos == pos2) { + if (counter == counter2 && DoubleEquals(pos, pos2)) { // 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); + pos = MapSensorRange(pos); units::turn_t turns{counter + pos - m_positionOffset}; m_lastPosition = turns; return turns; @@ -110,6 +111,33 @@ units::turn_t DutyCycleEncoder::Get() const { return m_lastPosition; } +double DutyCycleEncoder::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; +} + +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); diff --git a/wpilibc/src/main/native/include/frc/AnalogEncoder.h b/wpilibc/src/main/native/include/frc/AnalogEncoder.h index 6f12cb8a06..1860273ab4 100644 --- a/wpilibc/src/main/native/include/frc/AnalogEncoder.h +++ b/wpilibc/src/main/native/include/frc/AnalogEncoder.h @@ -71,10 +71,24 @@ class AnalogEncoder : public wpi::Sendable, */ units::turn_t Get() const; + /** + * 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 + */ + double GetAbsolutePosition() const; + /** * Get the offset of position relative to the last reset. * - * GetPositionInRotation() - GetPositionOffset() will give an encoder absolute + * GetAbsolutePosition() - GetPositionOffset() will give an encoder absolute * position relative to the last reset. This could potentially be negative, * which needs to be accounted for. * @@ -82,6 +96,15 @@ class AnalogEncoder : public wpi::Sendable, */ double GetPositionOffset() const; + /** + * Set the position offset. + * + *

This must be in the range of 0-1. + * + * @param offset the offset + */ + void SetPositionOffset(double offset); + /** * 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 @@ -131,5 +154,6 @@ class AnalogEncoder : public wpi::Sendable, 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 ab7ded9d3f..5d04ada87d 100644 --- a/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h +++ b/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h @@ -121,6 +121,40 @@ class DutyCycleEncoder : public wpi::Sendable, */ units::turn_t Get() const; + /** + * Get the absolute position of the duty cycle encoder 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 + */ + double GetAbsolutePosition() const; + + /** + * 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 + */ + double GetPositionOffset() const; + + /** + * Set the position offset. + * + *

This must be in the range of 0-1. + * + * @param offset the offset + */ + void SetPositionOffset(double offset); + /** * 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%. @@ -182,6 +216,7 @@ class DutyCycleEncoder : public wpi::Sendable, private: void Init(); + double MapSensorRange(double pos) const; std::shared_ptr m_dutyCycle; std::unique_ptr m_analogTrigger; @@ -195,6 +230,7 @@ class DutyCycleEncoder : public wpi::Sendable, hal::SimDevice m_simDevice; hal::SimDouble m_simPosition; + hal::SimDouble m_simAbsolutePosition; hal::SimDouble m_simDistancePerRotation; hal::SimBoolean m_simIsConnected; }; 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 24989ddd9b..83c93416e2 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java @@ -7,6 +7,7 @@ package edu.wpi.first.wpilibj; import edu.wpi.first.hal.SimDevice; import edu.wpi.first.hal.SimDevice.Direction; import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.util.sendable.SendableRegistry; @@ -23,6 +24,7 @@ public class AnalogEncoder implements Sendable, AutoCloseable { protected SimDevice m_simDevice; protected SimDouble m_simPosition; + protected SimDouble m_simAbsolutePosition; /** * Construct a new AnalogEncoder attached to a specific AnalogIn channel. @@ -51,6 +53,7 @@ public class AnalogEncoder implements Sendable, AutoCloseable { 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 @@ -61,6 +64,11 @@ public class AnalogEncoder implements Sendable, AutoCloseable { 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; + } + /** * Get the encoder value since the last reset. * @@ -80,7 +88,8 @@ public class AnalogEncoder implements Sendable, AutoCloseable { double pos = m_analogInput.getVoltage(); double counter2 = m_counter.get(); double pos2 = m_analogInput.getVoltage(); - if (counter == counter2 && pos == pos2) { + if (counter == counter2 && doubleEquals(pos, pos2)) { + pos = pos / RobotController.getVoltage5V(); double position = counter + pos - m_positionOffset; m_lastPosition = position; return position; @@ -92,12 +101,29 @@ public class AnalogEncoder implements Sendable, AutoCloseable { return m_lastPosition; } + /** + * 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(); + } + + return m_analogInput.getVoltage() / RobotController.getVoltage5V(); + } + /** * Get the offset of position relative to the last reset. * - *

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

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 */ @@ -105,6 +131,17 @@ public class AnalogEncoder implements Sendable, AutoCloseable { return m_positionOffset; } + /** + * 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 the how @@ -148,7 +185,7 @@ public class AnalogEncoder implements Sendable, AutoCloseable { /** Reset the Encoder distance to zero. */ public void reset() { m_counter.reset(); - m_positionOffset = m_analogInput.getVoltage(); + m_positionOffset = m_analogInput.getVoltage() / RobotController.getVoltage5V(); } @Override 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 c8d5bc9fc8..d0cfe8bbba 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java @@ -32,6 +32,7 @@ public class DutyCycleEncoder implements Sendable, AutoCloseable { protected SimDevice m_simDevice; protected SimDouble m_simPosition; + protected SimDouble m_simAbsolutePosition; protected SimDouble m_simDistancePerRotation; protected SimBoolean m_simIsConnected; @@ -75,6 +76,8 @@ public class DutyCycleEncoder implements Sendable, AutoCloseable { 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(); @@ -87,6 +90,23 @@ public class DutyCycleEncoder implements Sendable, AutoCloseable { SendableRegistry.addLW(this, "DutyCycle Encoder", m_dutyCycle.getSourceChannel()); } + 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; + } + + 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. * @@ -107,15 +127,9 @@ public class DutyCycleEncoder implements Sendable, AutoCloseable { double pos = m_dutyCycle.getOutput(); double counter2 = m_counter.get(); double pos2 = m_dutyCycle.getOutput(); - if (counter == counter2 && pos == pos2) { + if (counter == counter2 && doubleEquals(pos, pos2)) { // 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); + pos = mapSensorRange(pos); double position = counter + pos - m_positionOffset; m_lastPosition = position; return position; @@ -127,12 +141,29 @@ public class DutyCycleEncoder implements Sendable, AutoCloseable { 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(); + } + + return mapSensorRange(m_dutyCycle.getOutput()); + } + /** * Get the offset of position relative to the last reset. * - *

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

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 */ @@ -140,6 +171,17 @@ public class DutyCycleEncoder implements Sendable, AutoCloseable { return m_positionOffset; } + /** + * 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 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