diff --git a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp new file mode 100644 index 0000000000..a194961377 --- /dev/null +++ b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp @@ -0,0 +1,104 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/AnalogEncoder.h" + +#include "frc/AnalogInput.h" +#include "frc/Counter.h" +#include "frc/DriverStation.h" +#include "frc/smartdashboard/SendableBuilder.h" + +using namespace frc; + +AnalogEncoder::AnalogEncoder(AnalogInput& analogInput) + : m_analogInput{&analogInput, NullDeleter{}}, + m_analogTrigger{m_analogInput.get()}, + m_counter{} { + Init(); +} + +AnalogEncoder::AnalogEncoder(AnalogInput* analogInput) + : m_analogInput{analogInput, NullDeleter{}}, + m_analogTrigger{m_analogInput.get()}, + m_counter{} { + Init(); +} + +AnalogEncoder::AnalogEncoder(std::shared_ptr analogInput) + : m_analogInput{std::move(analogInput)}, + m_analogTrigger{m_analogInput.get()}, + m_counter{} { + Init(); +} + +void AnalogEncoder::Init() { + m_simDevice = hal::SimDevice{"AnalogEncoder", m_analogInput->GetChannel()}; + + if (m_simDevice) { + m_simPosition = m_simDevice.CreateDouble("Position", false, 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)); + + SendableRegistry::GetInstance().AddLW(this, "DutyCycle Encoder", + m_analogInput->GetChannel()); +} + +units::turn_t AnalogEncoder::Get() const { + if (m_simPosition) return units::turn_t{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 && pos == pos2) { + units::turn_t turns{counter + pos - m_positionOffset}; + m_lastPosition = turns; + return turns; + } + } + + frc::DriverStation::GetInstance().ReportWarning( + "Failed to read Analog Encoder. Potential Speed Overrun. Returning last " + "value"); + return m_lastPosition; +} + +double AnalogEncoder::GetPositionOffset() const { return m_positionOffset; } + +void AnalogEncoder::SetDistancePerRotation(double distancePerRotation) { + m_distancePerRotation = distancePerRotation; +} + +double AnalogEncoder::GetDistancePerRotation() const { + return m_distancePerRotation; +} + +double AnalogEncoder::GetDistance() const { + return Get().to() * GetDistancePerRotation(); +} + +void AnalogEncoder::Reset() { + m_counter.Reset(); + m_positionOffset = m_analogInput->GetVoltage(); +} + +void AnalogEncoder::InitSendable(SendableBuilder& builder) { + builder.SetSmartDashboardType("AbsoluteEncoder"); + builder.AddDoubleProperty("Distance", [this] { return this->GetDistance(); }, + nullptr); + builder.AddDoubleProperty("Distance Per Rotation", + [this] { return this->GetDistancePerRotation(); }, + nullptr); +} diff --git a/wpilibc/src/main/native/cpp/DutyCycle.cpp b/wpilibc/src/main/native/cpp/DutyCycle.cpp index 909b9aa02a..12f390e761 100644 --- a/wpilibc/src/main/native/cpp/DutyCycle.cpp +++ b/wpilibc/src/main/native/cpp/DutyCycle.cpp @@ -89,6 +89,8 @@ unsigned int DutyCycle::GetOutputScaleFactor() const { return retVal; } +int DutyCycle::GetSourceChannel() const { return m_source->GetChannel(); } + void DutyCycle::InitSendable(SendableBuilder& builder) { builder.SetSmartDashboardType("Duty Cycle"); builder.AddDoubleProperty("Frequency", diff --git a/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp b/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp new file mode 100644 index 0000000000..e2a927a800 --- /dev/null +++ b/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp @@ -0,0 +1,143 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/DutyCycleEncoder.h" + +#include "frc/Counter.h" +#include "frc/DigitalSource.h" +#include "frc/DriverStation.h" +#include "frc/DutyCycle.h" +#include "frc/smartdashboard/SendableBuilder.h" + +using namespace frc; + +DutyCycleEncoder::DutyCycleEncoder(DutyCycle& dutyCycle) + : m_dutyCycle{&dutyCycle, NullDeleter{}}, + m_analogTrigger{m_dutyCycle.get()}, + m_counter{} { + Init(); +} + +DutyCycleEncoder::DutyCycleEncoder(DutyCycle* dutyCycle) + : m_dutyCycle{dutyCycle, NullDeleter{}}, + m_analogTrigger{m_dutyCycle.get()}, + m_counter{} { + Init(); +} + +DutyCycleEncoder::DutyCycleEncoder(std::shared_ptr dutyCycle) + : m_dutyCycle{std::move(dutyCycle)}, + m_analogTrigger{m_dutyCycle.get()}, + m_counter{} { + Init(); +} + +DutyCycleEncoder::DutyCycleEncoder(DigitalSource& digitalSource) + : m_dutyCycle{std::make_shared(digitalSource)}, + m_analogTrigger{m_dutyCycle.get()}, + m_counter{} { + Init(); +} + +DutyCycleEncoder::DutyCycleEncoder(DigitalSource* digitalSource) + : m_dutyCycle{std::make_shared(digitalSource)}, + m_analogTrigger{m_dutyCycle.get()}, + m_counter{} { + Init(); +} + +DutyCycleEncoder::DutyCycleEncoder(std::shared_ptr digitalSource) + : m_dutyCycle{std::make_shared(digitalSource)}, + m_analogTrigger{m_dutyCycle.get()}, + m_counter{} { + Init(); +} + +void DutyCycleEncoder::Init() { + m_simDevice = hal::SimDevice{"DutyCycleEncoder", m_dutyCycle->GetFPGAIndex()}; + + if (m_simDevice) { + m_simPosition = m_simDevice.CreateDouble("Position", false, 0.0); + m_simIsConnected = m_simDevice.CreateBoolean("Connected", false, true); + } + + m_analogTrigger.SetLimitsDutyCycle(0.25, 0.75); + m_counter.SetUpSource( + m_analogTrigger.CreateOutput(AnalogTriggerType::kRisingPulse)); + m_counter.SetDownSource( + m_analogTrigger.CreateOutput(AnalogTriggerType::kFallingPulse)); + + SendableRegistry::GetInstance().AddLW(this, "DutyCycle Encoder", + m_dutyCycle->GetSourceChannel()); +} + +units::turn_t DutyCycleEncoder::Get() const { + if (m_simPosition) return units::turn_t{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 && pos == pos2) { + units::turn_t turns{counter + pos - m_positionOffset}; + m_lastPosition = turns; + return turns; + } + } + + frc::DriverStation::GetInstance().ReportWarning( + "Failed to read DutyCycle Encoder. Potential Speed Overrun. Returning " + "last value"); + return m_lastPosition; +} + +void DutyCycleEncoder::SetDistancePerRotation(double distancePerRotation) { + m_distancePerRotation = distancePerRotation; +} + +double DutyCycleEncoder::GetDistancePerRotation() const { + return m_distancePerRotation; +} + +double DutyCycleEncoder::GetDistance() const { + return Get().to() * GetDistancePerRotation(); +} + +int DutyCycleEncoder::GetFrequency() const { + return m_dutyCycle->GetFrequency(); +} + +void DutyCycleEncoder::Reset() { + m_counter.Reset(); + m_positionOffset = m_dutyCycle->GetOutput(); +} + +bool DutyCycleEncoder::IsConnected() const { + if (m_simIsConnected) return m_simIsConnected.Get(); + return GetFrequency() > m_frequencyThreshold; +} + +void DutyCycleEncoder::SetConnectedFrequencyThreshold(int frequency) { + if (frequency < 0) { + frequency = 0; + } + m_frequencyThreshold = frequency; +} + +void DutyCycleEncoder::InitSendable(SendableBuilder& builder) { + builder.SetSmartDashboardType("AbsoluteEncoder"); + builder.AddDoubleProperty("Distance", [this] { return this->GetDistance(); }, + nullptr); + builder.AddDoubleProperty("Distance Per Rotation", + [this] { return this->GetDistancePerRotation(); }, + nullptr); + builder.AddDoubleProperty("Is Connected", + [this] { return this->IsConnected(); }, nullptr); +} diff --git a/wpilibc/src/main/native/include/frc/AnalogEncoder.h b/wpilibc/src/main/native/include/frc/AnalogEncoder.h new file mode 100644 index 0000000000..872db87f8a --- /dev/null +++ b/wpilibc/src/main/native/include/frc/AnalogEncoder.h @@ -0,0 +1,126 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include +#include +#include + +#include "frc/AnalogTrigger.h" +#include "frc/Counter.h" +#include "frc/ErrorBase.h" +#include "frc/smartdashboard/Sendable.h" +#include "frc/smartdashboard/SendableHelper.h" + +namespace frc { +class AnalogInput; + +/** + * Class for supporting continuous analog encoders, such as the US Digital MA3. + */ +class AnalogEncoder : public ErrorBase, + public Sendable, + public SendableHelper { + public: + /** + * Construct a new AnalogEncoder attached to a specific AnalogInput. + * + * @param analogInput the analog input to attach to + */ + explicit AnalogEncoder(AnalogInput& analogInput); + + /** + * Construct a new AnalogEncoder attached to a specific AnalogInput. + * + * @param analogInput the analog input to attach to + */ + explicit AnalogEncoder(AnalogInput* analogInput); + + /** + * Construct a new AnalogEncoder attached to a specific AnalogInput. + * + * @param analogInput the analog input to attach to + */ + explicit AnalogEncoder(std::shared_ptr analogInput); + + ~AnalogEncoder() override = default; + + AnalogEncoder(AnalogEncoder&&) = default; + AnalogEncoder& operator=(AnalogEncoder&&) = default; + + /** + * Reset the Encoder distance to zero. + */ + void Reset(); + + /** + * Get the encoder value since the last reset. + * + * This is reported in rotations since the last reset. + * + * @return the encoder value in rotations + */ + units::turn_t Get() const; + + /** + * Get the offset of position relative to the last reset. + * + * GetPositionInRotation() - GetPositionOffset() will give an encoder absolute + * position relative to the last reset. This could potentially be negative, + * which needs to be accounted for. + * + * @return the position offset + */ + double GetPositionOffset() const; + + /** + * 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 InitSendable(SendableBuilder& builder) override; + + private: + void Init(); + + 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}; + + hal::SimDevice m_simDevice; + hal::SimDouble m_simPosition; +}; +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/DutyCycle.h b/wpilibc/src/main/native/include/frc/DutyCycle.h index 37db4fc6a2..1233ee0ca3 100644 --- a/wpilibc/src/main/native/include/frc/DutyCycle.h +++ b/wpilibc/src/main/native/include/frc/DutyCycle.h @@ -113,6 +113,13 @@ class DutyCycle : public ErrorBase, */ int GetFPGAIndex() const; + /** + * Get the channel of the source. + * + * @return the source channel + */ + int GetSourceChannel() const; + protected: void InitSendable(SendableBuilder& builder) override; diff --git a/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h b/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h new file mode 100644 index 0000000000..dd24f9bef2 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h @@ -0,0 +1,167 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include +#include +#include + +#include "frc/AnalogTrigger.h" +#include "frc/Counter.h" +#include "frc/ErrorBase.h" +#include "frc/smartdashboard/Sendable.h" +#include "frc/smartdashboard/SendableHelper.h" + +namespace frc { +class DutyCycle; +class DigitalSource; + +/** + * Class for supporting duty cycle/PWM encoders, such as the US Digital MA3 with + * PWM Output, the CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag + * Encoder. + */ +class DutyCycleEncoder : public ErrorBase, + public Sendable, + public SendableHelper { + public: + /** + * Construct a new DutyCycleEncoder attached to an existing DutyCycle object. + * + * @param dutyCycle the duty cycle to attach to + */ + explicit DutyCycleEncoder(DutyCycle& dutyCycle); + + /** + * Construct a new DutyCycleEncoder attached to an existing DutyCycle object. + * + * @param dutyCycle the duty cycle to attach to + */ + explicit DutyCycleEncoder(DutyCycle* dutyCycle); + + /** + * Construct a new DutyCycleEncoder attached to an existing DutyCycle object. + * + * @param dutyCycle the duty cycle to attach to + */ + explicit DutyCycleEncoder(std::shared_ptr dutyCycle); + + /** + * Construct a new DutyCycleEncoder attached to a DigitalSource object. + * + * @param source the digital source to attach to + */ + explicit DutyCycleEncoder(DigitalSource& digitalSource); + + /** + * Construct a new DutyCycleEncoder attached to a DigitalSource object. + * + * @param source the digital source to attach to + */ + explicit DutyCycleEncoder(DigitalSource* digitalSource); + + /** + * Construct a new DutyCycleEncoder attached to a DigitalSource object. + * + * @param source the digital source to attach to + */ + explicit DutyCycleEncoder(std::shared_ptr digitalSource); + + ~DutyCycleEncoder() override = default; + + DutyCycleEncoder(DutyCycleEncoder&&) = default; + DutyCycleEncoder& operator=(DutyCycleEncoder&&) = default; + + /** + * Get the frequency in Hz of the duty cycle signal from the encoder. + * + * @return duty cycle frequency in Hz + */ + int GetFrequency() const; + + /** + * Get if the sensor is connected + * + * This uses the duty cycle frequency to determine if the sensor is connected. + * By default, a value of 100 Hz is used as the threshold, and this value can + * be changed with SetConnectedFrequencyThreshold. + * + * @return true if the sensor is connected + */ + bool IsConnected() const; + + /** + * Change the frequency threshold for detecting connection used by + * IsConnected. + * + * @param frequency the minimum frequency in Hz. + */ + void SetConnectedFrequencyThreshold(int frequency); + + /** + * Reset the Encoder distance to zero. + */ + void Reset(); + + /** + * Get the encoder value since the last reset. + * + * This is reported in rotations since the last reset. + * + * @return the encoder value in rotations + */ + units::turn_t Get() const; + + /** + * 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 InitSendable(SendableBuilder& builder) override; + + private: + void Init(); + + std::shared_ptr m_dutyCycle; + AnalogTrigger m_analogTrigger; + Counter m_counter; + int m_frequencyThreshold = 100; + double m_positionOffset = 0; + double m_distancePerRotation = 1.0; + mutable units::turn_t m_lastPosition{0.0}; + + hal::SimDevice m_simDevice; + hal::SimDouble m_simPosition; + hal::SimBoolean m_simIsConnected; +}; +} // namespace frc diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java new file mode 100644 index 0000000000..75177e0cd4 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java @@ -0,0 +1,158 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj; + +import edu.wpi.first.hal.SimDevice; +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.wpilibj.AnalogTriggerOutput.AnalogTriggerType; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; +import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry; + +/** + * 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; + + protected SimDevice m_simDevice; + protected SimDouble m_simPosition; + + /** + * Construct a new AnalogEncoder attached to a specific AnalogInput. + * + * @param analogInput the analog input to attach to + */ + public AnalogEncoder(AnalogInput analogInput) { + m_analogInput = analogInput; + init(); + } + + private void init() { + m_analogTrigger = new AnalogTrigger(m_analogInput); + m_counter = new Counter(); + + m_simDevice = SimDevice.create("AnalogEncoder", m_analogInput.getChannel()); + + if (m_simDevice != null) { + m_simPosition = m_simDevice.createDouble("Position", false, 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); + + SendableRegistry.addLW(this, "Analog Encoder", m_analogInput.getChannel()); + } + + /** + * Get the encoder value since the last reset. + * + *

This is reported in rotations since the last reset. + * + * @return the encoder value in rotations + */ + 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 && pos == pos2) { + double position = counter + pos - m_positionOffset; + m_lastPosition = position; + return position; + } + } + + DriverStation.reportWarning( + "Failed to read Analog Encoder. Potential Speed Overrun. Returning last value", false); + return m_lastPosition; + } + + /** + * Get the offset of position relative to the last reset. + * + *

getPositionInRotation() - getPositionOffset() will give an encoder absolute + * position relative to the last reset. This could potentially be negative, + * which needs to be accounted for. + * + * @return the position offset + */ + public double getPositionOffset() { + return m_positionOffset; + } + + /** + * 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 + */ + 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(); + } + + /** + * Reset the Encoder distance to zero. + */ + public void reset() { + m_counter.reset(); + m_positionOffset = m_analogInput.getVoltage(); + } + + @Override + public void close() { + m_counter.close(); + m_analogTrigger.close(); + if (m_simDevice != null) { + m_simDevice.close(); + } + } + + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("AbsoluteEncoder"); + builder.addDoubleProperty("Distance", this::getDistance, null); + builder.addDoubleProperty("Distance Per Rotation", this::getDistancePerRotation, null); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycle.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycle.java index 6bc8e7ebea..0834fadc9e 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycle.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycle.java @@ -28,6 +28,8 @@ public class DutyCycle implements Sendable, AutoCloseable { // Explicitly package private final int m_handle; + private final DigitalSource m_source; + /** * Constructs a DutyCycle input from a DigitalSource input. * @@ -39,6 +41,7 @@ public class DutyCycle implements Sendable, AutoCloseable { m_handle = DutyCycleJNI.initialize(digitalSource.getPortHandleForRouting(), digitalSource.getAnalogTriggerTypeForRouting()); + m_source = digitalSource; int index = getFPGAIndex(); HAL.report(tResourceType.kResourceType_DutyCycle, index + 1); SendableRegistry.addLW(this, "Duty Cycle", index); @@ -107,6 +110,10 @@ public class DutyCycle implements Sendable, AutoCloseable { return DutyCycleJNI.getFPGAIndex(m_handle); } + public int getSourceChannel() { + return m_source.getChannel(); + } + @Override public void initSendable(SendableBuilder builder) { builder.setSmartDashboardType("Duty Cycle"); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java new file mode 100644 index 0000000000..b7d80e8edc --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java @@ -0,0 +1,219 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj; + +import edu.wpi.first.hal.SimBoolean; +import edu.wpi.first.hal.SimDevice; +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.wpilibj.AnalogTriggerOutput.AnalogTriggerType; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; +import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry; + +/** + * Class for supporting duty cycle/PWM encoders, such as the US Digital MA3 with + * PWM Output, the CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag + * Encoder. + */ +public class DutyCycleEncoder implements Sendable, AutoCloseable { + private final DutyCycle m_dutyCycle; + private boolean m_ownsDutyCycle; + 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; + + protected SimDevice m_simDevice; + protected SimDouble m_simPosition; + protected SimBoolean m_simIsConnected; + + /** + * Construct a new DutyCycleEncoder attached to an existing DutyCycle object. + * + * @param dutyCycle the duty cycle to attach to + */ + public DutyCycleEncoder(DutyCycle dutyCycle) { + m_dutyCycle = dutyCycle; + init(); + } + + /** + * Construct a new DutyCycleEncoder attached to a DigitalSource object. + * + * @param source the digital source to attach to + */ + public DutyCycleEncoder(DigitalSource source) { + m_ownsDutyCycle = true; + m_dutyCycle = new DutyCycle(source); + init(); + } + + private void init() { + m_analogTrigger = new AnalogTrigger(m_dutyCycle); + m_counter = new Counter(); + + m_simDevice = SimDevice.create("DutyCycleEncoder", m_dutyCycle.getFPGAIndex()); + + if (m_simDevice != null) { + m_simPosition = m_simDevice.createDouble("Position", false, 0.0); + m_simIsConnected = m_simDevice.createBoolean("Connected", false, true); + } + + m_analogTrigger.setLimitsDutyCycle(0.25, 0.75); + m_counter.setUpSource(m_analogTrigger, AnalogTriggerType.kRisingPulse); + m_counter.setDownSource(m_analogTrigger, AnalogTriggerType.kFallingPulse); + + SendableRegistry.addLW(this, "DutyCycle Encoder", m_dutyCycle.getSourceChannel()); + } + + /** + * Get the encoder value since the last reset. + * + *

This is reported in rotations since the last reset. + * + * @return the encoder value in rotations + */ + 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, 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 && pos == pos2) { + double position = counter + pos - m_positionOffset; + m_lastPosition = position; + return position; + } + } + + DriverStation.reportWarning( + "Failed to read Analog Encoder. Potential Speed Overrun. Returning last value", false); + return m_lastPosition; + } + + /** + * Get the offset of position relative to the last reset. + * + *

getPositionInRotation() - getPositionOffset() will give an encoder absolute + * position relative to the last reset. This could potentially be negative, + * which needs to be accounted for. + * + * @return the position offset + */ + public double getPositionOffset() { + return m_positionOffset; + } + + /** + * 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 + */ + public void setDistancePerRotation(double distancePerRotation) { + m_distancePerRotation = distancePerRotation; + } + + /** + * Get the distance per rotation for this encoder. + * + * @return The scale factor that will be used to convert rotation to useful + * units. + */ + public double getDistancePerRotation() { + return m_distancePerRotation; + } + + /** + * Get the distance the sensor has driven since the last reset as scaled by the + * value from {@link #setDistancePerRotation(double)}. + * + * @return The distance driven since the last reset + */ + public double getDistance() { + return get() * getDistancePerRotation(); + } + + /** + * Get the frequency in Hz of the duty cycle signal from the encoder. + * + * @return duty cycle frequency in Hz + */ + public int getFrequency() { + return m_dutyCycle.getFrequency(); + } + + /** + * Reset the Encoder distance to zero. + */ + public void reset() { + m_counter.reset(); + m_positionOffset = m_dutyCycle.getOutput(); + } + + /** + * Get if the sensor is connected + * + *

This uses the duty cycle frequency to determine if the sensor is connected. + * By default, a value of 100 Hz is used as the threshold, and this value can be + * changed with {@link #setConnectedFrequencyThreshold(int)}. + * + * @return true if the sensor is connected + */ + public boolean isConnected() { + if (m_simIsConnected != null) { + return m_simIsConnected.get(); + } + return getFrequency() > m_frequencyThreshold; + } + + /** + * Change the frequency threshold for detecting connection used by + * {@link #isConnected()}. + * + * @param frequency the minimum frequency in Hz. + */ + public void setConnectedFrequencyThreshold(int frequency) { + if (frequency < 0) { + frequency = 0; + } + + m_frequencyThreshold = frequency; + } + + @Override + public void close() { + m_counter.close(); + m_analogTrigger.close(); + if (m_ownsDutyCycle) { + m_dutyCycle.close(); + } + if (m_simDevice != null) { + m_simDevice.close(); + } + } + + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("AbsoluteEncoder"); + builder.addDoubleProperty("Distance", this::getDistance, null); + builder.addDoubleProperty("Distance Per Rotation", this::getDistancePerRotation, null); + builder.addBooleanProperty("Is Connected", this::isConnected, null); + } +}