Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] Rewrite DutyCycleEncoder and AnalogEncoder (#6398)

Browse Source
This commit is contained in:
Thad House
2024-05-24 11:53:56 -07:00
committed by GitHub
parent 294c9946ae
commit d05c7c125b
18 changed files with 638 additions and 883 deletions
Download Patch File Download Diff File Expand all files Collapse all files

151
wpilibc/src/main/native/cpp/AnalogEncoder.cpp
View File

@@ -8,130 +8,117 @@
#include <wpi/sendable/SendableBuilder.h>
#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<AnalogInput>(channel)) {}
AnalogEncoder::AnalogEncoder(AnalogInput& analogInput)
: m_analogInput{&analogInput, wpi::NullDeleter<AnalogInput>{}},
m_analogTrigger{m_analogInput.get()},
m_counter{} {
Init();
: m_analogInput{&analogInput, wpi::NullDeleter<AnalogInput>{}} {
Init(1.0, 0.0);
}
AnalogEncoder::AnalogEncoder(AnalogInput* analogInput)
: m_analogInput{analogInput, wpi::NullDeleter<AnalogInput>{}},
m_analogTrigger{m_analogInput.get()},
m_counter{} {
Init();
: m_analogInput{analogInput, wpi::NullDeleter<AnalogInput>{}} {
Init(1.0, 0.0);
}
AnalogEncoder::AnalogEncoder(std::shared_ptr<AnalogInput> 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<AnalogInput>(channel), fullRange,
expectedZero) {}
AnalogEncoder::AnalogEncoder(AnalogInput& analogInput, double fullRange,
double expectedZero)
: m_analogInput{&analogInput, wpi::NullDeleter<AnalogInput>{}} {
Init(fullRange, expectedZero);
}
AnalogEncoder::AnalogEncoder(AnalogInput* analogInput, double fullRange,
double expectedZero)
: m_analogInput{analogInput, wpi::NullDeleter<AnalogInput>{}} {
Init(fullRange, expectedZero);
}
AnalogEncoder::AnalogEncoder(std::shared_ptr<AnalogInput> 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);
}

185
wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp
View File

@@ -7,108 +7,137 @@
#include <wpi/NullDeleter.h>
#include <wpi/sendable/SendableBuilder.h>
#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<DutyCycle>(
std::make_shared<DigitalInput>(channel))} {
Init();
Init(1.0, 0.0);
}
DutyCycleEncoder::DutyCycleEncoder(DutyCycle& dutyCycle)
: m_dutyCycle{&dutyCycle, wpi::NullDeleter<DutyCycle>{}} {
Init();
Init(1.0, 0.0);
}
DutyCycleEncoder::DutyCycleEncoder(DutyCycle* dutyCycle)
: m_dutyCycle{dutyCycle, wpi::NullDeleter<DutyCycle>{}} {
Init();
Init(1.0, 0.0);
}
DutyCycleEncoder::DutyCycleEncoder(std::shared_ptr<DutyCycle> dutyCycle)
: m_dutyCycle{std::move(dutyCycle)} {
Init();
Init(1.0, 0.0);
}
DutyCycleEncoder::DutyCycleEncoder(DigitalSource& digitalSource)
: m_dutyCycle{std::make_shared<DutyCycle>(digitalSource)} {
Init();
Init(1.0, 0.0);
}
DutyCycleEncoder::DutyCycleEncoder(DigitalSource* digitalSource)
: m_dutyCycle{std::make_shared<DutyCycle>(digitalSource)} {
Init();
Init(1.0, 0.0);
}
DutyCycleEncoder::DutyCycleEncoder(std::shared_ptr<DigitalSource> digitalSource)
: m_dutyCycle{std::make_shared<DutyCycle>(digitalSource)} {
Init();
Init(1.0, 0.0);
}
void DutyCycleEncoder::Init() {
DutyCycleEncoder::DutyCycleEncoder(int channel, double fullRange,
double expectedZero)
: m_dutyCycle{std::make_shared<DutyCycle>(
std::make_shared<DigitalInput>(channel))} {
Init(fullRange, expectedZero);
}
DutyCycleEncoder::DutyCycleEncoder(DutyCycle& dutyCycle, double fullRange,
double expectedZero)
: m_dutyCycle{&dutyCycle, wpi::NullDeleter<DutyCycle>{}} {
Init(fullRange, expectedZero);
}
DutyCycleEncoder::DutyCycleEncoder(DutyCycle* dutyCycle, double fullRange,
double expectedZero)
: m_dutyCycle{dutyCycle, wpi::NullDeleter<DutyCycle>{}} {
Init(fullRange, expectedZero);
}
DutyCycleEncoder::DutyCycleEncoder(std::shared_ptr<DutyCycle> 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<DutyCycle>(digitalSource)} {
Init(fullRange, expectedZero);
}
DutyCycleEncoder::DutyCycleEncoder(DigitalSource* digitalSource,
double fullRange, double expectedZero)
: m_dutyCycle{std::make_shared<DutyCycle>(digitalSource)} {
Init(fullRange, expectedZero);
}
DutyCycleEncoder::DutyCycleEncoder(std::shared_ptr<DigitalSource> digitalSource,
double fullRange, double expectedZero)
: m_dutyCycle{std::make_shared<DutyCycle>(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<AnalogTrigger>(m_dutyCycle.get());
m_analogTrigger->SetLimitsDutyCycle(0.25, 0.75);
m_counter = std::make_unique<Counter>();
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);
}

16
wpilibc/src/main/native/cpp/simulation/AnalogEncoderSim.cpp
View File

@@ -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();
}

30
wpilibc/src/main/native/cpp/simulation/DutyCycleEncoderSim.cpp
View File

@@ -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() {

145
wpilibc/src/main/native/include/frc/AnalogEncoder.h
View File

@@ -8,13 +8,9 @@
#include <hal/SimDevice.h>
#include <hal/Types.h>
#include <units/angle.h>
#include <wpi/sendable/Sendable.h>
#include <wpi/sendable/SendableHelper.h>
#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.
*
* <p>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.
*
* <p>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.
*
* <p>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.
*
* <p>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> 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> 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.
*
* <p>GetAbsolutePosition() - GetPositionOffset() will give an encoder
* absolute position relative to the last reset. This could potentially be
* negative, which needs to be accounted for.
*
* <p>This will not account for rollovers, and will always be just the raw
* absolute position.
*
* @return the absolute position
* @param 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.
*
* <p>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<AnalogInput> 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

185
wpilibc/src/main/native/include/frc/DutyCycleEncoder.h
View File

@@ -8,13 +8,11 @@
#include <hal/SimDevice.h>
#include <hal/Types.h>
#include <units/angle.h>
#include <units/frequency.h>
#include <units/time.h>
#include <wpi/sendable/Sendable.h>
#include <wpi/sendable/SendableHelper.h>
#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.
*
* <p>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.
*
* <p>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.
*
* <p>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.
*
* <p>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> dutyCycle);
@@ -58,6 +64,8 @@ class DutyCycleEncoder : public wpi::Sendable,
/**
* Construct a new DutyCycleEncoder attached to a DigitalSource object.
*
* <p>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.
*
* <p>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.
*
* <p>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> 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> 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> 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.
*
* <p>GetAbsolutePosition() - GetPositionOffset() will give an encoder
* absolute position relative to the last reset. This could potentially be
* negative, which needs to be accounted for.
*
* <p>This will not account for rollovers, and will always be just the raw
* absolute position.
*
* @return the absolute position
*/
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.
*
* <p>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
* <p>By default, the DutyCycle engine has to compute the frequency of the
* input signal. This can result in both delayed readings and jumpy readings.
* To solve this, you can pass the expected frequency of the sensor to this
* function. This will use that frequency to compute the DutyCycle percentage,
* rather than the computed frequency.
*
* @param frequency the assumed frequency of the sensor
*/
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<DutyCycle> m_dutyCycle;
std::unique_ptr<AnalogTrigger> m_analogTrigger;
std::unique_ptr<Counter> 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

20
wpilibc/src/main/native/include/frc/simulation/AnalogEncoderSim.h
View File

@@ -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;

47
wpilibc/src/main/native/include/frc/simulation/DutyCycleEncoderSim.h
View File

@@ -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;
};

10
wpilibc/src/test/native/cpp/simulation/AnalogEncoderSimTest.cpp
View File

@@ -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);
}

43
wpilibc/src/test/native/cpp/simulation/DutyCycleEncoderSimTest.cpp
View File

@@ -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

46
wpilibcExamples/src/main/cpp/examples/DutyCycleEncoder/cpp/Robot.cpp
View File

@@ -3,17 +3,36 @@
// the WPILib BSD license file in the root directory of this project.
#include <frc/DutyCycleEncoder.h>
#include <frc/MathUtil.h>
#include <frc/TimedRobot.h>
#include <frc/smartdashboard/SmartDashboard.h>
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);
}
};

200
wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java
View File

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

248
wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java
View File

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

29
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java
View File

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

61
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java
View File

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

10
wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSimTest.java
View File

@@ -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);
}
}
}

52
wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSimTest.java
View File

@@ -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());
}
}
}

43
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java
View File

@@ -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);
}
}