mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-22 01:11:42 +00:00
[hal, wpilib] Add initial systemcore counter implementation (#7723)
This commit is contained in:
Thad House
@@ -1,322 +0,0 @@
|
||||
// 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.h"
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include <hal/Counter.h>
|
||||
#include <hal/FRCUsageReporting.h>
|
||||
#include <wpi/NullDeleter.h>
|
||||
#include <wpi/sendable/SendableBuilder.h>
|
||||
#include <wpi/sendable/SendableRegistry.h>
|
||||
|
||||
#include "frc/AnalogTrigger.h"
|
||||
#include "frc/DigitalInput.h"
|
||||
#include "frc/Errors.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
Counter::Counter(Mode mode) {
|
||||
int32_t status = 0;
|
||||
m_counter = HAL_InitializeCounter(static_cast<HAL_Counter_Mode>(mode),
|
||||
&m_index, &status);
|
||||
FRC_CheckErrorStatus(status, "InitializeCounter");
|
||||
|
||||
SetMaxPeriod(0.5_s);
|
||||
|
||||
HAL_Report(HALUsageReporting::kResourceType_Counter, m_index + 1, mode + 1);
|
||||
wpi::SendableRegistry::Add(this, "Counter", m_index);
|
||||
}
|
||||
|
||||
Counter::Counter(int channel) : Counter(kTwoPulse) {
|
||||
SetUpSource(channel);
|
||||
ClearDownSource();
|
||||
}
|
||||
|
||||
Counter::Counter(DigitalSource* source) : Counter(kTwoPulse) {
|
||||
SetUpSource(source);
|
||||
ClearDownSource();
|
||||
}
|
||||
|
||||
Counter::Counter(std::shared_ptr<DigitalSource> source) : Counter(kTwoPulse) {
|
||||
SetUpSource(source);
|
||||
ClearDownSource();
|
||||
}
|
||||
|
||||
Counter::Counter(const AnalogTrigger& trigger) : Counter(kTwoPulse) {
|
||||
SetUpSource(trigger.CreateOutput(AnalogTriggerType::kState));
|
||||
ClearDownSource();
|
||||
}
|
||||
|
||||
Counter::Counter(EncodingType encodingType, DigitalSource* upSource,
|
||||
DigitalSource* downSource, bool inverted)
|
||||
: Counter(encodingType,
|
||||
std::shared_ptr<DigitalSource>(upSource,
|
||||
wpi::NullDeleter<DigitalSource>()),
|
||||
std::shared_ptr<DigitalSource>(downSource,
|
||||
wpi::NullDeleter<DigitalSource>()),
|
||||
inverted) {}
|
||||
|
||||
Counter::Counter(EncodingType encodingType,
|
||||
std::shared_ptr<DigitalSource> upSource,
|
||||
std::shared_ptr<DigitalSource> downSource, bool inverted)
|
||||
: Counter(kExternalDirection) {
|
||||
if (encodingType != k1X && encodingType != k2X) {
|
||||
throw FRC_MakeError(err::ParameterOutOfRange,
|
||||
"Counter only supports 1X and 2X quadrature decoding");
|
||||
}
|
||||
SetUpSource(upSource);
|
||||
SetDownSource(downSource);
|
||||
int32_t status = 0;
|
||||
|
||||
if (encodingType == k1X) {
|
||||
SetUpSourceEdge(true, false);
|
||||
HAL_SetCounterAverageSize(m_counter, 1, &status);
|
||||
} else {
|
||||
SetUpSourceEdge(true, true);
|
||||
HAL_SetCounterAverageSize(m_counter, 2, &status);
|
||||
}
|
||||
|
||||
FRC_CheckErrorStatus(status, "Counter constructor");
|
||||
SetDownSourceEdge(inverted, true);
|
||||
}
|
||||
|
||||
Counter::~Counter() {
|
||||
if (m_counter != HAL_kInvalidHandle) {
|
||||
try {
|
||||
SetUpdateWhenEmpty(true);
|
||||
} catch (const RuntimeError& e) {
|
||||
e.Report();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Counter::SetUpSource(int channel) {
|
||||
SetUpSource(std::make_shared<DigitalInput>(channel));
|
||||
wpi::SendableRegistry::AddChild(this, m_upSource.get());
|
||||
}
|
||||
|
||||
void Counter::SetUpSource(AnalogTrigger* analogTrigger,
|
||||
AnalogTriggerType triggerType) {
|
||||
SetUpSource(std::shared_ptr<AnalogTrigger>(analogTrigger,
|
||||
wpi::NullDeleter<AnalogTrigger>()),
|
||||
triggerType);
|
||||
}
|
||||
|
||||
void Counter::SetUpSource(std::shared_ptr<AnalogTrigger> analogTrigger,
|
||||
AnalogTriggerType triggerType) {
|
||||
SetUpSource(analogTrigger->CreateOutput(triggerType));
|
||||
}
|
||||
|
||||
void Counter::SetUpSource(DigitalSource* source) {
|
||||
SetUpSource(std::shared_ptr<DigitalSource>(
|
||||
source, wpi::NullDeleter<DigitalSource>()));
|
||||
}
|
||||
|
||||
void Counter::SetUpSource(std::shared_ptr<DigitalSource> source) {
|
||||
m_upSource = source;
|
||||
int32_t status = 0;
|
||||
HAL_SetCounterUpSource(m_counter, source->GetPortHandleForRouting(),
|
||||
static_cast<HAL_AnalogTriggerType>(
|
||||
source->GetAnalogTriggerTypeForRouting()),
|
||||
&status);
|
||||
FRC_CheckErrorStatus(status, "SetUpSource");
|
||||
}
|
||||
|
||||
void Counter::SetUpSource(DigitalSource& source) {
|
||||
SetUpSource(std::shared_ptr<DigitalSource>(
|
||||
&source, wpi::NullDeleter<DigitalSource>()));
|
||||
}
|
||||
|
||||
void Counter::SetUpSourceEdge(bool risingEdge, bool fallingEdge) {
|
||||
if (m_upSource == nullptr) {
|
||||
throw FRC_MakeError(
|
||||
err::NullParameter,
|
||||
"Must set non-nullptr UpSource before setting UpSourceEdge");
|
||||
}
|
||||
int32_t status = 0;
|
||||
HAL_SetCounterUpSourceEdge(m_counter, risingEdge, fallingEdge, &status);
|
||||
FRC_CheckErrorStatus(status, "SetUpSourceEdge");
|
||||
}
|
||||
|
||||
void Counter::ClearUpSource() {
|
||||
m_upSource.reset();
|
||||
int32_t status = 0;
|
||||
HAL_ClearCounterUpSource(m_counter, &status);
|
||||
FRC_CheckErrorStatus(status, "ClearUpSource");
|
||||
}
|
||||
|
||||
void Counter::SetDownSource(int channel) {
|
||||
SetDownSource(std::make_shared<DigitalInput>(channel));
|
||||
wpi::SendableRegistry::AddChild(this, m_downSource.get());
|
||||
}
|
||||
|
||||
void Counter::SetDownSource(AnalogTrigger* analogTrigger,
|
||||
AnalogTriggerType triggerType) {
|
||||
SetDownSource(std::shared_ptr<AnalogTrigger>(
|
||||
analogTrigger, wpi::NullDeleter<AnalogTrigger>()),
|
||||
triggerType);
|
||||
}
|
||||
|
||||
void Counter::SetDownSource(std::shared_ptr<AnalogTrigger> analogTrigger,
|
||||
AnalogTriggerType triggerType) {
|
||||
SetDownSource(analogTrigger->CreateOutput(triggerType));
|
||||
}
|
||||
|
||||
void Counter::SetDownSource(DigitalSource* source) {
|
||||
SetDownSource(std::shared_ptr<DigitalSource>(
|
||||
source, wpi::NullDeleter<DigitalSource>()));
|
||||
}
|
||||
|
||||
void Counter::SetDownSource(DigitalSource& source) {
|
||||
SetDownSource(std::shared_ptr<DigitalSource>(
|
||||
&source, wpi::NullDeleter<DigitalSource>()));
|
||||
}
|
||||
|
||||
void Counter::SetDownSource(std::shared_ptr<DigitalSource> source) {
|
||||
m_downSource = source;
|
||||
int32_t status = 0;
|
||||
HAL_SetCounterDownSource(m_counter, source->GetPortHandleForRouting(),
|
||||
static_cast<HAL_AnalogTriggerType>(
|
||||
source->GetAnalogTriggerTypeForRouting()),
|
||||
&status);
|
||||
FRC_CheckErrorStatus(status, "SetDownSource");
|
||||
}
|
||||
|
||||
void Counter::SetDownSourceEdge(bool risingEdge, bool fallingEdge) {
|
||||
if (m_downSource == nullptr) {
|
||||
throw FRC_MakeError(
|
||||
err::NullParameter,
|
||||
"Must set non-nullptr DownSource before setting DownSourceEdge");
|
||||
}
|
||||
int32_t status = 0;
|
||||
HAL_SetCounterDownSourceEdge(m_counter, risingEdge, fallingEdge, &status);
|
||||
FRC_CheckErrorStatus(status, "SetDownSourceEdge");
|
||||
}
|
||||
|
||||
void Counter::ClearDownSource() {
|
||||
m_downSource.reset();
|
||||
int32_t status = 0;
|
||||
HAL_ClearCounterDownSource(m_counter, &status);
|
||||
FRC_CheckErrorStatus(status, "ClearDownSource");
|
||||
}
|
||||
|
||||
void Counter::SetUpDownCounterMode() {
|
||||
int32_t status = 0;
|
||||
HAL_SetCounterUpDownMode(m_counter, &status);
|
||||
FRC_CheckErrorStatus(status, "SetUpDownCounterMode");
|
||||
}
|
||||
|
||||
void Counter::SetExternalDirectionMode() {
|
||||
int32_t status = 0;
|
||||
HAL_SetCounterExternalDirectionMode(m_counter, &status);
|
||||
FRC_CheckErrorStatus(status, "SetExternalDirectionMode");
|
||||
}
|
||||
|
||||
void Counter::SetSemiPeriodMode(bool highSemiPeriod) {
|
||||
int32_t status = 0;
|
||||
HAL_SetCounterSemiPeriodMode(m_counter, highSemiPeriod, &status);
|
||||
FRC_CheckErrorStatus(status, "SetSemiPeriodMode to {}",
|
||||
highSemiPeriod ? "true" : "false");
|
||||
}
|
||||
|
||||
void Counter::SetPulseLengthMode(double threshold) {
|
||||
int32_t status = 0;
|
||||
HAL_SetCounterPulseLengthMode(m_counter, threshold, &status);
|
||||
FRC_CheckErrorStatus(status, "SetPulseLengthMode");
|
||||
}
|
||||
|
||||
void Counter::SetReverseDirection(bool reverseDirection) {
|
||||
int32_t status = 0;
|
||||
HAL_SetCounterReverseDirection(m_counter, reverseDirection, &status);
|
||||
FRC_CheckErrorStatus(status, "SetReverseDirection to {}",
|
||||
reverseDirection ? "true" : "false");
|
||||
}
|
||||
|
||||
void Counter::SetSamplesToAverage(int samplesToAverage) {
|
||||
if (samplesToAverage < 1 || samplesToAverage > 127) {
|
||||
throw FRC_MakeError(
|
||||
err::ParameterOutOfRange,
|
||||
"Average counter values must be between 1 and 127, {} out of range",
|
||||
samplesToAverage);
|
||||
}
|
||||
int32_t status = 0;
|
||||
HAL_SetCounterSamplesToAverage(m_counter, samplesToAverage, &status);
|
||||
FRC_CheckErrorStatus(status, "SetSamplesToAverage to {}", samplesToAverage);
|
||||
}
|
||||
|
||||
int Counter::GetSamplesToAverage() const {
|
||||
int32_t status = 0;
|
||||
int samples = HAL_GetCounterSamplesToAverage(m_counter, &status);
|
||||
FRC_CheckErrorStatus(status, "GetSamplesToAverage");
|
||||
return samples;
|
||||
}
|
||||
|
||||
int Counter::GetFPGAIndex() const {
|
||||
return m_index;
|
||||
}
|
||||
|
||||
void Counter::SetDistancePerPulse(double distancePerPulse) {
|
||||
m_distancePerPulse = distancePerPulse;
|
||||
}
|
||||
|
||||
double Counter::GetDistance() const {
|
||||
return Get() * m_distancePerPulse;
|
||||
}
|
||||
|
||||
double Counter::GetRate() const {
|
||||
return m_distancePerPulse / GetPeriod().value();
|
||||
}
|
||||
|
||||
int Counter::Get() const {
|
||||
int32_t status = 0;
|
||||
int value = HAL_GetCounter(m_counter, &status);
|
||||
FRC_CheckErrorStatus(status, "Get");
|
||||
return value;
|
||||
}
|
||||
|
||||
void Counter::Reset() {
|
||||
int32_t status = 0;
|
||||
HAL_ResetCounter(m_counter, &status);
|
||||
FRC_CheckErrorStatus(status, "Reset");
|
||||
}
|
||||
|
||||
units::second_t Counter::GetPeriod() const {
|
||||
int32_t status = 0;
|
||||
double value = HAL_GetCounterPeriod(m_counter, &status);
|
||||
FRC_CheckErrorStatus(status, "GetPeriod");
|
||||
return units::second_t{value};
|
||||
}
|
||||
|
||||
void Counter::SetMaxPeriod(units::second_t maxPeriod) {
|
||||
int32_t status = 0;
|
||||
HAL_SetCounterMaxPeriod(m_counter, maxPeriod.value(), &status);
|
||||
FRC_CheckErrorStatus(status, "SetMaxPeriod");
|
||||
}
|
||||
|
||||
void Counter::SetUpdateWhenEmpty(bool enabled) {
|
||||
int32_t status = 0;
|
||||
HAL_SetCounterUpdateWhenEmpty(m_counter, enabled, &status);
|
||||
FRC_CheckErrorStatus(status, "SetUpdateWhenEmpty");
|
||||
}
|
||||
|
||||
bool Counter::GetStopped() const {
|
||||
int32_t status = 0;
|
||||
bool value = HAL_GetCounterStopped(m_counter, &status);
|
||||
FRC_CheckErrorStatus(status, "GetStopped");
|
||||
return value;
|
||||
}
|
||||
|
||||
bool Counter::GetDirection() const {
|
||||
int32_t status = 0;
|
||||
bool value = HAL_GetCounterDirection(m_counter, &status);
|
||||
FRC_CheckErrorStatus(status, "GetDirection");
|
||||
return value;
|
||||
}
|
||||
|
||||
void Counter::InitSendable(wpi::SendableBuilder& builder) {
|
||||
builder.SetSmartDashboardType("Counter");
|
||||
builder.AddDoubleProperty("Value", [=, this] { return Get(); }, nullptr);
|
||||
}
|
||||
@@ -1,203 +0,0 @@
|
||||
// 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/Ultrasonic.h"
|
||||
|
||||
#include <memory>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include <hal/FRCUsageReporting.h>
|
||||
#include <wpi/NullDeleter.h>
|
||||
#include <wpi/sendable/SendableBuilder.h>
|
||||
#include <wpi/sendable/SendableRegistry.h>
|
||||
|
||||
#include "frc/Counter.h"
|
||||
#include "frc/DigitalInput.h"
|
||||
#include "frc/DigitalOutput.h"
|
||||
#include "frc/Errors.h"
|
||||
#include "frc/Timer.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
// Automatic round robin mode
|
||||
std::atomic<bool> Ultrasonic::m_automaticEnabled{false};
|
||||
|
||||
std::vector<Ultrasonic*> Ultrasonic::m_sensors;
|
||||
std::thread Ultrasonic::m_thread;
|
||||
|
||||
Ultrasonic::Ultrasonic(int pingChannel, int echoChannel)
|
||||
: m_pingChannel(std::make_shared<DigitalOutput>(pingChannel)),
|
||||
m_echoChannel(std::make_shared<DigitalInput>(echoChannel)),
|
||||
m_counter(m_echoChannel) {
|
||||
Initialize();
|
||||
wpi::SendableRegistry::AddChild(this, m_pingChannel.get());
|
||||
wpi::SendableRegistry::AddChild(this, m_echoChannel.get());
|
||||
}
|
||||
|
||||
Ultrasonic::Ultrasonic(DigitalOutput* pingChannel, DigitalInput* echoChannel)
|
||||
: m_pingChannel(pingChannel, wpi::NullDeleter<DigitalOutput>()),
|
||||
m_echoChannel(echoChannel, wpi::NullDeleter<DigitalInput>()),
|
||||
m_counter(m_echoChannel) {
|
||||
if (!pingChannel) {
|
||||
throw FRC_MakeError(err::NullParameter, "pingChannel");
|
||||
}
|
||||
if (!echoChannel) {
|
||||
throw FRC_MakeError(err::NullParameter, "echoChannel");
|
||||
}
|
||||
Initialize();
|
||||
}
|
||||
|
||||
Ultrasonic::Ultrasonic(DigitalOutput& pingChannel, DigitalInput& echoChannel)
|
||||
: m_pingChannel(&pingChannel, wpi::NullDeleter<DigitalOutput>()),
|
||||
m_echoChannel(&echoChannel, wpi::NullDeleter<DigitalInput>()),
|
||||
m_counter(m_echoChannel) {
|
||||
Initialize();
|
||||
}
|
||||
|
||||
Ultrasonic::Ultrasonic(std::shared_ptr<DigitalOutput> pingChannel,
|
||||
std::shared_ptr<DigitalInput> echoChannel)
|
||||
: m_pingChannel(std::move(pingChannel)),
|
||||
m_echoChannel(std::move(echoChannel)),
|
||||
m_counter(m_echoChannel) {
|
||||
Initialize();
|
||||
}
|
||||
|
||||
Ultrasonic::~Ultrasonic() {
|
||||
// Delete the instance of the ultrasonic sensor by freeing the allocated
|
||||
// digital channels. If the system was in automatic mode (round robin), then
|
||||
// it is stopped, then started again after this sensor is removed (provided
|
||||
// this wasn't the last sensor).
|
||||
|
||||
bool wasAutomaticMode = m_automaticEnabled;
|
||||
SetAutomaticMode(false);
|
||||
|
||||
// No synchronization needed because the background task is stopped.
|
||||
m_sensors.erase(std::remove(m_sensors.begin(), m_sensors.end(), this),
|
||||
m_sensors.end());
|
||||
|
||||
if (!m_sensors.empty() && wasAutomaticMode) {
|
||||
SetAutomaticMode(true);
|
||||
}
|
||||
}
|
||||
|
||||
int Ultrasonic::GetEchoChannel() const {
|
||||
return m_echoChannel->GetChannel();
|
||||
}
|
||||
|
||||
void Ultrasonic::Ping() {
|
||||
SetAutomaticMode(false); // turn off automatic round-robin if pinging
|
||||
|
||||
// Reset the counter to zero (invalid data now)
|
||||
m_counter.Reset();
|
||||
|
||||
// Do the ping to start getting a single range
|
||||
m_pingChannel->Pulse(kPingTime);
|
||||
}
|
||||
|
||||
bool Ultrasonic::IsRangeValid() const {
|
||||
if (m_simRangeValid) {
|
||||
return m_simRangeValid.Get();
|
||||
}
|
||||
return m_counter.Get() > 1;
|
||||
}
|
||||
|
||||
void Ultrasonic::SetAutomaticMode(bool enabling) {
|
||||
if (enabling == m_automaticEnabled) {
|
||||
return; // ignore the case of no change
|
||||
}
|
||||
|
||||
m_automaticEnabled = enabling;
|
||||
|
||||
if (enabling) {
|
||||
/* Clear all the counters so no data is valid. No synchronization is needed
|
||||
* because the background task is stopped.
|
||||
*/
|
||||
for (auto& sensor : m_sensors) {
|
||||
sensor->m_counter.Reset();
|
||||
}
|
||||
|
||||
m_thread = std::thread(&Ultrasonic::UltrasonicChecker);
|
||||
} else {
|
||||
// Wait for background task to stop running
|
||||
if (m_thread.joinable()) {
|
||||
m_thread.join();
|
||||
}
|
||||
|
||||
// Clear all the counters (data now invalid) since automatic mode is
|
||||
// disabled. No synchronization is needed because the background task is
|
||||
// stopped.
|
||||
for (auto& sensor : m_sensors) {
|
||||
sensor->m_counter.Reset();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
units::meter_t Ultrasonic::GetRange() const {
|
||||
if (IsRangeValid()) {
|
||||
if (m_simRange) {
|
||||
return units::inch_t{m_simRange.Get()};
|
||||
}
|
||||
return m_counter.GetPeriod() * kSpeedOfSound / 2.0;
|
||||
} else {
|
||||
return 0_m;
|
||||
}
|
||||
}
|
||||
|
||||
bool Ultrasonic::IsEnabled() const {
|
||||
return m_enabled;
|
||||
}
|
||||
|
||||
void Ultrasonic::SetEnabled(bool enable) {
|
||||
m_enabled = enable;
|
||||
}
|
||||
|
||||
void Ultrasonic::InitSendable(wpi::SendableBuilder& builder) {
|
||||
builder.SetSmartDashboardType("Ultrasonic");
|
||||
builder.AddDoubleProperty(
|
||||
"Value", [=, this] { return units::inch_t{GetRange()}.value(); },
|
||||
nullptr);
|
||||
}
|
||||
|
||||
void Ultrasonic::Initialize() {
|
||||
m_simDevice = hal::SimDevice("Ultrasonic", m_echoChannel->GetChannel());
|
||||
if (m_simDevice) {
|
||||
m_simRangeValid = m_simDevice.CreateBoolean("Range Valid", false, true);
|
||||
m_simRange = m_simDevice.CreateDouble("Range (in)", false, 0.0);
|
||||
m_pingChannel->SetSimDevice(m_simDevice);
|
||||
m_echoChannel->SetSimDevice(m_simDevice);
|
||||
}
|
||||
|
||||
bool originalMode = m_automaticEnabled;
|
||||
SetAutomaticMode(false); // Kill task when adding a new sensor
|
||||
// Link this instance on the list
|
||||
m_sensors.emplace_back(this);
|
||||
|
||||
m_counter.SetMaxPeriod(1_s);
|
||||
m_counter.SetSemiPeriodMode(true);
|
||||
m_counter.Reset();
|
||||
m_enabled = true; // Make it available for round robin scheduling
|
||||
SetAutomaticMode(originalMode);
|
||||
|
||||
static int instances = 0;
|
||||
instances++;
|
||||
HAL_Report(HALUsageReporting::kResourceType_Ultrasonic, instances);
|
||||
wpi::SendableRegistry::Add(this, "Ultrasonic", m_echoChannel->GetChannel());
|
||||
}
|
||||
|
||||
void Ultrasonic::UltrasonicChecker() {
|
||||
while (m_automaticEnabled) {
|
||||
for (auto& sensor : m_sensors) {
|
||||
if (!m_automaticEnabled) {
|
||||
break;
|
||||
}
|
||||
|
||||
if (sensor->IsEnabled()) {
|
||||
sensor->m_pingChannel->Pulse(kPingTime); // do the ping
|
||||
}
|
||||
|
||||
Wait(100_ms); // wait for ping to return
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,99 +0,0 @@
|
||||
// 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 <memory>
|
||||
|
||||
#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::Add(this, "External Direction Counter", m_index);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
@@ -4,37 +4,35 @@
|
||||
|
||||
#include "frc/counter/Tachometer.h"
|
||||
|
||||
#include <frc/DigitalSource.h>
|
||||
#include <string>
|
||||
|
||||
#include <hal/Counter.h>
|
||||
#include <hal/FRCUsageReporting.h>
|
||||
#include <wpi/NullDeleter.h>
|
||||
#include <wpi/StackTrace.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;
|
||||
|
||||
Tachometer::Tachometer(int channel, EdgeConfiguration configuration)
|
||||
: m_channel{channel} {
|
||||
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);
|
||||
std::string stackTrace = wpi::GetStackTrace(1);
|
||||
m_handle = HAL_InitializeCounter(
|
||||
channel, configuration == EdgeConfiguration::kRisingEdge,
|
||||
stackTrace.c_str(), &status);
|
||||
FRC_CheckErrorStatus(status, "{}", channel);
|
||||
|
||||
HAL_Report(HALUsageReporting::kResourceType_Counter, m_index + 1);
|
||||
wpi::SendableRegistry::Add(this, "Tachometer", m_index);
|
||||
HAL_Report(HALUsageReporting::kResourceType_Counter, channel + 1);
|
||||
wpi::SendableRegistry::Add(this, "Tachometer", channel);
|
||||
}
|
||||
|
||||
void Tachometer::SetEdgeConfiguration(EdgeConfiguration configuration) {
|
||||
int32_t status = 0;
|
||||
bool rising = configuration == EdgeConfiguration::kRisingEdge;
|
||||
HAL_SetCounterEdgeConfiguration(m_handle, rising, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", m_channel);
|
||||
}
|
||||
|
||||
units::hertz_t Tachometer::GetFrequency() const {
|
||||
@@ -48,7 +46,7 @@ units::hertz_t Tachometer::GetFrequency() const {
|
||||
units::second_t Tachometer::GetPeriod() const {
|
||||
int32_t status = 0;
|
||||
double period = HAL_GetCounterPeriod(m_handle, &status);
|
||||
FRC_CheckErrorStatus(status, "Channel {}", m_source->GetChannel());
|
||||
FRC_CheckErrorStatus(status, "Channel {}", m_channel);
|
||||
return units::second_t{period};
|
||||
}
|
||||
|
||||
@@ -79,33 +77,14 @@ units::revolutions_per_minute_t Tachometer::GetRevolutionsPerMinute() const {
|
||||
bool Tachometer::GetStopped() const {
|
||||
int32_t status = 0;
|
||||
bool stopped = HAL_GetCounterStopped(m_handle, &status);
|
||||
FRC_CheckErrorStatus(status, "Channel {}", m_source->GetChannel());
|
||||
FRC_CheckErrorStatus(status, "Channel {}", m_channel);
|
||||
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.value(), &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());
|
||||
FRC_CheckErrorStatus(status, "Channel {}", m_channel);
|
||||
}
|
||||
|
||||
void Tachometer::InitSendable(wpi::SendableBuilder& builder) {
|
||||
|
||||
@@ -5,10 +5,11 @@
|
||||
#include "frc/counter/UpDownCounter.h"
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include <hal/Counter.h>
|
||||
#include <hal/FRCUsageReporting.h>
|
||||
#include <wpi/NullDeleter.h>
|
||||
#include <wpi/StackTrace.h>
|
||||
#include <wpi/sendable/SendableBuilder.h>
|
||||
|
||||
#include "frc/DigitalSource.h"
|
||||
@@ -16,84 +17,39 @@
|
||||
|
||||
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;
|
||||
|
||||
UpDownCounter::UpDownCounter(int channel, EdgeConfiguration configuration)
|
||||
: m_channel{channel} {
|
||||
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);
|
||||
}
|
||||
std::string stackTrace = wpi::GetStackTrace(1);
|
||||
m_handle = HAL_InitializeCounter(
|
||||
channel, configuration == EdgeConfiguration::kRisingEdge,
|
||||
stackTrace.c_str(), &status);
|
||||
FRC_CheckErrorStatus(status, "{}", channel);
|
||||
|
||||
Reset();
|
||||
|
||||
HAL_Report(HALUsageReporting::kResourceType_Counter, m_index + 1);
|
||||
wpi::SendableRegistry::Add(this, "UpDown Counter", m_index);
|
||||
HAL_Report(HALUsageReporting::kResourceType_Counter, channel + 1);
|
||||
wpi::SendableRegistry::Add(this, "UpDown Counter", channel);
|
||||
}
|
||||
|
||||
int UpDownCounter::GetCount() const {
|
||||
int32_t status = 0;
|
||||
int val = HAL_GetCounter(m_handle, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", m_index);
|
||||
FRC_CheckErrorStatus(status, "{}", m_channel);
|
||||
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);
|
||||
FRC_CheckErrorStatus(status, "{}", m_channel);
|
||||
}
|
||||
|
||||
void UpDownCounter::SetUpEdgeConfiguration(EdgeConfiguration configuration) {
|
||||
void UpDownCounter::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 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);
|
||||
bool rising = configuration == EdgeConfiguration::kRisingEdge;
|
||||
HAL_SetCounterEdgeConfiguration(m_handle, rising, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", m_channel);
|
||||
}
|
||||
|
||||
void UpDownCounter::InitSendable(wpi::SendableBuilder& builder) {
|
||||
|
||||
@@ -1,27 +0,0 @@
|
||||
// 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/simulation/UltrasonicSim.h"
|
||||
|
||||
#include "frc/Ultrasonic.h"
|
||||
#include "frc/simulation/SimDeviceSim.h"
|
||||
|
||||
using namespace frc::sim;
|
||||
|
||||
UltrasonicSim::UltrasonicSim(const frc::Ultrasonic& ultrasonic)
|
||||
: UltrasonicSim(0, ultrasonic.GetEchoChannel()) {}
|
||||
|
||||
UltrasonicSim::UltrasonicSim(int ping, int echo) {
|
||||
frc::sim::SimDeviceSim deviceSim{"Ultrasonic", echo};
|
||||
m_simRangeValid = deviceSim.GetBoolean("Range Valid");
|
||||
m_simRange = deviceSim.GetDouble("Range (in)");
|
||||
}
|
||||
|
||||
void UltrasonicSim::SetRangeValid(bool valid) {
|
||||
m_simRangeValid.Set(valid);
|
||||
}
|
||||
|
||||
void UltrasonicSim::SetRange(units::inch_t range) {
|
||||
m_simRange.Set(range.value());
|
||||
}
|
||||
Reference in New Issue
Block a user