diff --git a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp index 0c29bea5e5..152b34f98c 100644 --- a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp +++ b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp @@ -8,130 +8,117 @@ #include #include "frc/AnalogInput.h" -#include "frc/Counter.h" #include "frc/Errors.h" +#include "frc/MathUtil.h" #include "frc/RobotController.h" using namespace frc; +AnalogEncoder::~AnalogEncoder() {} + 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() { +AnalogEncoder::AnalogEncoder(int channel, double fullRange, double expectedZero) + : AnalogEncoder(std::make_shared(channel), fullRange, + expectedZero) {} + +AnalogEncoder::AnalogEncoder(AnalogInput& analogInput, double fullRange, + double expectedZero) + : m_analogInput{&analogInput, wpi::NullDeleter{}} { + Init(fullRange, expectedZero); +} + +AnalogEncoder::AnalogEncoder(AnalogInput* analogInput, double fullRange, + double expectedZero) + : m_analogInput{analogInput, wpi::NullDeleter{}} { + Init(fullRange, expectedZero); +} + +AnalogEncoder::AnalogEncoder(std::shared_ptr analogInput, + double fullRange, double expectedZero) + : m_analogInput{std::move(analogInput)} { + Init(fullRange, expectedZero); +} + +void AnalogEncoder::Init(double fullRange, double expectedZero) { m_simDevice = hal::SimDevice{"AnalogEncoder", m_analogInput->GetChannel()}; 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)); + m_fullRange = fullRange; + m_expectedZero = expectedZero; - wpi::SendableRegistry::AddLW(this, "DutyCycle Encoder", + wpi::SendableRegistry::AddLW(this, "Analog Encoder", 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 { +double AnalogEncoder::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_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; - } + 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; } - - FRC_ReportError( - warn::Warning, - "Failed to read Analog Encoder. Potential Speed Overrun. Returning last " - "value"); - return m_lastPosition; + return result; } -double AnalogEncoder::GetAbsolutePosition() const { - if (m_simAbsolutePosition) { - return m_simAbsolutePosition.Get(); - } - - return m_analogInput->GetVoltage() / frc::RobotController::GetVoltage5V(); +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); } -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; -} - -double AnalogEncoder::GetDistancePerRotation() const { - return m_distancePerRotation; -} - -double AnalogEncoder::GetDistance() const { - return Get().value() * GetDistancePerRotation(); -} - -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 f2ac77fb82..f127cf2ed1 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 435e29e378..7bc58bd514 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 3df775df9a..8d5292ad8c 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 1860273ab4..8baf491013 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; @@ -27,6 +23,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); @@ -34,6 +32,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); @@ -41,6 +41,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); @@ -48,90 +50,79 @@ 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); - ~AnalogEncoder() override = default; + /** + * 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); + + /** + * 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); + + /** + * 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); + + /** + * 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); + + ~AnalogEncoder() override; AnalogEncoder(AnalogEncoder&&) = default; AnalogEncoder& operator=(AnalogEncoder&&) = default; /** - * Reset the Encoder distance to zero. + * Get the encoder value. + * + * @return the encoder value scaled by the full range input */ - void Reset(); + double Get() const; /** - * Get the encoder value since the last reset. + * 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. * - * This is reported in rotations since the last reset. - * - * @return the encoder value in rotations + * @param min minimum voltage percentage (0-1 range) + * @param max maximum voltage percentage (0-1 range) */ - units::turn_t Get() const; + void SetVoltagePercentageRange(double min, double max); /** - * Get the absolute position of the analog encoder. + * Set if this encoder is inverted. * - *

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 + * @param inverted true to invert the encoder, false otherwise */ - 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 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; - - /** - * 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 channel number. @@ -143,17 +134,17 @@ 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}; + 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 5d04ada87d..7ee04c0d9a 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; @@ -30,6 +28,8 @@ class DutyCycleEncoder : public wpi::Sendable, /** * 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 */ explicit DutyCycleEncoder(int channel); @@ -37,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); @@ -44,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); @@ -51,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); @@ -58,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); @@ -65,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); @@ -72,10 +82,79 @@ 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); + /** + * 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); + + /** + * 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); + + /** + * 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); + + /** + * 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); + + /** + * 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); + + /** + * 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); + + /** + * 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); + ~DutyCycleEncoder() override = default; DutyCycleEncoder(DutyCycleEncoder&&) = default; @@ -108,52 +187,11 @@ class DutyCycleEncoder : public wpi::Sendable, void SetConnectedFrequencyThreshold(int frequency); /** - * Reset the Encoder distance to zero. + * Get the encoder value. + * + * @return the encoder value scaled by the full range input */ - void Reset(); - - /** - * Get the encoder value since the last reset. - * - * This is reported in rotations since the last reset. - * - * @return the encoder value in rotations - */ - 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); + double Get() const; /** * Set the encoder duty cycle range. As the encoder needs to maintain a duty @@ -171,32 +209,24 @@ 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. + * Sets the assumed frequency of the connected device. * - * @param distancePerRotation the distance per rotation of the encoder + *

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 SetDistancePerRotation(double distancePerRotation); + void SetAssumedFrequency(units::hertz_t frequency); /** - * Get the distance per rotation for this encoder. + * Set if this encoder is inverted. * - * @return The scale factor that will be used to convert rotation to useful - * units. + * @param inverted true to invert the encoder, false otherwise */ - double GetDistancePerRotation() const; - - /** - * 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 +245,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 6d76326704..3cd8f22306 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 value 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 18c04f0562..e5d84f12fd 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. + * @param value 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 3f0ea45920..fdfd5cec64 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 e81c63e3e2..714edf655e 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 a9343d8aea..3889d1c4da 100644 --- a/wpilibcExamples/src/main/cpp/examples/DutyCycleEncoder/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/DutyCycleEncoder/cpp/Robot.cpp @@ -3,17 +3,36 @@ // the WPILib BSD license file in the root directory of this project. #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/AnalogEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java index 727ad2cd3d..4a896126ca 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java @@ -11,167 +11,143 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.util.sendable.SendableRegistry; -import edu.wpi.first.wpilibj.AnalogTriggerOutput.AnalogTriggerType; /** Class for supporting continuous analog encoders, such as the US Digital MA3. */ public class AnalogEncoder implements Sendable, AutoCloseable { private final AnalogInput m_analogInput; - private AnalogTrigger m_analogTrigger; - private Counter m_counter; - private double m_positionOffset; - private double m_distancePerRotation = 1.0; - private double m_lastPosition; + private boolean m_ownsAnalogInput; + private double m_fullRange; + private double m_expectedZero; + private double m_sensorMin; + private double m_sensorMax = 1.0; + private boolean m_isInverted; private SimDevice m_simDevice; private SimDouble m_simPosition; - private SimDouble m_simAbsolutePosition; /** * Construct a new AnalogEncoder attached to a specific AnalogIn channel. * * @param channel the analog input channel to attach to + * @param fullRange the value to report at maximum travel + * @param expectedZero the reading where you would expect a 0 from get() */ - public AnalogEncoder(int channel) { - this(new AnalogInput(channel)); + public AnalogEncoder(int channel, double fullRange, double expectedZero) { + this(new AnalogInput(channel), fullRange, expectedZero); + m_ownsAnalogInput = true; } /** * Construct a new AnalogEncoder attached to a specific AnalogInput. * * @param analogInput the analog input to attach to + * @param fullRange the value to report at maximum travel + * @param expectedZero the reading where you would expect a 0 from get() + */ + @SuppressWarnings("this-escape") + public AnalogEncoder(AnalogInput analogInput, double fullRange, double expectedZero) { + m_analogInput = analogInput; + init(fullRange, expectedZero); + } + + /** + * Construct a new AnalogEncoder attached to a specific AnalogIn channel. + * + *

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

This has a fullRange of 1 and an expectedZero of 0. + * + * @param analogInput the analog input to attach to */ @SuppressWarnings("this-escape") public AnalogEncoder(AnalogInput analogInput) { - m_analogInput = analogInput; - init(); + this(analogInput, 1.0, 0.0); } - private void init() { - m_analogTrigger = new AnalogTrigger(m_analogInput); - m_counter = new Counter(); - + private void init(double fullRange, double expectedZero) { m_simDevice = SimDevice.create("AnalogEncoder", m_analogInput.getChannel()); if (m_simDevice != null) { m_simPosition = m_simDevice.createDouble("Position", Direction.kInput, 0.0); - m_simAbsolutePosition = m_simDevice.createDouble("absPosition", Direction.kInput, 0.0); } - // Limits need to be 25% from each end - m_analogTrigger.setLimitsVoltage(1.25, 3.75); - m_counter.setUpSource(m_analogTrigger, AnalogTriggerType.kRisingPulse); - m_counter.setDownSource(m_analogTrigger, AnalogTriggerType.kFallingPulse); + m_fullRange = fullRange; + m_expectedZero = expectedZero; SendableRegistry.addLW(this, "Analog Encoder", m_analogInput.getChannel()); } - private boolean doubleEquals(double a, double b) { - double epsilon = 0.00001d; - return Math.abs(a - b) < epsilon; + private double mapSensorRange(double pos) { + // map sensor range + if (pos < m_sensorMin) { + pos = m_sensorMin; + } + if (pos > m_sensorMax) { + pos = m_sensorMax; + } + pos = (pos - m_sensorMin) / (m_sensorMax - m_sensorMin); + return pos; } /** - * Get the encoder value since the last reset. + * Get the encoder value. * - *

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) { return m_simPosition.get(); } - // As the values are not atomic, keep trying until we get 2 reads of the same - // value. If we don't within 10 attempts, warn - for (int i = 0; i < 10; i++) { - double counter = m_counter.get(); - double pos = m_analogInput.getVoltage(); - double counter2 = m_counter.get(); - double pos2 = m_analogInput.getVoltage(); - if (counter == counter2 && doubleEquals(pos, pos2)) { - pos = pos / RobotController.getVoltage5V(); - double position = counter + pos - m_positionOffset; - m_lastPosition = position; - return position; - } + double analog = m_analogInput.getVoltage(); + double pos = analog / RobotController.getVoltage5V(); + + // Map sensor range if range isn't full + pos = mapSensorRange(pos); + + // Compute full range and offset + pos = pos * m_fullRange - m_expectedZero; + + // Map from 0 - Full Range + double result = MathUtil.inputModulus(pos, 0, m_fullRange); + // Invert if necessary + if (m_isInverted) { + return m_fullRange - result; } - - DriverStation.reportWarning( - "Failed to read Analog Encoder. Potential Speed Overrun. Returning last value", false); - return m_lastPosition; + return result; } /** - * Get the absolute position of the analog encoder. + * Set the encoder voltage percentage range. Analog sensors are not always fully stable at the end + * of their travel ranges. Shrinking this range down can help mitigate issues with that. * - *

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 + * @param min minimum voltage percentage (0-1 range) + * @param max maximum voltage percentage (0-1 range) */ - public double getAbsolutePosition() { - if (m_simAbsolutePosition != null) { - return m_simAbsolutePosition.get(); - } - - return m_analogInput.getVoltage() / RobotController.getVoltage5V(); + public void setVoltagePercentageRange(double min, double max) { + m_sensorMin = MathUtil.clamp(min, 0.0, 1.0); + m_sensorMax = MathUtil.clamp(max, 0.0, 1.0); } /** - * Get the offset of position relative to the last reset. + * Set if this encoder is inverted. * - *

getAbsolutePosition() - getPositionOffset() will give an encoder absolute position relative - * to the last reset. This could potentially be negative, which needs to be accounted for. - * - * @return the position offset + * @param inverted true to invert the encoder, false otherwise */ - public double getPositionOffset() { - return m_positionOffset; - } - - /** - * Set the position offset. - * - *

This must be in the range of 0-1. - * - * @param offset the offset - */ - public void setPositionOffset(double offset) { - m_positionOffset = MathUtil.clamp(offset, 0.0, 1.0); - } - - /** - * Set the distance per rotation of the encoder. This sets the multiplier used to determine the - * distance driven based on the rotation value from the encoder. Set this value based on how far - * the mechanism travels in 1 rotation of the encoder, and factor in gearing reductions following - * the encoder shaft. This distance can be in any units you like, linear or angular. - * - * @param distancePerRotation the distance per rotation of the encoder - */ - public void setDistancePerRotation(double distancePerRotation) { - m_distancePerRotation = distancePerRotation; - } - - /** - * Get the distance per rotation for this encoder. - * - * @return The scale factor that will be used to convert rotation to useful units. - */ - public double getDistancePerRotation() { - return m_distancePerRotation; - } - - /** - * Get the distance the sensor has driven since the last reset as scaled by the value from {@link - * #setDistancePerRotation(double)}. - * - * @return The distance driven since the last reset - */ - public double getDistance() { - return get() * getDistancePerRotation(); + public void setInverted(boolean inverted) { + m_isInverted = inverted; } /** @@ -183,16 +159,11 @@ public class AnalogEncoder implements Sendable, AutoCloseable { return m_analogInput.getChannel(); } - /** Reset the Encoder distance to zero. */ - public void reset() { - m_counter.reset(); - m_positionOffset = m_analogInput.getVoltage() / RobotController.getVoltage5V(); - } - @Override public void close() { - m_counter.close(); - m_analogTrigger.close(); + if (m_ownsAnalogInput) { + m_analogInput.close(); + } if (m_simDevice != null) { m_simDevice.close(); } @@ -201,7 +172,6 @@ public class AnalogEncoder implements Sendable, AutoCloseable { @Override public void initSendable(SendableBuilder builder) { builder.setSmartDashboardType("AbsoluteEncoder"); - builder.addDoubleProperty("Distance", this::getDistance, null); - builder.addDoubleProperty("Distance Per Rotation", this::getDistancePerRotation, null); + builder.addDoubleProperty("Position", this::get, null); } } 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 a0858cef92..22cf1995c8 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java @@ -11,7 +11,6 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.util.sendable.SendableRegistry; -import edu.wpi.first.wpilibj.AnalogTriggerOutput.AnalogTriggerType; /** * Class for supporting duty cycle/PWM encoders, such as the US Digital MA3 with PWM Output, the @@ -21,75 +20,107 @@ public class DutyCycleEncoder implements Sendable, AutoCloseable { private final DutyCycle m_dutyCycle; private boolean m_ownsDutyCycle; private DigitalInput m_digitalInput; - private AnalogTrigger m_analogTrigger; - private Counter m_counter; private int m_frequencyThreshold = 100; - private double m_positionOffset; - private double m_distancePerRotation = 1.0; - private double m_lastPosition; + private double m_fullRange; + private double m_expectedZero; + private double m_periodNanos; private double m_sensorMin; private double m_sensorMax = 1.0; + private boolean m_isInverted; private SimDevice m_simDevice; private SimDouble m_simPosition; - private SimDouble m_simAbsolutePosition; - private SimDouble m_simDistancePerRotation; private SimBoolean m_simIsConnected; /** * Construct a new DutyCycleEncoder on a specific channel. * * @param channel the channel to attach to + * @param fullRange the value to report at maximum travel + * @param expectedZero the reading where you would expect a 0 from get() */ @SuppressWarnings("this-escape") - public DutyCycleEncoder(int channel) { + public DutyCycleEncoder(int channel, double fullRange, double expectedZero) { m_digitalInput = new DigitalInput(channel); m_ownsDutyCycle = true; m_dutyCycle = new DutyCycle(m_digitalInput); - init(); + init(fullRange, expectedZero); } /** * Construct a new DutyCycleEncoder attached to an existing DutyCycle object. * * @param dutyCycle the duty cycle to attach to + * @param fullRange the value to report at maximum travel + * @param expectedZero the reading where you would expect a 0 from get() */ @SuppressWarnings("this-escape") - public DutyCycleEncoder(DutyCycle dutyCycle) { + public DutyCycleEncoder(DutyCycle dutyCycle, double fullRange, double expectedZero) { m_dutyCycle = dutyCycle; - init(); + init(fullRange, expectedZero); } /** * Construct a new DutyCycleEncoder attached to a DigitalSource object. * * @param source the digital source to attach to + * @param fullRange the value to report at maximum travel + * @param expectedZero the reading where you would expect a 0 from get() + */ + @SuppressWarnings("this-escape") + public DutyCycleEncoder(DigitalSource source, double fullRange, double expectedZero) { + m_ownsDutyCycle = true; + m_dutyCycle = new DutyCycle(source); + init(fullRange, expectedZero); + } + + /** + * Construct a new DutyCycleEncoder on a specific channel. + * + *

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

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

This has a fullRange of 1 and an expectedZero of 0. + * + * @param source the digital source to attach to */ @SuppressWarnings("this-escape") public DutyCycleEncoder(DigitalSource source) { - m_ownsDutyCycle = true; - m_dutyCycle = new DutyCycle(source); - init(); + this(source, 1.0, 0.0); } - private void init() { + private void init(double fullRange, double expectedZero) { m_simDevice = SimDevice.create("DutyCycle:DutyCycleEncoder", m_dutyCycle.getSourceChannel()); if (m_simDevice != null) { - m_simPosition = m_simDevice.createDouble("position", SimDevice.Direction.kInput, 0.0); - m_simDistancePerRotation = - m_simDevice.createDouble("distance_per_rot", SimDevice.Direction.kOutput, 1.0); - m_simAbsolutePosition = - m_simDevice.createDouble("absPosition", SimDevice.Direction.kInput, 0.0); - m_simIsConnected = m_simDevice.createBoolean("connected", SimDevice.Direction.kInput, true); - } else { - m_counter = new Counter(); - m_analogTrigger = new AnalogTrigger(m_dutyCycle); - m_analogTrigger.setLimitsDutyCycle(0.25, 0.75); - m_counter.setUpSource(m_analogTrigger, AnalogTriggerType.kRisingPulse); - m_counter.setDownSource(m_analogTrigger, AnalogTriggerType.kFallingPulse); + m_simPosition = m_simDevice.createDouble("Position", SimDevice.Direction.kInput, 0.0); + m_simIsConnected = m_simDevice.createBoolean("Connected", SimDevice.Direction.kInput, true); } + m_fullRange = fullRange; + m_expectedZero = expectedZero; + SendableRegistry.addLW(this, "DutyCycle Encoder", m_dutyCycle.getSourceChannel()); } @@ -105,11 +136,6 @@ public class DutyCycleEncoder implements Sendable, AutoCloseable { return pos; } - private boolean doubleEquals(double a, double b) { - double epsilon = 0.00001d; - return Math.abs(a - b) < epsilon; - } - /** * Get the encoder value since the last reset. * @@ -122,67 +148,28 @@ public class DutyCycleEncoder implements Sendable, AutoCloseable { return m_simPosition.get(); } - // As the values are not atomic, keep trying until we get 2 reads of the same - // value - // If we don't within 10 attempts, error - for (int i = 0; i < 10; i++) { - double counter = m_counter.get(); - double pos = m_dutyCycle.getOutput(); - double counter2 = m_counter.get(); - double pos2 = m_dutyCycle.getOutput(); - if (counter == counter2 && doubleEquals(pos, pos2)) { - // map sensor range - pos = mapSensorRange(pos); - double position = counter + pos - m_positionOffset; - m_lastPosition = position; - return position; - } + double pos; + // Compute output percentage (0-1) + if (m_periodNanos == 0.0) { + pos = m_dutyCycle.getOutput(); + } else { + int highTime = m_dutyCycle.getHighTimeNanoseconds(); + pos = highTime / m_periodNanos; } - DriverStation.reportWarning( - "Failed to read Analog Encoder. Potential Speed Overrun. Returning last value", false); - return m_lastPosition; - } + // Map sensor range if range isn't full + pos = mapSensorRange(pos); - /** - * Get the absolute position of the duty cycle encoder. - * - *

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; + + // Map from 0 - Full Range + double result = MathUtil.inputModulus(pos, 0, m_fullRange); + // Invert if necessary + if (m_isInverted) { + return m_fullRange - result; } - - return mapSensorRange(m_dutyCycle.getOutput()); - } - - /** - * Get the offset of position relative to the last reset. - * - *

getAbsolutePosition() - getPositionOffset() will give an encoder absolute position relative - * to the last reset. This could potentially be negative, which needs to be accounted for. - * - * @return the position offset - */ - public double getPositionOffset() { - return m_positionOffset; - } - - /** - * Set the position offset. - * - *

This must be in the range of 0-1. - * - * @param offset the offset - */ - public void setPositionOffset(double offset) { - m_positionOffset = MathUtil.clamp(offset, 0.0, 1.0); + return result; } /** @@ -201,40 +188,6 @@ public class DutyCycleEncoder implements Sendable, AutoCloseable { m_sensorMax = MathUtil.clamp(max, 0.0, 1.0); } - /** - * Set the distance per rotation of the encoder. This sets the multiplier used to determine the - * distance driven based on the rotation value from the encoder. Set this value based on how far - * the mechanism travels in 1 rotation of the encoder, and factor in gearing reductions following - * the encoder shaft. This distance can be in any units you like, linear or angular. - * - * @param distancePerRotation the distance per rotation of the encoder - */ - public void setDistancePerRotation(double distancePerRotation) { - m_distancePerRotation = distancePerRotation; - if (m_simDistancePerRotation != null) { - m_simDistancePerRotation.set(distancePerRotation); - } - } - - /** - * Get the distance per rotation for this encoder. - * - * @return The scale factor that will be used to convert rotation to useful units. - */ - public double getDistancePerRotation() { - return m_distancePerRotation; - } - - /** - * Get the distance the sensor has driven since the last reset as scaled by the value from {@link - * #setDistancePerRotation(double)}. - * - * @return The distance driven since the last reset - */ - public double getDistance() { - return get() * getDistancePerRotation(); - } - /** * Get the frequency in Hz of the duty cycle signal from the encoder. * @@ -244,17 +197,6 @@ public class DutyCycleEncoder implements Sendable, AutoCloseable { return m_dutyCycle.getFrequency(); } - /** Reset the Encoder distance to zero. */ - public void reset() { - if (m_counter != null) { - m_counter.reset(); - } - if (m_simPosition != null) { - m_simPosition.set(0); - } - m_positionOffset = getAbsolutePosition(); - } - /** * Get if the sensor is connected * @@ -284,14 +226,35 @@ public class DutyCycleEncoder implements Sendable, AutoCloseable { m_frequencyThreshold = frequency; } + /** + * Sets the assumed frequency of the connected device. + * + *

By default, the DutyCycle engine has to compute the frequency of the input signal. This can + * result in both delayed readings and jumpy readings. To solve this, you can pass the expected + * frequency of the sensor to this function. This will use that frequency to compute the DutyCycle + * percentage, rather than the computed frequency. + * + * @param frequency the assumed frequency of the sensor + */ + public void setAssumedFrequency(double frequency) { + if (frequency == 0.0) { + m_periodNanos = 0.0; + } else { + m_periodNanos = 1000000000 / frequency; + } + } + + /** + * Set if this encoder is inverted. + * + * @param inverted true to invert the encoder, false otherwise + */ + public void setInverted(boolean inverted) { + m_isInverted = inverted; + } + @Override public void close() { - if (m_counter != null) { - m_counter.close(); - } - if (m_analogTrigger != null) { - m_analogTrigger.close(); - } if (m_ownsDutyCycle) { m_dutyCycle.close(); } @@ -324,8 +287,7 @@ public class DutyCycleEncoder implements Sendable, AutoCloseable { @Override public void initSendable(SendableBuilder builder) { builder.setSmartDashboardType("AbsoluteEncoder"); - builder.addDoubleProperty("Distance", this::getDistance, null); - builder.addDoubleProperty("Distance Per Rotation", this::getDistancePerRotation, null); + builder.addDoubleProperty("Position", this::get, null); builder.addBooleanProperty("Is Connected", this::isConnected, null); } } 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 6b116731a5..b4a85f0e8c 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 class AnalogEncoderSim { } /** - * Set the position using an {@link Rotation2d}. + * Set the position. * - * @param angle The angle. + * @param value The position. */ - public void setPosition(Rotation2d angle) { - setTurns(angle.getDegrees() / 360.0); - } - - /** - * Set the position of the encoder. - * - * @param turns The position. - */ - public void setTurns(double turns) { - m_simPosition.set(turns); + public void set(double value) { + m_simPosition.set(value); } /** @@ -46,16 +36,7 @@ public class AnalogEncoderSim { * * @return The simulated position. */ - public double getTurns() { + public double get() { return m_simPosition.get(); } - - /** - * Get the position as a {@link Rotation2d}. - * - * @return The position as a {@link Rotation2d}. - */ - public Rotation2d getPosition() { - return Rotation2d.fromDegrees(getTurns() * 360.0); - } } 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 3c2c741b62..e836618855 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 @@ -11,8 +11,6 @@ import edu.wpi.first.wpilibj.DutyCycleEncoder; /** Class to control a simulated duty cycle encoder. */ public class DutyCycleEncoderSim { private final SimDouble m_simPosition; - private final SimDouble m_simDistancePerRotation; - private final SimDouble m_simAbsolutePosition; private final SimBoolean m_simIsConnected; /** @@ -31,10 +29,8 @@ public class DutyCycleEncoderSim { */ public DutyCycleEncoderSim(int channel) { SimDeviceSim wrappedSimDevice = new SimDeviceSim("DutyCycle:DutyCycleEncoder", channel); - m_simPosition = wrappedSimDevice.getDouble("position"); - m_simDistancePerRotation = wrappedSimDevice.getDouble("distance_per_rot"); - m_simAbsolutePosition = wrappedSimDevice.getDouble("absPosition"); - m_simIsConnected = wrappedSimDevice.getBoolean("connected"); + m_simPosition = wrappedSimDevice.getDouble("Position"); + m_simIsConnected = wrappedSimDevice.getBoolean("Connected"); } /** @@ -47,57 +43,12 @@ public class DutyCycleEncoderSim { } /** - * Set the position in turns. + * Set the position. * - * @param turns The position. + * @param value The position. */ - public void set(double turns) { - m_simPosition.set(turns); - } - - /** - * Get the distance. - * - * @return The distance. - */ - public double getDistance() { - return m_simPosition.get() * m_simDistancePerRotation.get(); - } - - /** - * Set the distance. - * - * @param distance The distance. - */ - public void setDistance(double distance) { - m_simPosition.set(distance / m_simDistancePerRotation.get()); - } - - /** - * Get the absolute position. - * - * @return The absolute position - */ - public double getAbsolutePosition() { - return m_simAbsolutePosition.get(); - } - - /** - * Set the absolute position. - * - * @param position The absolute position - */ - public void setAbsolutePosition(double position) { - m_simAbsolutePosition.set(position); - } - - /** - * Get the distance per rotation for this encoder. - * - * @return The scale factor that will be used to convert rotation to useful units. - */ - public double getDistancePerRotation() { - return m_simDistancePerRotation.get(); + public void set(double value) { + m_simPosition.set(value); } /** 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 765420dac3..ea5edaafcf 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 @@ package edu.wpi.first.wpilibj.simulation; 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.kPi); - 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 42a6faa628..3ccc05e7ac 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 @@ import org.junit.jupiter.api.Test; 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 @@ class DutyCycleEncoderSimTest { } } - @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 @@ class DutyCycleEncoderSimTest { 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()); - } - } } 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 bb99244248..c301505828 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,39 @@ 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 static final double m_fullRange = 1.3; + private static 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 +50,23 @@ public class Robot extends TimedRobot { // 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); } }