Initial attempt at new unit-safe encoder API

wpilibc/src/main/native/cpp/encoder/LinearAnalogEncoder.cpp

@@ -0,0 +1,66 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/encoder/LinearAnalogEncoder.h"
+
+#include <wpi/NullDeleter.h>
+
+#include "frc/Errors.h"
+
+using namespace frc;
+
+LinearAnalogEncoder::LinearAnalogEncoder(int channel,
+                                         decltype(1_V / 1_m) voltsPerMeter)
+    : LinearAnalogEncoder{std::make_shared<AnalogInput>(channel),
+                          voltsPerMeter} {}
+
+LinearAnalogEncoder::LinearAnalogEncoder(AnalogInput& analogInput,
+                                         decltype(1_V / 1_m) voltsPerMeter)
+    : m_analogInput{&analogInput, wpi::NullDeleter<AnalogInput>{}},
+      m_analogTrigger{m_analogInput.get()} {
+  Init(voltsPerMeter);
+}
+
+LinearAnalogEncoder::LinearAnalogEncoder(
+    std::shared_ptr<AnalogInput> analogInput, decltype(1_V / 1_m) voltsPerMeter)
+    : m_analogInput{std::move(analogInput)},
+      m_analogTrigger{m_analogInput.get()} {
+  Init(voltsPerMeter);
+}
+
+units::meter_t LinearAnalogEncoder::GetDisplacement() const {
+  return 0_m;
+}
+
+units::meters_per_second_t LinearAnalogEncoder::GetVelocity() const {
+  return 0_mps;
+}
+
+void LinearAnalogEncoder::SetInverted(bool inverted) {}
+
+void LinearAnalogEncoder::Reset() {}
+
+void LinearAnalogEncoder::SetSimDevice(HAL_SimDeviceHandle device) {}
+
+int LinearAnalogEncoder::GetChannel() const {
+  return m_analogInput->GetChannel();
+}
+
+void LinearAnalogEncoder::Init(decltype(1_V / 1_m) voltsPerMeter) {
+  m_simDevice =
+      hal::SimDevice{"LinearAnalogEncoder", 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));
+
+  wpi::SendableRegistry::AddLW(this, "Linear Analog Encoder",
+                               m_analogInput->GetChannel());
+}

wpilibc/src/main/native/cpp/encoder/LinearDutyCycleEncoder.cpp

@@ -0,0 +1,62 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/encoder/LinearDutyCycleEncoder.h"
+
+#include <wpi/NullDeleter.h>
+
+#include "frc/DigitalInput.h"
+
+using namespace frc;
+
+LinearDutyCycleEncoder::LinearDutyCycleEncoder(int channel,
+                                               decltype(1.0 /
+                                                        1_m) cyclesPerMeter)
+    : m_dutyCycle{std::make_shared<DutyCycle>(
+          std::make_shared<DigitalInput>(channel))} {
+  Init();
+}
+
+LinearDutyCycleEncoder::LinearDutyCycleEncoder(DutyCycle& dutyCycle,
+                                               decltype(1.0 /
+                                                        1_m) cyclesPerMeter)
+    : m_dutyCycle{&dutyCycle, wpi::NullDeleter<DutyCycle>{}} {
+  Init();
+}
+
+LinearDutyCycleEncoder::LinearDutyCycleEncoder(
+    std::shared_ptr<DutyCycle> dutyCycle, decltype(1.0 / 1_m) cyclesPerMeter)
+    : m_dutyCycle{std::move(dutyCycle)} {
+  Init();
+}
+
+LinearDutyCycleEncoder::LinearDutyCycleEncoder(DigitalSource& digitalSource,
+                                               decltype(1.0 /
+                                                        1_m) cyclesPerMeter) {
+  Init();
+}
+
+LinearDutyCycleEncoder::LinearDutyCycleEncoder(
+    std::shared_ptr<DigitalSource> digitalSource,
+    decltype(1.0 / 1_m) cyclesPerMeter) {}
+
+units::meter_t LinearDutyCycleEncoder::GetDisplacement() const {
+  return 0_m;
+}
+
+units::meters_per_second_t LinearDutyCycleEncoder::GetVelocity() const {
+  return 0_mps;
+}
+
+void LinearDutyCycleEncoder::SetInverted(bool inverted) {}
+
+void LinearDutyCycleEncoder::Reset() {}
+
+void LinearDutyCycleEncoder::SetSimDevice(HAL_SimDeviceHandle device) {}
+
+int LinearDutyCycleEncoder::GetFPGAIndex() const {
+  return 0;
+}
+
+void LinearDutyCycleEncoder::Init() {}

wpilibc/src/main/native/cpp/encoder/LinearQuadratureEncoder.cpp

@@ -0,0 +1,111 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/encoder/LinearQuadratureEncoder.h"
+
+#include <hal/Encoder.h>
+#include <hal/FRCUsageReporting.h>
+#include <wpi/NullDeleter.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
+
+#include "frc/DigitalInput.h"
+#include "frc/Errors.h"
+
+using namespace frc;
+
+LinearQuadratureEncoder::LinearQuadratureEncoder(int aChannel, int bChannel,
+                                                 decltype(1.0 /
+                                                          1_m) cyclesPerMeter,
+                                                 EncodingType encodingType)
+    : m_aSource{std::make_shared<DigitalInput>(aChannel)},
+      m_bSource{std::make_shared<DigitalInput>(bChannel)} {
+  Init(cyclesPerMeter, encodingType);
+  wpi::SendableRegistry::AddChild(this, m_aSource.get());
+  wpi::SendableRegistry::AddChild(this, m_bSource.get());
+}
+
+LinearQuadratureEncoder::LinearQuadratureEncoder(
+    DigitalSource& aSource, DigitalSource& bSource,
+    decltype(1.0 / 1_m) cyclesPerMeter, EncodingType encodingType)
+    : m_aSource(&aSource, wpi::NullDeleter<DigitalSource>()),
+      m_bSource(&bSource, wpi::NullDeleter<DigitalSource>()) {
+  Init(cyclesPerMeter, encodingType);
+}
+
+LinearQuadratureEncoder::LinearQuadratureEncoder(
+    std::shared_ptr<DigitalSource> aSource,
+    std::shared_ptr<DigitalSource> bSource, decltype(1.0 / 1_m) cyclesPerMeter,
+    EncodingType encodingType)
+    : m_aSource{std::move(aSource)}, m_bSource{std::move(bSource)} {
+  Init(cyclesPerMeter, encodingType);
+}
+
+units::meter_t LinearQuadratureEncoder::GetDisplacement() const {
+  int32_t status = 0;
+  double value = HAL_GetEncoderDistance(m_encoder, &status);
+  FRC_CheckErrorStatus(status, "{}", "GetDisplacement");
+  return units::meter_t{value};
+}
+
+units::meters_per_second_t LinearQuadratureEncoder::GetVelocity() const {
+  int32_t status = 0;
+  double value = HAL_GetEncoderRate(m_encoder, &status);
+  FRC_CheckErrorStatus(status, "{}", "GetVelocity");
+  return units::meters_per_second_t{value};
+}
+
+void LinearQuadratureEncoder::SetInverted(bool inverted) {
+  int32_t status = 0;
+  HAL_SetEncoderReverseDirection(m_encoder, inverted, &status);
+  FRC_CheckErrorStatus(status, "{}", "SetInverted");
+}
+
+void LinearQuadratureEncoder::Reset() {
+  int32_t status = 0;
+  HAL_ResetEncoder(m_encoder, &status);
+  FRC_CheckErrorStatus(status, "{}", "Reset");
+}
+
+void LinearQuadratureEncoder::SetSimDevice(HAL_SimDeviceHandle device) {
+  HAL_SetEncoderSimDevice(m_encoder, device);
+}
+
+int LinearQuadratureEncoder::GetFPGAIndex() const {
+  int32_t status = 0;
+  int val = HAL_GetEncoderFPGAIndex(m_encoder, &status);
+  FRC_CheckErrorStatus(status, "{}", "GetFPGAIndex");
+  return val;
+}
+
+void LinearQuadratureEncoder::InitSendable(wpi::SendableBuilder& builder) {
+  builder.AddDoubleProperty(
+      "Displacement", [=] { return GetDisplacement().to<double>(); }, nullptr);
+  builder.AddDoubleProperty(
+      "Velocity", [=] { return GetVelocity().to<double>(); }, nullptr);
+}
+
+void LinearQuadratureEncoder::Init(decltype(1.0 / 1_m) cyclesPerMeter,
+                                   EncodingType encodingType) {
+  int32_t status = 0;
+  m_encoder = HAL_InitializeEncoder(
+      m_aSource->GetPortHandleForRouting(),
+      static_cast<HAL_AnalogTriggerType>(
+          m_aSource->GetAnalogTriggerTypeForRouting()),
+      m_bSource->GetPortHandleForRouting(),
+      static_cast<HAL_AnalogTriggerType>(
+          m_bSource->GetAnalogTriggerTypeForRouting()),
+      false, static_cast<HAL_EncoderEncodingType>(encodingType), &status);
+  FRC_CheckErrorStatus(status, "{}", "Init");
+
+  HAL_Report(HALUsageReporting::kResourceType_Encoder, GetFPGAIndex() + 1,
+             static_cast<uint32_t>(encodingType));
+  wpi::SendableRegistry::AddLW(this, "LinearQuadratureEncoder",
+                               m_aSource->GetChannel());
+
+  status = 0;
+  HAL_SetEncoderDistancePerPulse(m_encoder, 1.0 / cyclesPerMeter.to<double>(),
+                                 &status);
+  FRC_CheckErrorStatus(status, "{}", "SetDistancePerPulse");
+}

wpilibc/src/main/native/cpp/encoder/RotaryAnalogEncoder.cpp

@@ -0,0 +1,45 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/encoder/RotaryAnalogEncoder.h"
+
+#include <wpi/NullDeleter.h>
+
+using namespace frc;
+
+RotaryAnalogEncoder::RotaryAnalogEncoder(int channel,
+                                         decltype(1_V /
+                                                  1_tr) voltsPerRevolution)
+    : RotaryAnalogEncoder{std::make_shared<AnalogInput>(channel),
+                          voltsPerRevolution} {}
+
+RotaryAnalogEncoder::RotaryAnalogEncoder(AnalogInput& analogInput,
+                                         decltype(1_V /
+                                                  1_tr) voltsPerRevolution)
+    : m_analogInput{&analogInput, wpi::NullDeleter<AnalogInput>{}},
+      m_analogTrigger{m_analogInput.get()} {}
+
+RotaryAnalogEncoder::RotaryAnalogEncoder(
+    std::shared_ptr<AnalogInput> analogInput,
+    decltype(1_V / 1_tr) voltsPerRevolution)
+    : m_analogInput{std::move(analogInput)},
+      m_analogTrigger{m_analogInput.get()} {}
+
+units::radian_t RotaryAnalogEncoder::GetAngle() const {
+  return 0_rad;
+}
+
+units::radians_per_second_t RotaryAnalogEncoder::GetAngularVelocity() const {
+  return 0_rad_per_s;
+}
+
+void RotaryAnalogEncoder::SetInverted(bool inverted) {}
+
+void RotaryAnalogEncoder::Reset() {}
+
+void RotaryAnalogEncoder::SetSimDevice(HAL_SimDeviceHandle device) {}
+
+int RotaryAnalogEncoder::GetChannel() const {
+  return m_analogInput->GetChannel();
+}

wpilibc/src/main/native/cpp/encoder/RotaryDutyCycleEncoder.cpp

@@ -0,0 +1,50 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/encoder/RotaryDutyCycleEncoder.h"
+
+#include <wpi/NullDeleter.h>
+
+#include "frc/DigitalInput.h"
+
+using namespace frc;
+
+RotaryDutyCycleEncoder::RotaryDutyCycleEncoder(
+    int channel, decltype(1.0 / 1_tr) cyclesPerRevolution)
+    : m_dutyCycle{std::make_shared<DutyCycle>(
+          std::make_shared<DigitalInput>(channel))} {}
+
+RotaryDutyCycleEncoder::RotaryDutyCycleEncoder(
+    DutyCycle& dutyCycle, decltype(1.0 / 1_tr) cyclesPerRevolution)
+    : m_dutyCycle{&dutyCycle, wpi::NullDeleter<DutyCycle>{}} {}
+
+RotaryDutyCycleEncoder::RotaryDutyCycleEncoder(
+    std::shared_ptr<DutyCycle> dutyCycle,
+    decltype(1.0 / 1_tr) cyclesPerRevolution)
+    : m_dutyCycle{std::move(dutyCycle)} {}
+
+RotaryDutyCycleEncoder::RotaryDutyCycleEncoder(
+    DigitalSource& digitalSource, decltype(1.0 / 1_tr) cyclesPerRevolution) {}
+
+RotaryDutyCycleEncoder::RotaryDutyCycleEncoder(
+    std::shared_ptr<DigitalSource> digitalSource,
+    decltype(1.0 / 1_tr) cyclesPerRevolution) {}
+
+units::radian_t RotaryDutyCycleEncoder::GetAngle() const {
+  return 0_rad;
+}
+
+units::radians_per_second_t RotaryDutyCycleEncoder::GetAngularVelocity() const {
+  return 0_rad_per_s;
+}
+
+void RotaryDutyCycleEncoder::SetInverted(bool inverted) {}
+
+void RotaryDutyCycleEncoder::Reset() {}
+
+void RotaryDutyCycleEncoder::SetSimDevice(HAL_SimDeviceHandle device) {}
+
+int RotaryDutyCycleEncoder::GetFPGAIndex() const {
+  return 0;
+}