[wpilib] Add new counter implementations (#2447)

wpilibc/src/main/native/cpp/counter/ExternalDirectionCounter.cpp

@@ -0,0 +1,103 @@
+// 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/counter/ExternalDirectionCounter.h"
+
+#include <hal/Counter.h>
+#include <hal/FRCUsageReporting.h>
+#include <wpi/NullDeleter.h>
+#include <wpi/sendable/SendableBuilder.h>
+
+#include "frc/DigitalSource.h"
+#include "frc/Errors.h"
+
+using namespace frc;
+
+ExternalDirectionCounter::ExternalDirectionCounter(
+    DigitalSource& countSource, DigitalSource& directionSource)
+    : ExternalDirectionCounter(
+          {&countSource, wpi::NullDeleter<DigitalSource>()},
+          {&directionSource, wpi::NullDeleter<DigitalSource>()}) {}
+
+ExternalDirectionCounter::ExternalDirectionCounter(
+    std::shared_ptr<DigitalSource> countSource,
+    std::shared_ptr<DigitalSource> directionSource) {
+  if (countSource == nullptr) {
+    throw FRC_MakeError(err::NullParameter, "{}", "countSource");
+  }
+  if (directionSource == nullptr) {
+    throw FRC_MakeError(err::NullParameter, "{}", "directionSource");
+  }
+
+  m_countSource = countSource;
+  m_directionSource = directionSource;
+
+  int32_t status = 0;
+  m_handle = HAL_InitializeCounter(
+      HAL_Counter_Mode::HAL_Counter_kExternalDirection, &m_index, &status);
+  FRC_CheckErrorStatus(status, "{}", m_index);
+
+  HAL_SetCounterUpSource(m_handle, m_countSource->GetPortHandleForRouting(),
+                         static_cast<HAL_AnalogTriggerType>(
+                             m_countSource->GetAnalogTriggerTypeForRouting()),
+                         &status);
+  FRC_CheckErrorStatus(status, "{}", m_index);
+  HAL_SetCounterUpSourceEdge(m_handle, true, false, &status);
+  FRC_CheckErrorStatus(status, "{}", m_index);
+
+  HAL_SetCounterDownSource(
+      m_handle, m_directionSource->GetPortHandleForRouting(),
+      static_cast<HAL_AnalogTriggerType>(
+          m_directionSource->GetAnalogTriggerTypeForRouting()),
+      &status);
+  FRC_CheckErrorStatus(status, "{}", m_index);
+  HAL_SetCounterDownSourceEdge(m_handle, false, true, &status);
+  FRC_CheckErrorStatus(status, "{}", m_index);
+
+  Reset();
+
+  HAL_Report(HALUsageReporting::kResourceType_Counter, m_index + 1);
+  wpi::SendableRegistry::AddLW(this, "External Direction Counter", m_index);
+}
+
+ExternalDirectionCounter::~ExternalDirectionCounter() {
+  int32_t status = 0;
+  HAL_FreeCounter(m_handle, &status);
+}
+
+int ExternalDirectionCounter::GetCount() const {
+  int32_t status = 0;
+  int val = HAL_GetCounter(m_handle, &status);
+  FRC_CheckErrorStatus(status, "{}", m_index);
+  return val;
+}
+
+void ExternalDirectionCounter::SetReverseDirection(bool reverseDirection) {
+  int32_t status = 0;
+  HAL_SetCounterReverseDirection(m_handle, reverseDirection, &status);
+  FRC_CheckErrorStatus(status, "{}", m_index);
+}
+
+void ExternalDirectionCounter::Reset() {
+  int32_t status = 0;
+  HAL_ResetCounter(m_handle, &status);
+  FRC_CheckErrorStatus(status, "{}", m_index);
+}
+
+void ExternalDirectionCounter::SetEdgeConfiguration(
+    EdgeConfiguration configuration) {
+  int32_t status = 0;
+  bool rising = configuration == EdgeConfiguration::kRisingEdge ||
+                configuration == EdgeConfiguration::kBoth;
+  bool falling = configuration == EdgeConfiguration::kFallingEdge ||
+                 configuration == EdgeConfiguration::kBoth;
+  HAL_SetCounterUpSourceEdge(m_handle, rising, falling, &status);
+  FRC_CheckErrorStatus(status, "{}", m_index);
+}
+
+void ExternalDirectionCounter::InitSendable(wpi::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("External Direction Counter");
+  builder.AddDoubleProperty(
+      "Count", [&] { return GetCount(); }, nullptr);
+}

wpilibc/src/main/native/cpp/counter/Tachometer.cpp

@@ -0,0 +1,116 @@
+// 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/counter/Tachometer.h"
+
+#include <frc/DigitalSource.h>
+
+#include <hal/Counter.h>
+#include <hal/FRCUsageReporting.h>
+#include <wpi/NullDeleter.h>
+#include <wpi/sendable/SendableBuilder.h>
+
+#include "frc/Errors.h"
+
+using namespace frc;
+
+Tachometer::Tachometer(DigitalSource& source)
+    : Tachometer({&source, wpi::NullDeleter<DigitalSource>()}) {}
+Tachometer::Tachometer(std::shared_ptr<DigitalSource> source) {
+  if (source == nullptr) {
+    throw FRC_MakeError(err::NullParameter, "{}", "source");
+  }
+
+  m_source = source;
+
+  int32_t status = 0;
+  HAL_SetCounterUpSource(m_handle, m_source->GetPortHandleForRouting(),
+                         static_cast<HAL_AnalogTriggerType>(
+                             m_source->GetAnalogTriggerTypeForRouting()),
+                         &status);
+  FRC_CheckErrorStatus(status, "{}", m_index);
+  HAL_SetCounterUpSourceEdge(m_handle, true, false, &status);
+  FRC_CheckErrorStatus(status, "{}", m_index);
+
+  HAL_Report(HALUsageReporting::kResourceType_Counter, m_index + 1);
+  wpi::SendableRegistry::AddLW(this, "Tachometer", m_index);
+}
+
+Tachometer::~Tachometer() {
+  int32_t status = 0;
+  HAL_FreeCounter(m_handle, &status);
+}
+
+units::hertz_t Tachometer::GetFrequency() const {
+  auto period = GetPeriod();
+  if (period.to<double>() == 0) {
+    return units::hertz_t{0.0};
+  }
+  return 1 / period;
+}
+
+units::second_t Tachometer::GetPeriod() const {
+  int32_t status = 0;
+  double period = HAL_GetCounterPeriod(m_handle, &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_source->GetChannel());
+  return units::second_t{period};
+}
+
+int Tachometer::GetEdgesPerRevolution() const {
+  return m_edgesPerRevolution;
+}
+void Tachometer::SetEdgesPerRevolution(int edges) {
+  m_edgesPerRevolution = edges;
+}
+
+units::revolutions_per_minute_t Tachometer::GetRevolutionsPerMinute() const {
+  auto period = GetPeriod();
+  if (period.to<double>() == 0) {
+    return units::revolutions_per_minute_t{0.0};
+  }
+  int edgesPerRevolution = GetEdgesPerRevolution();
+  if (edgesPerRevolution == 0) {
+    return units::revolutions_per_minute_t{0.0};
+  }
+  auto rotationHz = ((1.0 / edgesPerRevolution) / period);
+  return units::revolutions_per_minute_t{rotationHz.to<double>() * 60};
+}
+
+bool Tachometer::GetStopped() const {
+  int32_t status = 0;
+  bool stopped = HAL_GetCounterStopped(m_handle, &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_source->GetChannel());
+  return stopped;
+}
+
+int Tachometer::GetSamplesToAverage() const {
+  int32_t status = 0;
+  int32_t samplesToAverage = HAL_GetCounterSamplesToAverage(m_handle, &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_source->GetChannel());
+  return samplesToAverage;
+}
+
+void Tachometer::SetSamplesToAverage(int samples) {
+  int32_t status = 0;
+  HAL_SetCounterSamplesToAverage(m_handle, samples, &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_source->GetChannel());
+}
+
+void Tachometer::SetMaxPeriod(units::second_t maxPeriod) {
+  int32_t status = 0;
+  HAL_SetCounterMaxPeriod(m_handle, maxPeriod.to<double>(), &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_source->GetChannel());
+}
+
+void Tachometer::SetUpdateWhenEmpty(bool updateWhenEmpty) {
+  int32_t status = 0;
+  HAL_SetCounterUpdateWhenEmpty(m_handle, updateWhenEmpty, &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_source->GetChannel());
+}
+
+void Tachometer::InitSendable(wpi::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Tachometer");
+  builder.AddDoubleProperty(
+      "RPM", [&] { return GetRevolutionsPerMinute().to<double>(); }, nullptr);
+}

wpilibc/src/main/native/cpp/counter/UpDownCounter.cpp

@@ -0,0 +1,106 @@
+// 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/counter/UpDownCounter.h"
+
+#include <hal/Counter.h>
+#include <hal/FRCUsageReporting.h>
+#include <wpi/NullDeleter.h>
+#include <wpi/sendable/SendableBuilder.h>
+
+#include "frc/DigitalSource.h"
+#include "frc/Errors.h"
+
+using namespace frc;
+
+UpDownCounter::UpDownCounter(DigitalSource& upSource, DigitalSource& downSource)
+    : UpDownCounter({&upSource, wpi::NullDeleter<DigitalSource>()},
+                    {&downSource, wpi::NullDeleter<DigitalSource>()}) {}
+
+UpDownCounter::UpDownCounter(std::shared_ptr<DigitalSource> upSource,
+                             std::shared_ptr<DigitalSource> downSource) {
+  m_upSource = upSource;
+  m_downSource = downSource;
+
+  int32_t status = 0;
+  m_handle = HAL_InitializeCounter(HAL_Counter_Mode::HAL_Counter_kTwoPulse,
+                                   &m_index, &status);
+  FRC_CheckErrorStatus(status, "{}", m_index);
+
+  if (m_upSource) {
+    HAL_SetCounterUpSource(m_handle, m_upSource->GetPortHandleForRouting(),
+                           static_cast<HAL_AnalogTriggerType>(
+                               m_upSource->GetAnalogTriggerTypeForRouting()),
+                           &status);
+    FRC_CheckErrorStatus(status, "{}", m_index);
+    HAL_SetCounterUpSourceEdge(m_handle, true, false, &status);
+    FRC_CheckErrorStatus(status, "{}", m_index);
+  }
+
+  if (m_downSource) {
+    HAL_SetCounterDownSource(
+        m_handle, m_downSource->GetPortHandleForRouting(),
+        static_cast<HAL_AnalogTriggerType>(
+            m_downSource->GetAnalogTriggerTypeForRouting()),
+        &status);
+    FRC_CheckErrorStatus(status, "{}", m_index);
+    HAL_SetCounterDownSourceEdge(m_handle, true, false, &status);
+    FRC_CheckErrorStatus(status, "{}", m_index);
+  }
+
+  Reset();
+
+  HAL_Report(HALUsageReporting::kResourceType_Counter, m_index + 1);
+  wpi::SendableRegistry::AddLW(this, "UpDown Counter", m_index);
+}
+
+UpDownCounter::~UpDownCounter() {
+  int32_t status = 0;
+  HAL_FreeCounter(m_handle, &status);
+}
+
+int UpDownCounter::GetCount() const {
+  int32_t status = 0;
+  int val = HAL_GetCounter(m_handle, &status);
+  FRC_CheckErrorStatus(status, "{}", m_index);
+  return val;
+}
+
+void UpDownCounter::SetReverseDirection(bool reverseDirection) {
+  int32_t status = 0;
+  HAL_SetCounterReverseDirection(m_handle, reverseDirection, &status);
+  FRC_CheckErrorStatus(status, "{}", m_index);
+}
+
+void UpDownCounter::Reset() {
+  int32_t status = 0;
+  HAL_ResetCounter(m_handle, &status);
+  FRC_CheckErrorStatus(status, "{}", m_index);
+}
+
+void UpDownCounter::SetUpEdgeConfiguration(EdgeConfiguration configuration) {
+  int32_t status = 0;
+  bool rising = configuration == EdgeConfiguration::kRisingEdge ||
+                configuration == EdgeConfiguration::kBoth;
+  bool falling = configuration == EdgeConfiguration::kFallingEdge ||
+                 configuration == EdgeConfiguration::kBoth;
+  HAL_SetCounterUpSourceEdge(m_handle, rising, falling, &status);
+  FRC_CheckErrorStatus(status, "{}", m_index);
+}
+
+void UpDownCounter::SetDownEdgeConfiguration(EdgeConfiguration configuration) {
+  int32_t status = 0;
+  bool rising = configuration == EdgeConfiguration::kRisingEdge ||
+                configuration == EdgeConfiguration::kBoth;
+  bool falling = configuration == EdgeConfiguration::kFallingEdge ||
+                 configuration == EdgeConfiguration::kBoth;
+  HAL_SetCounterDownSourceEdge(m_handle, rising, falling, &status);
+  FRC_CheckErrorStatus(status, "{}", m_index);
+}
+
+void UpDownCounter::InitSendable(wpi::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("UpDown Counter");
+  builder.AddDoubleProperty(
+      "Count", [&] { return GetCount(); }, nullptr);
+}