Skip to content

Commit

Permalink
[wpilib] Rewrite DutyCycleEncoder and AnalogEncoder (#6398)
Browse files Browse the repository at this point in the history
  • Loading branch information
@ThadHouse
ThadHouse authored May 24, 2024
1 parent 294c994 commit d05c7c1
Show file tree
Hide file tree
Showing 18 changed files with 618 additions and 863 deletions.
147 changes: 67 additions & 80 deletions wpilibc/src/main/native/cpp/AnalogEncoder.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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() {
m_simDevice = hal::SimDevice{"AnalogEncoder", m_analogInput->GetChannel()};
AnalogEncoder::AnalogEncoder(int channel, double fullRange, double expectedZero)
: AnalogEncoder(std::make_shared<AnalogInput>(channel), fullRange,
expectedZero) {}

if (m_simDevice) {
m_simPosition = m_simDevice.CreateDouble("Position", false, 0.0);
m_simAbsolutePosition =
m_simDevice.CreateDouble("absPosition", hal::SimDevice::kInput, 0.0);
}

m_analogTrigger.SetLimitsVoltage(1.25, 3.75);
m_counter.SetUpSource(
m_analogTrigger.CreateOutput(AnalogTriggerType::kRisingPulse));
m_counter.SetDownSource(
m_analogTrigger.CreateOutput(AnalogTriggerType::kFallingPulse));
AnalogEncoder::AnalogEncoder(AnalogInput& analogInput, double fullRange,
double expectedZero)
: m_analogInput{&analogInput, wpi::NullDeleter<AnalogInput>{}} {
Init(fullRange, expectedZero);
}

wpi::SendableRegistry::AddLW(this, "DutyCycle Encoder",
m_analogInput->GetChannel());
AnalogEncoder::AnalogEncoder(AnalogInput* analogInput, double fullRange,
double expectedZero)
: m_analogInput{analogInput, wpi::NullDeleter<AnalogInput>{}} {
Init(fullRange, expectedZero);
}

static bool DoubleEquals(double a, double b) {
constexpr double epsilon = 0.00001;
return std::abs(a - b) < epsilon;
AnalogEncoder::AnalogEncoder(std::shared_ptr<AnalogInput> analogInput,
double fullRange, double expectedZero)
: m_analogInput{std::move(analogInput)} {
Init(fullRange, expectedZero);
}

units::turn_t AnalogEncoder::Get() const {
if (m_simPosition) {
return units::turn_t{m_simPosition.Get()};
}
void AnalogEncoder::Init(double fullRange, double expectedZero) {
m_simDevice = hal::SimDevice{"AnalogEncoder", m_analogInput->GetChannel()};

// As the values are not atomic, keep trying until we get 2 reads of the same
// value If we don't within 10 attempts, error
for (int i = 0; i < 10; i++) {
auto counter = m_counter.Get();
auto pos = m_analogInput->GetVoltage();
auto counter2 = m_counter.Get();
auto pos2 = m_analogInput->GetVoltage();
if (counter == counter2 && DoubleEquals(pos, pos2)) {
pos = pos / frc::RobotController::GetVoltage5V();
units::turn_t turns{counter + pos - m_positionOffset};
m_lastPosition = turns;
return turns;
}
if (m_simDevice) {
m_simPosition = m_simDevice.CreateDouble("Position", false, 0.0);
}

FRC_ReportError(
warn::Warning,
"Failed to read Analog Encoder. Potential Speed Overrun. Returning last "
"value");
return m_lastPosition;
m_fullRange = fullRange;
m_expectedZero = expectedZero;

wpi::SendableRegistry::AddLW(this, "Analog Encoder",
m_analogInput->GetChannel());
}

double AnalogEncoder::GetAbsolutePosition() const {
if (m_simAbsolutePosition) {
return m_simAbsolutePosition.Get();
double AnalogEncoder::Get() const {
if (m_simPosition) {
return m_simPosition.Get();
}

return m_analogInput->GetVoltage() / frc::RobotController::GetVoltage5V();
}
double analog = m_analogInput->GetVoltage();
double pos = analog / RobotController::GetVoltage5V();

double AnalogEncoder::GetPositionOffset() const {
return m_positionOffset;
}
// Map sensor range if range isn't full
pos = MapSensorRange(pos);

void AnalogEncoder::SetPositionOffset(double offset) {
m_positionOffset = std::clamp(offset, 0.0, 1.0);
}

void AnalogEncoder::SetDistancePerRotation(double distancePerRotation) {
m_distancePerRotation = distancePerRotation;
}
// Compute full range and offset
pos = pos * m_fullRange - m_expectedZero;

double AnalogEncoder::GetDistancePerRotation() const {
return m_distancePerRotation;
// Map from 0 - Full Range
double result = InputModulus(pos, 0.0, m_fullRange);
// Invert if necessary
if (m_isInverted) {
return m_fullRange - result;
}
return result;
}

double AnalogEncoder::GetDistance() const {
return Get().value() * GetDistancePerRotation();
void AnalogEncoder::SetVoltagePercentageRange(double min, double max) {
m_sensorMin = std::clamp(min, 0.0, 1.0);
m_sensorMax = std::clamp(max, 0.0, 1.0);
}

void AnalogEncoder::Reset() {
m_counter.Reset();
m_positionOffset =
m_analogInput->GetVoltage() / frc::RobotController::GetVoltage5V();
void AnalogEncoder::SetInverted(bool inverted) {
m_isInverted = inverted;
}

int AnalogEncoder::GetChannel() const {
return m_analogInput->GetChannel();
}

double AnalogEncoder::MapSensorRange(double pos) const {
if (pos < m_sensorMin) {
pos = m_sensorMin;
}
if (pos > m_sensorMax) {
pos = m_sensorMax;
}
pos = (pos - m_sensorMin) / (m_sensorMax - m_sensorMin);
return pos;
}

void AnalogEncoder::InitSendable(wpi::SendableBuilder& builder) {
builder.SetSmartDashboardType("AbsoluteEncoder");
builder.AddDoubleProperty(
"Distance", [this] { return this->GetDistance(); }, nullptr);
builder.AddDoubleProperty(
"Distance Per Rotation",
[this] { return this->GetDistancePerRotation(); }, nullptr);
"Position", [this] { return this->Get(); }, nullptr);
}
Loading

0 comments on commit d05c7c1

Please sign in to comment.