Merge commit '849ebe917b82f5099633e6b5a915d550e76bc6bc' into HEAD
Change-Id: Ib317023b5d8bffb49eb4e05c49f635e094a88e63
diff --git a/frc971/analysis/BUILD b/frc971/analysis/BUILD
index b45031f..ad5552b 100644
--- a/frc971/analysis/BUILD
+++ b/frc971/analysis/BUILD
@@ -3,10 +3,10 @@
py_binary(
name = "plot_action",
srcs = [
- "logentry.py",
- "logreader.py",
- "plot_action.py",
"plotter.py",
+ "plot_action.py",
+ "logreader.py",
+ "logentry.py",
],
legacy_create_init = False,
restricted_to = ["//tools:k8"],
diff --git a/frc971/config/BUILD b/frc971/config/BUILD
index 0f4f7ba..07394d4 100644
--- a/frc971/config/BUILD
+++ b/frc971/config/BUILD
@@ -1,14 +1,14 @@
filegroup(
- name = "rio_robotCommand",
- srcs = ["robotCommand"],
+ name = 'rio_robotCommand',
+ srcs = [ 'robotCommand' ],
)
sh_binary(
- name = "setup_roborio",
- srcs = ["setup_roborio.sh"],
- data = [
- ":rio_robotCommand",
- "@arm_frc_linux_gnueabi_repo//:compiler_pieces",
- ],
- visibility = ["//visibility:public"],
+ name = 'setup_roborio',
+ srcs = [ 'setup_roborio.sh' ],
+ visibility = [ '//visibility:public' ],
+ data = [
+ ':rio_robotCommand',
+ '@arm_frc_linux_gnueabi_repo//:compiler_pieces',
+ ],
)
diff --git a/frc971/control_loops/voltage_cap/BUILD b/frc971/control_loops/voltage_cap/BUILD
index 2aa1dea..8c2df46 100644
--- a/frc971/control_loops/voltage_cap/BUILD
+++ b/frc971/control_loops/voltage_cap/BUILD
@@ -1,23 +1,23 @@
-package(default_visibility = ["//visibility:public"])
+package(default_visibility = ['//visibility:public'])
cc_library(
- name = "voltage_cap",
- srcs = [
- "voltage_cap.cc",
- ],
- hdrs = [
- "voltage_cap.h",
- ],
+ name = 'voltage_cap',
+ srcs = [
+ 'voltage_cap.cc',
+ ],
+ hdrs = [
+ 'voltage_cap.h',
+ ],
)
cc_test(
- name = "voltage_cap_test",
- srcs = [
- "voltage_cap_test.cc",
- ],
- deps = [
- ":voltage_cap",
- "//aos/testing:googletest",
- "//aos/testing:test_shm",
- ],
+ name = 'voltage_cap_test',
+ srcs = [
+ 'voltage_cap_test.cc',
+ ],
+ deps = [
+ ':voltage_cap',
+ '//aos/testing:googletest',
+ '//aos/testing:test_shm',
+ ],
)
diff --git a/frc971/shooter_interpolation/BUILD b/frc971/shooter_interpolation/BUILD
index cce93e1..3509040 100644
--- a/frc971/shooter_interpolation/BUILD
+++ b/frc971/shooter_interpolation/BUILD
@@ -1,21 +1,21 @@
cc_library(
- name = "interpolation",
- srcs = [
- "interpolation.cc",
- ],
- hdrs = [
- "interpolation.h",
- ],
- visibility = ["//visibility:public"],
+ name = 'interpolation',
+ hdrs = [
+ 'interpolation.h',
+ ],
+ srcs = [
+ 'interpolation.cc',
+ ],
+ visibility = ['//visibility:public'],
)
cc_test(
- name = "interpolation_test",
- srcs = [
- "interpolation_test.cc",
- ],
- deps = [
- ":interpolation",
- "//aos/testing:googletest",
- ],
+ name = 'interpolation_test',
+ srcs = [
+ 'interpolation_test.cc',
+ ],
+ deps = [
+ ':interpolation',
+ '//aos/testing:googletest',
+ ]
)
diff --git a/frc971/wpilib/ahal/AnalogTrigger.cc b/frc971/wpilib/ahal/AnalogTrigger.cc
index 194537c..825a655 100644
--- a/frc971/wpilib/ahal/AnalogTrigger.cc
+++ b/frc971/wpilib/ahal/AnalogTrigger.cc
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-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. */
@@ -8,176 +8,135 @@
#include "frc971/wpilib/ahal/AnalogTrigger.h"
#include <memory>
+#include <utility>
+#include <hal/FRCUsageReporting.h>
#include <hal/HAL.h>
#include "frc971/wpilib/ahal/AnalogInput.h"
+#include "frc971/wpilib/ahal/Base.h"
+#include "frc971/wpilib/ahal/DutyCycle.h"
#include "frc971/wpilib/ahal/WPIErrors.h"
using namespace frc;
-/**
- * Constructor for an analog trigger given a channel number.
- *
- * @param channel The channel number on the roboRIO to represent. 0-3 are
- * on-board 4-7 are on the MXP port.
- */
AnalogTrigger::AnalogTrigger(int channel)
: AnalogTrigger(new AnalogInput(channel)) {
m_ownsAnalog = true;
}
-/**
- * Construct an analog trigger given an analog input.
- *
- * This should be used in the case of sharing an analog channel between the
- * trigger and an analog input object.
- *
- * @param channel The pointer to the existing AnalogInput object
- */
-AnalogTrigger::AnalogTrigger(AnalogInput *input) {
+AnalogTrigger::AnalogTrigger(AnalogInput* input) {
m_analogInput = input;
int32_t status = 0;
- int index = 0;
- m_trigger = HAL_InitializeAnalogTrigger(input->m_port, &index, &status);
+ m_trigger = HAL_InitializeAnalogTrigger(input->m_port, &status);
if (status != 0) {
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
- m_index = std::numeric_limits<int>::max();
+ wpi_setHALError(status);
m_trigger = HAL_kInvalidHandle;
return;
}
- m_index = index;
+ int index = GetIndex();
- HAL_Report(HALUsageReporting::kResourceType_AnalogTrigger, input->m_channel);
+ HAL_Report(HALUsageReporting::kResourceType_AnalogTrigger, index + 1);
+}
+
+AnalogTrigger::AnalogTrigger(DutyCycle* input) {
+ m_dutyCycle = input;
+ int32_t status = 0;
+ m_trigger = HAL_InitializeAnalogTriggerDutyCycle(input->m_handle, &status);
+ if (status != 0) {
+ wpi_setHALError(status);
+ m_trigger = HAL_kInvalidHandle;
+ return;
+ }
+ int index = GetIndex();
+
+ HAL_Report(HALUsageReporting::kResourceType_AnalogTrigger, index + 1);
}
AnalogTrigger::~AnalogTrigger() {
int32_t status = 0;
HAL_CleanAnalogTrigger(m_trigger, &status);
- if (m_ownsAnalog && m_analogInput != nullptr) {
+ if (m_ownsAnalog) {
delete m_analogInput;
}
}
-/**
- * Set the upper and lower limits of the analog trigger.
- *
- * The limits are given in ADC codes. If oversampling is used, the units must
- * be scaled appropriately.
- *
- * @param lower The lower limit of the trigger in ADC codes (12-bit values).
- * @param upper The upper limit of the trigger in ADC codes (12-bit values).
- */
-void AnalogTrigger::SetLimitsRaw(int lower, int upper) {
- if (StatusIsFatal()) return;
- int32_t status = 0;
- HAL_SetAnalogTriggerLimitsRaw(m_trigger, lower, upper, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+AnalogTrigger::AnalogTrigger(AnalogTrigger &&rhs)
+ : m_trigger(std::move(rhs.m_trigger)) {
+ std::swap(m_analogInput, rhs.m_analogInput);
+ std::swap(m_dutyCycle, rhs.m_dutyCycle);
+ std::swap(m_ownsAnalog, rhs.m_ownsAnalog);
}
-/**
- * Set the upper and lower limits of the analog trigger.
- *
- * The limits are given as floating point voltage values.
- *
- * @param lower The lower limit of the trigger in Volts.
- * @param upper The upper limit of the trigger in Volts.
- */
+AnalogTrigger& AnalogTrigger::operator=(AnalogTrigger&& rhs) {
+ m_trigger = std::move(rhs.m_trigger);
+ std::swap(m_analogInput, rhs.m_analogInput);
+ std::swap(m_dutyCycle, rhs.m_dutyCycle);
+ std::swap(m_ownsAnalog, rhs.m_ownsAnalog);
+
+ return *this;
+}
+
void AnalogTrigger::SetLimitsVoltage(double lower, double upper) {
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetAnalogTriggerLimitsVoltage(m_trigger, lower, upper, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
-/**
- * Configure the analog trigger to use the averaged vs. raw values.
- *
- * If the value is true, then the averaged value is selected for the analog
- * trigger, otherwise the immediate value is used.
- *
- * @param useAveragedValue If true, use the Averaged value, otherwise use the
- * instantaneous reading
- */
+void AnalogTrigger::SetLimitsDutyCycle(double lower, double upper) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetAnalogTriggerLimitsDutyCycle(m_trigger, lower, upper, &status);
+ wpi_setHALError(status);
+}
+
+void AnalogTrigger::SetLimitsRaw(int lower, int upper) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetAnalogTriggerLimitsRaw(m_trigger, lower, upper, &status);
+ wpi_setHALError(status);
+}
+
void AnalogTrigger::SetAveraged(bool useAveragedValue) {
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetAnalogTriggerAveraged(m_trigger, useAveragedValue, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
-/**
- * Configure the analog trigger to use a filtered value.
- *
- * The analog trigger will operate with a 3 point average rejection filter. This
- * is designed to help with 360 degree pot applications for the period where
- * the pot crosses through zero.
- *
- * @param useFilteredValue If true, use the 3 point rejection filter, otherwise
- * use the unfiltered value
- */
void AnalogTrigger::SetFiltered(bool useFilteredValue) {
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetAnalogTriggerFiltered(m_trigger, useFilteredValue, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
-/**
- * Return the index of the analog trigger.
- *
- * This is the FPGA index of this analog trigger instance.
- *
- * @return The index of the analog trigger.
- */
int AnalogTrigger::GetIndex() const {
if (StatusIsFatal()) return -1;
- return m_index;
+ int32_t status = 0;
+ auto ret = HAL_GetAnalogTriggerFPGAIndex(m_trigger, &status);
+ wpi_setHALError(status);
+ return ret;
}
-/**
- * Return the InWindow output of the analog trigger.
- *
- * True if the analog input is between the upper and lower limits.
- *
- * @return True if the analog input is between the upper and lower limits.
- */
bool AnalogTrigger::GetInWindow() {
if (StatusIsFatal()) return false;
int32_t status = 0;
bool result = HAL_GetAnalogTriggerInWindow(m_trigger, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return result;
}
-/**
- * Return the TriggerState output of the analog trigger.
- *
- * True if above upper limit.
- * False if below lower limit.
- * If in Hysteresis, maintain previous state.
- *
- * @return True if above upper limit. False if below lower limit. If in
- * Hysteresis, maintain previous state.
- */
bool AnalogTrigger::GetTriggerState() {
if (StatusIsFatal()) return false;
int32_t status = 0;
bool result = HAL_GetAnalogTriggerTriggerState(m_trigger, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return result;
}
-/**
- * Creates an AnalogTriggerOutput object.
- *
- * Gets an output object that can be used for routing.
- * Caller is responsible for deleting the AnalogTriggerOutput object.
- *
- * @param type An enum of the type of output object to create.
- * @return A pointer to a new AnalogTriggerOutput object.
- */
std::shared_ptr<AnalogTriggerOutput> AnalogTrigger::CreateOutput(
AnalogTriggerType type) const {
if (StatusIsFatal()) return nullptr;
diff --git a/frc971/wpilib/ahal/AnalogTrigger.h b/frc971/wpilib/ahal/AnalogTrigger.h
index 30a07d1..9fad5db 100644
--- a/frc971/wpilib/ahal/AnalogTrigger.h
+++ b/frc971/wpilib/ahal/AnalogTrigger.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
+/* Copyright (c) 2008-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. */
@@ -9,36 +9,54 @@
#include <memory>
-#include "hal/Types.h"
+#include <hal/Types.h>
+
#include "frc971/wpilib/ahal/AnalogTriggerOutput.h"
-#include "frc971/wpilib/ahal/SensorBase.h"
namespace frc {
class AnalogInput;
+class DutyCycle;
+class SendableBuilder;
class AnalogTrigger {
friend class AnalogTriggerOutput;
public:
explicit AnalogTrigger(int channel);
- explicit AnalogTrigger(AnalogInput *channel);
+ explicit AnalogTrigger(AnalogInput* channel);
+ explicit AnalogTrigger(DutyCycle* dutyCycle);
+
virtual ~AnalogTrigger();
+ AnalogTrigger(AnalogTrigger&& rhs);
+ AnalogTrigger& operator=(AnalogTrigger&& rhs);
+
void SetLimitsVoltage(double lower, double upper);
+
+ void SetLimitsDutyCycle(double lower, double upper);
+
void SetLimitsRaw(int lower, int upper);
+
void SetAveraged(bool useAveragedValue);
+
+ void SetDutyCycle(bool useDutyCycle);
+
void SetFiltered(bool useFilteredValue);
+
int GetIndex() const;
+
bool GetInWindow();
+
bool GetTriggerState();
+
std::shared_ptr<AnalogTriggerOutput> CreateOutput(
AnalogTriggerType type) const;
private:
- int m_index;
- HAL_AnalogTriggerHandle m_trigger;
- AnalogInput *m_analogInput = nullptr;
+ hal::Handle<HAL_AnalogTriggerHandle> m_trigger;
+ AnalogInput* m_analogInput = nullptr;
+ DutyCycle* m_dutyCycle = nullptr;
bool m_ownsAnalog = false;
};
diff --git a/frc971/wpilib/ahal/AnalogTriggerOutput.cc b/frc971/wpilib/ahal/AnalogTriggerOutput.cc
index d95b1d7..db55efa 100644
--- a/frc971/wpilib/ahal/AnalogTriggerOutput.cc
+++ b/frc971/wpilib/ahal/AnalogTriggerOutput.cc
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-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. */
@@ -14,67 +14,30 @@
using namespace frc;
-/**
- * Create an object that represents one of the four outputs from an analog
- * trigger.
- *
- * Because this class derives from DigitalSource, it can be passed into routing
- * functions for Counter, Encoder, etc.
- *
- * @param trigger A pointer to the trigger for which this is an output.
- * @param outputType An enum that specifies the output on the trigger to
- * represent.
- */
-AnalogTriggerOutput::AnalogTriggerOutput(const AnalogTrigger &trigger,
- AnalogTriggerType outputType)
- : m_trigger(trigger), m_outputType(outputType) {
- HAL_Report(HALUsageReporting::kResourceType_AnalogTriggerOutput,
- trigger.GetIndex(), static_cast<uint8_t>(outputType));
-}
-
-AnalogTriggerOutput::~AnalogTriggerOutput() {
- if (m_interrupt != HAL_kInvalidHandle) {
- int32_t status = 0;
- HAL_CleanInterrupts(m_interrupt, &status);
- // ignore status, as an invalid handle just needs to be ignored.
- m_interrupt = HAL_kInvalidHandle;
- }
-}
-
-/**
- * Get the state of the analog trigger output.
- *
- * @return The state of the analog trigger output.
- */
bool AnalogTriggerOutput::Get() const {
int32_t status = 0;
bool result = HAL_GetAnalogTriggerOutput(
- m_trigger.m_trigger, static_cast<HAL_AnalogTriggerType>(m_outputType),
+ m_trigger->m_trigger, static_cast<HAL_AnalogTriggerType>(m_outputType),
&status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return result;
}
-/**
- * @return The HAL Handle to the specified source.
- */
HAL_Handle AnalogTriggerOutput::GetPortHandleForRouting() const {
- return m_trigger.m_trigger;
+ return m_trigger->m_trigger;
}
-/**
- * Is source an AnalogTrigger
- */
-bool AnalogTriggerOutput::IsAnalogTrigger() const { return true; }
-
-/**
- * @return The type of analog trigger output to be used.
- */
AnalogTriggerType AnalogTriggerOutput::GetAnalogTriggerTypeForRouting() const {
return m_outputType;
}
-/**
- * @return The channel of the source.
- */
-int AnalogTriggerOutput::GetChannel() const { return m_trigger.m_index; }
+bool AnalogTriggerOutput::IsAnalogTrigger() const { return true; }
+
+int AnalogTriggerOutput::GetChannel() const { return m_trigger->GetIndex(); }
+
+AnalogTriggerOutput::AnalogTriggerOutput(const AnalogTrigger& trigger,
+ AnalogTriggerType outputType)
+ : m_trigger(&trigger), m_outputType(outputType) {
+ HAL_Report(HALUsageReporting::kResourceType_AnalogTriggerOutput,
+ trigger.GetIndex() + 1, static_cast<uint8_t>(outputType) + 1);
+}
diff --git a/frc971/wpilib/ahal/AnalogTriggerOutput.h b/frc971/wpilib/ahal/AnalogTriggerOutput.h
index 7eb3163..8896f37 100644
--- a/frc971/wpilib/ahal/AnalogTriggerOutput.h
+++ b/frc971/wpilib/ahal/AnalogTriggerOutput.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
+/* Copyright (c) 2008-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. */
@@ -63,10 +63,10 @@
AnalogTriggerType outputType);
private:
- // Uses reference rather than smart pointer because a user can not construct
+ // Uses pointer rather than smart pointer because a user can not construct
// an AnalogTriggerOutput themselves and because the AnalogTriggerOutput
// should always be in scope at the same time as an AnalogTrigger.
- const AnalogTrigger &m_trigger;
+ const AnalogTrigger* m_trigger;
AnalogTriggerType m_outputType;
};
diff --git a/frc971/wpilib/ahal/Counter.cc b/frc971/wpilib/ahal/Counter.cc
index 3614b8c..81e2c61 100644
--- a/frc971/wpilib/ahal/Counter.cc
+++ b/frc971/wpilib/ahal/Counter.cc
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
+/* Copyright (c) 2008-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. */
@@ -9,129 +9,51 @@
#include "hal/HAL.h"
#include "frc971/wpilib/ahal/AnalogTrigger.h"
+#include "frc971/wpilib/ahal/Base.h"
#include "frc971/wpilib/ahal/DigitalInput.h"
#include "frc971/wpilib/ahal/WPIErrors.h"
using namespace frc;
-/**
- * Create an instance of a counter where no sources are selected.
- *
- * They all must be selected by calling functions to specify the upsource and
- * the downsource independently.
- *
- * This creates a ChipObject counter and initializes status variables
- * appropriately.
- *
- * The counter will start counting immediately.
- *
- * @param mode The counter mode
- */
Counter::Counter(Mode mode) {
int32_t status = 0;
m_counter = HAL_InitializeCounter((HAL_Counter_Mode)mode, &m_index, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
- SetMaxPeriod(.5);
+ SetMaxPeriod(0.5);
- HAL_Report(HALUsageReporting::kResourceType_Counter, m_index, mode);
+ HAL_Report(HALUsageReporting::kResourceType_Counter, m_index + 1, mode + 1);
}
-/**
- * Create an instance of a counter from a Digital Source (such as a Digital
- * Input).
- *
- * This is used if an existing digital input is to be shared by multiple other
- * objects such as encoders or if the Digital Source is not a Digital Input
- * channel (such as an Analog Trigger).
- *
- * The counter will start counting immediately.
- * @param source A pointer to the existing DigitalSource object. It will be set
- * as the Up Source.
- */
-Counter::Counter(DigitalSource *source) : Counter(kTwoPulse) {
- SetUpSource(source);
- ClearDownSource();
-}
-
-/**
- * Create an instance of a counter from a Digital Source (such as a Digital
- * Input).
- *
- * This is used if an existing digital input is to be shared by multiple other
- * objects such as encoders or if the Digital Source is not a Digital Input
- * channel (such as an Analog Trigger).
- *
- * The counter will start counting immediately.
- *
- * @param source A pointer to the existing DigitalSource object. It will be
- * set as the Up Source.
- */
-Counter::Counter(std::shared_ptr<DigitalSource> source) : Counter(kTwoPulse) {
- SetUpSource(source);
- ClearDownSource();
-}
-
-/**
- * Create an instance of a Counter object.
- *
- * Create an up-Counter instance given a channel.
- *
- * The counter will start counting immediately.
- *
- * @param channel The DIO channel to use as the up source. 0-9 are on-board,
- * 10-25 are on the MXP
- */
Counter::Counter(int channel) : Counter(kTwoPulse) {
SetUpSource(channel);
ClearDownSource();
}
-/**
- * Create an instance of a Counter object.
- *
- * Create an instance of a simple up-Counter given an analog trigger.
- * Use the trigger state output from the analog trigger.
- *
- * The counter will start counting immediately.
- *
- * @param trigger The reference to the existing AnalogTrigger object.
- */
-Counter::Counter(const AnalogTrigger &trigger) : Counter(kTwoPulse) {
+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();
}
-/**
- * Create an instance of a Counter object.
- *
- * Creates a full up-down counter given two Digital Sources.
- *
- * @param encodingType The quadrature decoding mode (1x or 2x)
- * @param upSource The pointer to the DigitalSource to set as the up source
- * @param downSource The pointer to the DigitalSource to set as the down
- * source
- * @param inverted True to invert the output (reverse the direction)
- */
-Counter::Counter(EncodingType encodingType, DigitalSource *upSource,
- DigitalSource *downSource, bool inverted)
- : Counter(encodingType, std::shared_ptr<DigitalSource>(
- upSource, NullDeleter<DigitalSource>()),
+Counter::Counter(EncodingType encodingType, DigitalSource* upSource,
+ DigitalSource* downSource, bool inverted)
+ : Counter(encodingType,
+ std::shared_ptr<DigitalSource>(upSource,
+ NullDeleter<DigitalSource>()),
std::shared_ptr<DigitalSource>(downSource,
NullDeleter<DigitalSource>()),
inverted) {}
-/**
- * Create an instance of a Counter object.
- *
- * Creates a full up-down counter given two Digital Sources.
- *
- * @param encodingType The quadrature decoding mode (1x or 2x)
- * @param upSource The pointer to the DigitalSource to set as the up source
- * @param downSource The pointer to the DigitalSource to set as the down
- * source
- * @param inverted True to invert the output (reverse the direction)
- */
Counter::Counter(EncodingType encodingType,
std::shared_ptr<DigitalSource> upSource,
std::shared_ptr<DigitalSource> downSource, bool inverted)
@@ -154,98 +76,58 @@
HAL_SetCounterAverageSize(m_counter, 2, &status);
}
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
SetDownSourceEdge(inverted, true);
}
-/**
- * Delete the Counter object.
- */
Counter::~Counter() {
SetUpdateWhenEmpty(true);
int32_t status = 0;
HAL_FreeCounter(m_counter, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
- m_counter = HAL_kInvalidHandle;
+ wpi_setHALError(status);
}
-/**
- * Set the upsource for the counter as a digital input channel.
- *
- * @param channel The DIO channel to use as the up source. 0-9 are on-board,
- * 10-25 are on the MXP
- */
void Counter::SetUpSource(int channel) {
+ if (StatusIsFatal()) return;
SetUpSource(std::make_shared<DigitalInput>(channel));
}
-/**
- * Set the up counting source to be an analog trigger.
- *
- * @param analogTrigger The analog trigger object that is used for the Up Source
- * @param triggerType The analog trigger output that will trigger the counter.
- */
-void Counter::SetUpSource(AnalogTrigger *analogTrigger,
+void Counter::SetUpSource(AnalogTrigger* analogTrigger,
AnalogTriggerType triggerType) {
SetUpSource(std::shared_ptr<AnalogTrigger>(analogTrigger,
NullDeleter<AnalogTrigger>()),
triggerType);
}
-/**
- * Set the up counting source to be an analog trigger.
- *
- * @param analogTrigger The analog trigger object that is used for the Up Source
- * @param triggerType The analog trigger output that will trigger the counter.
- */
void Counter::SetUpSource(std::shared_ptr<AnalogTrigger> analogTrigger,
AnalogTriggerType triggerType) {
+ if (StatusIsFatal()) return;
SetUpSource(analogTrigger->CreateOutput(triggerType));
}
-/**
- * Set the source object that causes the counter to count up.
- *
- * Set the up counting DigitalSource.
- *
- * @param source Pointer to the DigitalSource object to set as the up source
- */
+void Counter::SetUpSource(DigitalSource* source) {
+ SetUpSource(
+ std::shared_ptr<DigitalSource>(source, NullDeleter<DigitalSource>()));
+}
+
void Counter::SetUpSource(std::shared_ptr<DigitalSource> source) {
+ if (StatusIsFatal()) return;
m_upSource = source;
int32_t status = 0;
HAL_SetCounterUpSource(
m_counter, source->GetPortHandleForRouting(),
(HAL_AnalogTriggerType)source->GetAnalogTriggerTypeForRouting(), &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
-void Counter::SetUpSource(DigitalSource *source) {
- SetUpSource(
- std::shared_ptr<DigitalSource>(source, NullDeleter<DigitalSource>()));
-}
-
-/**
- * Set the source object that causes the counter to count up.
- *
- * Set the up counting DigitalSource.
- *
- * @param source Reference to the DigitalSource object to set as the up source
- */
-void Counter::SetUpSource(DigitalSource &source) {
+void Counter::SetUpSource(DigitalSource& source) {
SetUpSource(
std::shared_ptr<DigitalSource>(&source, NullDeleter<DigitalSource>()));
}
-/**
- * Set the edge sensitivity on an up counting source.
- *
- * Set the up source to either detect rising edges or falling edges or both.
- *
- * @param risingEdge True to trigger on rising edges
- * @param fallingEdge True to trigger on falling edges
- */
void Counter::SetUpSourceEdge(bool risingEdge, bool fallingEdge) {
+ if (StatusIsFatal()) return;
if (m_upSource == nullptr) {
wpi_setWPIErrorWithContext(
NullParameter,
@@ -253,97 +135,57 @@
}
int32_t status = 0;
HAL_SetCounterUpSourceEdge(m_counter, risingEdge, fallingEdge, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
-/**
- * Disable the up counting source to the counter.
- */
void Counter::ClearUpSource() {
+ if (StatusIsFatal()) return;
m_upSource.reset();
int32_t status = 0;
HAL_ClearCounterUpSource(m_counter, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
-/**
- * Set the down counting source to be a digital input channel.
- *
- * @param channel The DIO channel to use as the up source. 0-9 are on-board,
- * 10-25 are on the MXP
- */
void Counter::SetDownSource(int channel) {
+ if (StatusIsFatal()) return;
SetDownSource(std::make_shared<DigitalInput>(channel));
}
-/**
- * Set the down counting source to be an analog trigger.
- *
- * @param analogTrigger The analog trigger object that is used for the Down
- * Source
- * @param triggerType The analog trigger output that will trigger the counter.
- */
-void Counter::SetDownSource(AnalogTrigger *analogTrigger,
+void Counter::SetDownSource(AnalogTrigger* analogTrigger,
AnalogTriggerType triggerType) {
SetDownSource(std::shared_ptr<AnalogTrigger>(analogTrigger,
NullDeleter<AnalogTrigger>()),
triggerType);
}
-/**
- * Set the down counting source to be an analog trigger.
- *
- * @param analogTrigger The analog trigger object that is used for the Down
- * Source
- * @param triggerType The analog trigger output that will trigger the counter.
- */
void Counter::SetDownSource(std::shared_ptr<AnalogTrigger> analogTrigger,
AnalogTriggerType triggerType) {
+ if (StatusIsFatal()) return;
SetDownSource(analogTrigger->CreateOutput(triggerType));
}
-/**
- * Set the source object that causes the counter to count down.
- *
- * Set the down counting DigitalSource.
- *
- * @param source Pointer to the DigitalSource object to set as the down source
- */
+void Counter::SetDownSource(DigitalSource* source) {
+ SetDownSource(
+ std::shared_ptr<DigitalSource>(source, NullDeleter<DigitalSource>()));
+}
+
+void Counter::SetDownSource(DigitalSource& source) {
+ SetDownSource(
+ std::shared_ptr<DigitalSource>(&source, NullDeleter<DigitalSource>()));
+}
+
void Counter::SetDownSource(std::shared_ptr<DigitalSource> source) {
+ if (StatusIsFatal()) return;
m_downSource = source;
int32_t status = 0;
HAL_SetCounterDownSource(
m_counter, source->GetPortHandleForRouting(),
(HAL_AnalogTriggerType)source->GetAnalogTriggerTypeForRouting(), &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
-void Counter::SetDownSource(DigitalSource *source) {
- SetDownSource(
- std::shared_ptr<DigitalSource>(source, NullDeleter<DigitalSource>()));
-}
-
-/**
- * Set the source object that causes the counter to count down.
- *
- * Set the down counting DigitalSource.
- *
- * @param source Reference to the DigitalSource object to set as the down source
- */
-void Counter::SetDownSource(DigitalSource &source) {
- SetDownSource(
- std::shared_ptr<DigitalSource>(&source, NullDeleter<DigitalSource>()));
-}
-
-/**
- * Set the edge sensitivity on a down counting source.
- *
- * Set the down source to either detect rising edges or falling edges.
- *
- * @param risingEdge True to trigger on rising edges
- * @param fallingEdge True to trigger on falling edges
- */
void Counter::SetDownSourceEdge(bool risingEdge, bool fallingEdge) {
+ if (StatusIsFatal()) return;
if (m_downSource == nullptr) {
wpi_setWPIErrorWithContext(
NullParameter,
@@ -351,91 +193,52 @@
}
int32_t status = 0;
HAL_SetCounterDownSourceEdge(m_counter, risingEdge, fallingEdge, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
-/**
- * Disable the down counting source to the counter.
- */
void Counter::ClearDownSource() {
+ if (StatusIsFatal()) return;
m_downSource.reset();
int32_t status = 0;
HAL_ClearCounterDownSource(m_counter, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
-/**
- * Set standard up / down counting mode on this counter.
- *
- * Up and down counts are sourced independently from two inputs.
- */
void Counter::SetUpDownCounterMode() {
+ if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetCounterUpDownMode(m_counter, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
-/**
- * Set external direction mode on this counter.
- *
- * Counts are sourced on the Up counter input.
- * The Down counter input represents the direction to count.
- */
void Counter::SetExternalDirectionMode() {
+ if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetCounterExternalDirectionMode(m_counter, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
-/**
- * Set Semi-period mode on this counter.
- *
- * Counts up on both rising and falling edges.
- */
void Counter::SetSemiPeriodMode(bool highSemiPeriod) {
+ if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetCounterSemiPeriodMode(m_counter, highSemiPeriod, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
-/**
- * Configure the counter to count in up or down based on the length of the input
- * pulse.
- *
- * This mode is most useful for direction sensitive gear tooth sensors.
- *
- * @param threshold The pulse length beyond which the counter counts the
- * opposite direction. Units are seconds.
- */
void Counter::SetPulseLengthMode(double threshold) {
+ if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetCounterPulseLengthMode(m_counter, threshold, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
-/**
- * Get the Samples to Average which specifies the number of samples of the timer
- * to average when calculating the period.
- *
- * Perform averaging to account for mechanical imperfections or as oversampling
- * to increase resolution.
- *
- * @return The number of samples being averaged (from 1 to 127)
- */
-int Counter::GetSamplesToAverage() const {
+void Counter::SetReverseDirection(bool reverseDirection) {
+ if (StatusIsFatal()) return;
int32_t status = 0;
- int samples = HAL_GetCounterSamplesToAverage(m_counter, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
- return samples;
+ HAL_SetCounterReverseDirection(m_counter, reverseDirection, &status);
+ wpi_setHALError(status);
}
-/**
- * Set the Samples to Average which specifies the number of samples of the timer
- * to average when calculating the period. Perform averaging to account for
- * mechanical imperfections or as oversampling to increase resolution.
- *
- * @param samplesToAverage The number of samples to average from 1 to 127.
- */
void Counter::SetSamplesToAverage(int samplesToAverage) {
if (samplesToAverage < 1 || samplesToAverage > 127) {
wpi_setWPIErrorWithContext(
@@ -444,127 +247,67 @@
}
int32_t status = 0;
HAL_SetCounterSamplesToAverage(m_counter, samplesToAverage, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
-/**
- * Read the current counter value.
- *
- * Read the value at this instant. It may still be running, so it reflects the
- * current value. Next time it is read, it might have a different value.
- */
+int Counter::GetSamplesToAverage() const {
+ int32_t status = 0;
+ int samples = HAL_GetCounterSamplesToAverage(m_counter, &status);
+ wpi_setHALError(status);
+ return samples;
+}
+
+int Counter::GetFPGAIndex() const { return m_index; }
+
int Counter::Get() const {
+ if (StatusIsFatal()) return 0;
int32_t status = 0;
int value = HAL_GetCounter(m_counter, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
-/**
- * Reset the Counter to zero.
- *
- * Set the counter value to zero. This doesn't effect the running state of the
- * counter, just sets the current value to zero.
- */
void Counter::Reset() {
+ if (StatusIsFatal()) return;
int32_t status = 0;
HAL_ResetCounter(m_counter, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
-/**
- * Get the Period of the most recent count.
- *
- * Returns the time interval of the most recent count. This can be used for
- * velocity calculations to determine shaft speed.
- *
- * @returns The period between the last two pulses in units of seconds.
- */
double Counter::GetPeriod() const {
+ if (StatusIsFatal()) return 0.0;
int32_t status = 0;
double value = HAL_GetCounterPeriod(m_counter, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
-/**
- * Set the maximum period where the device is still considered "moving".
- *
- * Sets the maximum period where the device is considered moving. This value is
- * used to determine the "stopped" state of the counter using the GetStopped
- * method.
- *
- * @param maxPeriod The maximum period where the counted device is considered
- * moving in seconds.
- */
void Counter::SetMaxPeriod(double maxPeriod) {
+ if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetCounterMaxPeriod(m_counter, maxPeriod, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
-/**
- * Select whether you want to continue updating the event timer output when
- * there are no samples captured.
- *
- * The output of the event timer has a buffer of periods that are averaged and
- * posted to a register on the FPGA. When the timer detects that the event
- * source has stopped (based on the MaxPeriod) the buffer of samples to be
- * averaged is emptied. If you enable the update when empty, you will be
- * notified of the stopped source and the event time will report 0 samples.
- * If you disable update when empty, the most recent average will remain on
- * the output until a new sample is acquired. You will never see 0 samples
- * output (except when there have been no events since an FPGA reset) and you
- * will likely not see the stopped bit become true (since it is updated at the
- * end of an average and there are no samples to average).
- *
- * @param enabled True to enable update when empty
- */
void Counter::SetUpdateWhenEmpty(bool enabled) {
+ if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetCounterUpdateWhenEmpty(m_counter, enabled, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
-/**
- * Determine if the clock is stopped.
- *
- * Determine if the clocked input is stopped based on the MaxPeriod value set
- * using the SetMaxPeriod method. If the clock exceeds the MaxPeriod, then the
- * device (and counter) are assumed to be stopped and it returns true.
- *
- * @return Returns true if the most recent counter period exceeds the MaxPeriod
- * value set by SetMaxPeriod.
- */
bool Counter::GetStopped() const {
+ if (StatusIsFatal()) return false;
int32_t status = 0;
bool value = HAL_GetCounterStopped(m_counter, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
-/**
- * The last direction the counter value changed.
- *
- * @return The last direction the counter value changed.
- */
bool Counter::GetDirection() const {
+ if (StatusIsFatal()) return false;
int32_t status = 0;
bool value = HAL_GetCounterDirection(m_counter, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
-
-/**
- * Set the Counter to return reversed sensing on the direction.
- *
- * This allows counters to change the direction they are counting in the case of
- * 1X and 2X quadrature encoding only. Any other counter mode isn't supported.
- *
- * @param reverseDirection true if the value counted should be negated.
- */
-void Counter::SetReverseDirection(bool reverseDirection) {
- int32_t status = 0;
- HAL_SetCounterReverseDirection(m_counter, reverseDirection, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
-}
diff --git a/frc971/wpilib/ahal/Counter.h b/frc971/wpilib/ahal/Counter.h
index 26ae7a2..b2c6ecb 100644
--- a/frc971/wpilib/ahal/Counter.h
+++ b/frc971/wpilib/ahal/Counter.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
+/* Copyright (c) 2008-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. */
@@ -30,6 +30,9 @@
* to be zeroed before use.
*/
class Counter : public CounterBase {
+ friend class DMA;
+ friend class DMASample;
+
public:
enum Mode {
kTwoPulse = 0,
@@ -37,68 +40,395 @@
kPulseLength = 2,
kExternalDirection = 3
};
+
+ /**
+ * Create an instance of a counter where no sources are selected.
+ *
+ * They all must be selected by calling functions to specify the upsource and
+ * the downsource independently.
+ *
+ * This creates a ChipObject counter and initializes status variables
+ * appropriately.
+ *
+ * The counter will start counting immediately.
+ *
+ * @param mode The counter mode
+ */
explicit Counter(Mode mode = kTwoPulse);
+
+ /**
+ * Create an instance of a Counter object.
+ *
+ * Create an up-Counter instance given a channel.
+ *
+ * The counter will start counting immediately.
+ *
+ * @param channel The DIO channel to use as the up source. 0-9 are on-board,
+ * 10-25 are on the MXP
+ */
explicit Counter(int channel);
- explicit Counter(DigitalSource *source);
+
+ /**
+ * Create an instance of a counter from a Digital Source (such as a Digital
+ * Input).
+ *
+ * This is used if an existing digital input is to be shared by multiple other
+ * objects such as encoders or if the Digital Source is not a Digital Input
+ * channel (such as an Analog Trigger).
+ *
+ * The counter will start counting immediately.
+ * @param source A pointer to the existing DigitalSource object. It will be
+ * set as the Up Source.
+ */
+ explicit Counter(DigitalSource* source);
+
+ /**
+ * Create an instance of a counter from a Digital Source (such as a Digital
+ * Input).
+ *
+ * This is used if an existing digital input is to be shared by multiple other
+ * objects such as encoders or if the Digital Source is not a Digital Input
+ * channel (such as an Analog Trigger).
+ *
+ * The counter will start counting immediately.
+ *
+ * @param source A pointer to the existing DigitalSource object. It will be
+ * set as the Up Source.
+ */
explicit Counter(std::shared_ptr<DigitalSource> source);
- explicit Counter(const AnalogTrigger &trigger);
- Counter(EncodingType encodingType, DigitalSource *upSource,
- DigitalSource *downSource, bool inverted);
+
+ /**
+ * Create an instance of a Counter object.
+ *
+ * Create an instance of a simple up-Counter given an analog trigger.
+ * Use the trigger state output from the analog trigger.
+ *
+ * The counter will start counting immediately.
+ *
+ * @param trigger The reference to the existing AnalogTrigger object.
+ */
+ explicit Counter(const AnalogTrigger& trigger);
+
+ /**
+ * Create an instance of a Counter object.
+ *
+ * Creates a full up-down counter given two Digital Sources.
+ *
+ * @param encodingType The quadrature decoding mode (1x or 2x)
+ * @param upSource The pointer to the DigitalSource to set as the up
+ * source
+ * @param downSource The pointer to the DigitalSource to set as the down
+ * source
+ * @param inverted True to invert the output (reverse the direction)
+ */
+ Counter(EncodingType encodingType, DigitalSource* upSource,
+ DigitalSource* downSource, bool inverted);
+
+ /**
+ * Create an instance of a Counter object.
+ *
+ * Creates a full up-down counter given two Digital Sources.
+ *
+ * @param encodingType The quadrature decoding mode (1x or 2x)
+ * @param upSource The pointer to the DigitalSource to set as the up
+ * source
+ * @param downSource The pointer to the DigitalSource to set as the down
+ * source
+ * @param inverted True to invert the output (reverse the direction)
+ */
Counter(EncodingType encodingType, std::shared_ptr<DigitalSource> upSource,
std::shared_ptr<DigitalSource> downSource, bool inverted);
- virtual ~Counter();
+ ~Counter() override;
+
+ Counter(Counter&&) = default;
+ Counter& operator=(Counter&&) = default;
+
+ /**
+ * Set the upsource for the counter as a digital input channel.
+ *
+ * @param channel The DIO channel to use as the up source. 0-9 are on-board,
+ * 10-25 are on the MXP
+ */
void SetUpSource(int channel);
- void SetUpSource(AnalogTrigger *analogTrigger, AnalogTriggerType triggerType);
+
+ /**
+ * Set the up counting source to be an analog trigger.
+ *
+ * @param analogTrigger The analog trigger object that is used for the Up
+ * Source
+ * @param triggerType The analog trigger output that will trigger the
+ * counter.
+ */
+ void SetUpSource(AnalogTrigger* analogTrigger, AnalogTriggerType triggerType);
+
+ /**
+ * Set the up counting source to be an analog trigger.
+ *
+ * @param analogTrigger The analog trigger object that is used for the Up
+ * Source
+ * @param triggerType The analog trigger output that will trigger the
+ * counter.
+ */
void SetUpSource(std::shared_ptr<AnalogTrigger> analogTrigger,
AnalogTriggerType triggerType);
- void SetUpSource(DigitalSource *source);
+
+ void SetUpSource(DigitalSource* source);
+
+ /**
+ * Set the source object that causes the counter to count up.
+ *
+ * Set the up counting DigitalSource.
+ *
+ * @param source Pointer to the DigitalSource object to set as the up source
+ */
void SetUpSource(std::shared_ptr<DigitalSource> source);
- void SetUpSource(DigitalSource &source);
+
+ /**
+ * Set the source object that causes the counter to count up.
+ *
+ * Set the up counting DigitalSource.
+ *
+ * @param source Reference to the DigitalSource object to set as the up source
+ */
+ void SetUpSource(DigitalSource& source);
+
+ /**
+ * Set the edge sensitivity on an up counting source.
+ *
+ * Set the up source to either detect rising edges or falling edges or both.
+ *
+ * @param risingEdge True to trigger on rising edges
+ * @param fallingEdge True to trigger on falling edges
+ */
void SetUpSourceEdge(bool risingEdge, bool fallingEdge);
+
+ /**
+ * Disable the up counting source to the counter.
+ */
void ClearUpSource();
+ /**
+ * Set the down counting source to be a digital input channel.
+ *
+ * @param channel The DIO channel to use as the up source. 0-9 are on-board,
+ * 10-25 are on the MXP
+ */
void SetDownSource(int channel);
- void SetDownSource(AnalogTrigger *analogTrigger,
+
+ /**
+ * Set the down counting source to be an analog trigger.
+ *
+ * @param analogTrigger The analog trigger object that is used for the Down
+ * Source
+ * @param triggerType The analog trigger output that will trigger the
+ * counter.
+ */
+ void SetDownSource(AnalogTrigger* analogTrigger,
AnalogTriggerType triggerType);
+
+ /**
+ * Set the down counting source to be an analog trigger.
+ *
+ * @param analogTrigger The analog trigger object that is used for the Down
+ * Source
+ * @param triggerType The analog trigger output that will trigger the
+ * counter.
+ */
void SetDownSource(std::shared_ptr<AnalogTrigger> analogTrigger,
AnalogTriggerType triggerType);
- void SetDownSource(DigitalSource *source);
+
+ /**
+ * Set the source object that causes the counter to count down.
+ *
+ * Set the down counting DigitalSource.
+ *
+ * @param source Pointer to the DigitalSource object to set as the down source
+ */
+ void SetDownSource(DigitalSource* source);
+
+ /**
+ * Set the source object that causes the counter to count down.
+ *
+ * Set the down counting DigitalSource.
+ *
+ * @param source Reference to the DigitalSource object to set as the down
+ * source
+ */
+ void SetDownSource(DigitalSource& source);
+
void SetDownSource(std::shared_ptr<DigitalSource> source);
- void SetDownSource(DigitalSource &source);
+
+ /**
+ * Set the edge sensitivity on a down counting source.
+ *
+ * Set the down source to either detect rising edges or falling edges.
+ *
+ * @param risingEdge True to trigger on rising edges
+ * @param fallingEdge True to trigger on falling edges
+ */
void SetDownSourceEdge(bool risingEdge, bool fallingEdge);
+
+ /**
+ * Disable the down counting source to the counter.
+ */
void ClearDownSource();
+ /**
+ * Set standard up / down counting mode on this counter.
+ *
+ * Up and down counts are sourced independently from two inputs.
+ */
void SetUpDownCounterMode();
+
+ /**
+ * Set external direction mode on this counter.
+ *
+ * Counts are sourced on the Up counter input.
+ * The Down counter input represents the direction to count.
+ */
void SetExternalDirectionMode();
+
+ /**
+ * Set Semi-period mode on this counter.
+ *
+ * Counts up on both rising and falling edges.
+ */
void SetSemiPeriodMode(bool highSemiPeriod);
+
+ /**
+ * Configure the counter to count in up or down based on the length of the
+ * input pulse.
+ *
+ * This mode is most useful for direction sensitive gear tooth sensors.
+ *
+ * @param threshold The pulse length beyond which the counter counts the
+ * opposite direction. Units are seconds.
+ */
void SetPulseLengthMode(double threshold);
+ /**
+ * Set the Counter to return reversed sensing on the direction.
+ *
+ * This allows counters to change the direction they are counting in the case
+ * of 1X and 2X quadrature encoding only. Any other counter mode isn't
+ * supported.
+ *
+ * @param reverseDirection true if the value counted should be negated.
+ */
void SetReverseDirection(bool reverseDirection);
- // CounterBase interface
- int Get() const override;
- void Reset() override;
- double GetPeriod() const override;
- void SetMaxPeriod(double maxPeriod) override;
- void SetUpdateWhenEmpty(bool enabled);
- bool GetStopped() const override;
- bool GetDirection() const override;
-
+ /**
+ * Set the Samples to Average which specifies the number of samples of the
+ * timer to average when calculating the period. Perform averaging to account
+ * for mechanical imperfections or as oversampling to increase resolution.
+ *
+ * @param samplesToAverage The number of samples to average from 1 to 127.
+ */
void SetSamplesToAverage(int samplesToAverage);
+
+ /**
+ * Get the Samples to Average which specifies the number of samples of the
+ * timer to average when calculating the period.
+ *
+ * Perform averaging to account for mechanical imperfections or as
+ * oversampling to increase resolution.
+ *
+ * @return The number of samples being averaged (from 1 to 127)
+ */
int GetSamplesToAverage() const;
- int GetFPGAIndex() const { return m_index; }
+
+ int GetFPGAIndex() const;
+
+ // CounterBase interface
+ /**
+ * Read the current counter value.
+ *
+ * Read the value at this instant. It may still be running, so it reflects the
+ * current value. Next time it is read, it might have a different value.
+ */
+ int Get() const override;
+
+ /**
+ * Reset the Counter to zero.
+ *
+ * Set the counter value to zero. This doesn't effect the running state of the
+ * counter, just sets the current value to zero.
+ */
+ void Reset() override;
+
+ /**
+ * Get the Period of the most recent count.
+ *
+ * Returns the time interval of the most recent count. This can be used for
+ * velocity calculations to determine shaft speed.
+ *
+ * @returns The period between the last two pulses in units of seconds.
+ */
+ double GetPeriod() const override;
+
+ /**
+ * Set the maximum period where the device is still considered "moving".
+ *
+ * Sets the maximum period where the device is considered moving. This value
+ * is used to determine the "stopped" state of the counter using the
+ * GetStopped method.
+ *
+ * @param maxPeriod The maximum period where the counted device is considered
+ * moving in seconds.
+ */
+ void SetMaxPeriod(double maxPeriod) override;
+
+ /**
+ * Select whether you want to continue updating the event timer output when
+ * there are no samples captured.
+ *
+ * The output of the event timer has a buffer of periods that are averaged and
+ * posted to a register on the FPGA. When the timer detects that the event
+ * source has stopped (based on the MaxPeriod) the buffer of samples to be
+ * averaged is emptied. If you enable the update when empty, you will be
+ * notified of the stopped source and the event time will report 0 samples.
+ * If you disable update when empty, the most recent average will remain on
+ * the output until a new sample is acquired. You will never see 0 samples
+ * output (except when there have been no events since an FPGA reset) and you
+ * will likely not see the stopped bit become true (since it is updated at the
+ * end of an average and there are no samples to average).
+ *
+ * @param enabled True to enable update when empty
+ */
+ void SetUpdateWhenEmpty(bool enabled);
+
+ /**
+ * Determine if the clock is stopped.
+ *
+ * Determine if the clocked input is stopped based on the MaxPeriod value set
+ * using the SetMaxPeriod method. If the clock exceeds the MaxPeriod, then the
+ * device (and counter) are assumed to be stopped and it returns true.
+ *
+ * @return Returns true if the most recent counter period exceeds the
+ * MaxPeriod value set by SetMaxPeriod.
+ */
+ bool GetStopped() const override;
+
+ /**
+ * The last direction the counter value changed.
+ *
+ * @return The last direction the counter value changed.
+ */
+ bool GetDirection() const override;
protected:
// Makes the counter count up.
std::shared_ptr<DigitalSource> m_upSource;
+
// Makes the counter count down.
std::shared_ptr<DigitalSource> m_downSource;
+
// The FPGA counter object
- HAL_CounterHandle m_counter = HAL_kInvalidHandle;
+ hal::Handle<HAL_CounterHandle> m_counter;
private:
- int m_index = 0; ///< The index of this counter.
+ int m_index = 0; // The index of this counter.
friend class DigitalGlitchFilter;
};
diff --git a/frc971/wpilib/ahal/DutyCycle.cc b/frc971/wpilib/ahal/DutyCycle.cc
new file mode 100644
index 0000000..13ccaa0
--- /dev/null
+++ b/frc971/wpilib/ahal/DutyCycle.cc
@@ -0,0 +1,92 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "frc971/wpilib/ahal/DutyCycle.h"
+
+#include <hal/DutyCycle.h>
+#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
+
+#include "frc971/wpilib/ahal/Base.h"
+#include "frc971/wpilib/ahal/DigitalSource.h"
+#include "frc971/wpilib/ahal/WPIErrors.h"
+
+using namespace frc;
+
+DutyCycle::DutyCycle(DigitalSource* source)
+ : m_source{source, NullDeleter<DigitalSource>()} {
+ if (m_source == nullptr) {
+ wpi_setWPIError(NullParameter);
+ } else {
+ InitDutyCycle();
+ }
+}
+
+DutyCycle::DutyCycle(DigitalSource& source)
+ : m_source{&source, NullDeleter<DigitalSource>()} {
+ InitDutyCycle();
+}
+
+DutyCycle::DutyCycle(std::shared_ptr<DigitalSource> source)
+ : m_source{std::move(source)} {
+ if (m_source == nullptr) {
+ wpi_setWPIError(NullParameter);
+ } else {
+ InitDutyCycle();
+ }
+}
+
+DutyCycle::~DutyCycle() { HAL_FreeDutyCycle(m_handle); }
+
+void DutyCycle::InitDutyCycle() {
+ int32_t status = 0;
+ m_handle =
+ HAL_InitializeDutyCycle(m_source->GetPortHandleForRouting(),
+ static_cast<HAL_AnalogTriggerType>(
+ m_source->GetAnalogTriggerTypeForRouting()),
+ &status);
+ wpi_setHALError(status);
+ int index = GetFPGAIndex();
+ HAL_Report(HALUsageReporting::kResourceType_DutyCycle, index + 1);
+}
+
+int DutyCycle::GetFPGAIndex() const {
+ int32_t status = 0;
+ auto retVal = HAL_GetDutyCycleFPGAIndex(m_handle, &status);
+ wpi_setHALError(status);
+ return retVal;
+}
+
+int DutyCycle::GetFrequency() const {
+ int32_t status = 0;
+ auto retVal = HAL_GetDutyCycleFrequency(m_handle, &status);
+ wpi_setHALError(status);
+ return retVal;
+}
+
+double DutyCycle::GetOutput() const {
+ int32_t status = 0;
+ auto retVal = HAL_GetDutyCycleOutput(m_handle, &status);
+ wpi_setHALError(status);
+ return retVal;
+}
+
+unsigned int DutyCycle::GetOutputRaw() const {
+ int32_t status = 0;
+ auto retVal = HAL_GetDutyCycleOutputRaw(m_handle, &status);
+ wpi_setHALError(status);
+ return retVal;
+}
+
+unsigned int DutyCycle::GetOutputScaleFactor() const {
+ int32_t status = 0;
+ auto retVal = HAL_GetDutyCycleOutputScaleFactor(m_handle, &status);
+ wpi_setHALError(status);
+ return retVal;
+}
+
+int DutyCycle::GetSourceChannel() const { return m_source->GetChannel(); }
diff --git a/frc971/wpilib/ahal/DutyCycle.h b/frc971/wpilib/ahal/DutyCycle.h
new file mode 100644
index 0000000..cfc9256
--- /dev/null
+++ b/frc971/wpilib/ahal/DutyCycle.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 <memory>
+
+#include <hal/Types.h>
+
+namespace frc {
+class DigitalSource;
+class AnalogTrigger;
+class DMA;
+class DMASample;
+
+/**
+ * Class to read a duty cycle PWM input.
+ *
+ * <p>PWM input signals are specified with a frequency and a ratio of high to
+ * low in that frequency. There are 8 of these in the roboRIO, and they can be
+ * attached to any DigitalSource.
+ *
+ * <p>These can be combined as the input of an AnalogTrigger to a Counter in
+ * order to implement rollover checking.
+ *
+ */
+class DutyCycle {
+ friend class AnalogTrigger;
+ friend class DMA;
+ friend class DMASample;
+
+ public:
+ /**
+ * Constructs a DutyCycle input from a DigitalSource input.
+ *
+ * <p> This class does not own the inputted source.
+ *
+ * @param source The DigitalSource to use.
+ */
+ explicit DutyCycle(DigitalSource& source);
+ /**
+ * Constructs a DutyCycle input from a DigitalSource input.
+ *
+ * <p> This class does not own the inputted source.
+ *
+ * @param source The DigitalSource to use.
+ */
+ explicit DutyCycle(DigitalSource* source);
+ /**
+ * Constructs a DutyCycle input from a DigitalSource input.
+ *
+ * <p> This class does not own the inputted source.
+ *
+ * @param source The DigitalSource to use.
+ */
+ explicit DutyCycle(std::shared_ptr<DigitalSource> source);
+
+ /**
+ * Close the DutyCycle and free all resources.
+ */
+ virtual ~DutyCycle();
+
+ DutyCycle(DutyCycle&&) = default;
+ DutyCycle& operator=(DutyCycle&&) = default;
+
+ /**
+ * Get the frequency of the duty cycle signal.
+ *
+ * @return frequency in Hertz
+ */
+ int GetFrequency() const;
+
+ /**
+ * Get the output ratio of the duty cycle signal.
+ *
+ * <p> 0 means always low, 1 means always high.
+ *
+ * @return output ratio between 0 and 1
+ */
+ double GetOutput() const;
+
+ /**
+ * Get the raw output ratio of the duty cycle signal.
+ *
+ * <p> 0 means always low, an output equal to
+ * GetOutputScaleFactor() means always high.
+ *
+ * @return output ratio in raw units
+ */
+ unsigned int GetOutputRaw() const;
+
+ /**
+ * Get the scale factor of the output.
+ *
+ * <p> An output equal to this value is always high, and then linearly scales
+ * down to 0. Divide the result of getOutputRaw by this in order to get the
+ * percentage between 0 and 1.
+ *
+ * @return the output scale factor
+ */
+ unsigned int GetOutputScaleFactor() const;
+
+ /**
+ * Get the FPGA index for the DutyCycle.
+ *
+ * @return the FPGA index
+ */
+ int GetFPGAIndex() const;
+
+ /**
+ * Get the channel of the source.
+ *
+ * @return the source channel
+ */
+ int GetSourceChannel() const;
+
+ private:
+ void InitDutyCycle();
+ std::shared_ptr<DigitalSource> m_source;
+ hal::Handle<HAL_DutyCycleHandle> m_handle;
+};
+} // namespace frc
diff --git a/frc971/wpilib/ahal/ErrorBase.h b/frc971/wpilib/ahal/ErrorBase.h
index 8228df1..c92ca52 100644
--- a/frc971/wpilib/ahal/ErrorBase.h
+++ b/frc971/wpilib/ahal/ErrorBase.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
+/* Copyright (c) 2008-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. */
@@ -7,42 +7,63 @@
#pragma once
-#include "frc971/wpilib/ahal/Base.h"
-#include "wpi/StringRef.h"
+#include <vector>
+
+#include <wpi/StringRef.h>
+#include <wpi/Twine.h>
+#include <wpi/mutex.h>
+
+// Forward declared manually to avoid needing to pull in entire HAL header.
+extern "C" const char* HAL_GetErrorMessage(int32_t code);
#define wpi_setErrnoErrorWithContext(context) \
this->SetErrnoError((context), __FILE__, __FUNCTION__, __LINE__)
#define wpi_setErrnoError() wpi_setErrnoErrorWithContext("")
-#define wpi_setImaqErrorWithContext(code, context) \
- do { \
+#define wpi_setImaqErrorWithContext(code, context) \
+ do { \
} while (0)
-#define wpi_setErrorWithContext(code, context) \
- do { \
+#define wpi_setErrorWithContext(code, context) \
+ do { \
} while (0)
-#define wpi_setErrorWithContextRange(code, min, max, req, context) \
- do { \
+#define wpi_setErrorWithContextRange(code, min, max, req, context) \
+ do { \
} while (0)
+
+#define wpi_setHALError(code) \
+ do { \
+ } while (0)
+
+#define wpi_setHALErrorWithRange(code, min, max, req) \
+ do { \
+ } while (0)
+
#define wpi_setError(code) wpi_setErrorWithContext(code, "")
-#define wpi_setStaticErrorWithContext(object, code, context) \
- do { \
+#define wpi_setStaticErrorWithContext(object, code, context) \
+ do { \
} while (0)
#define wpi_setStaticError(object, code) \
wpi_setStaticErrorWithContext(object, code, "")
-#define wpi_setGlobalErrorWithContext(code, context) \
- do { \
+
+#define wpi_setGlobalErrorWithContext(code, context) \
+ do { \
} while (0)
+
+#define wpi_setGlobalHALError(code) \
+ do { \
+ } while (0)
+
#define wpi_setGlobalError(code) wpi_setGlobalErrorWithContext(code, "")
#define wpi_setWPIErrorWithContext(error, context) \
do { \
} while (0)
#define wpi_setWPIError(error) wpi_setWPIErrorWithContext(error, "")
#define wpi_setStaticWPIErrorWithContext(object, error, context) \
- object->SetWPIError((wpi_error_s_##error), (context), __FILE__, \
+ object->SetWPIError(wpi_error_s_##error(), (context), __FILE__, \
__FUNCTION__, __LINE__)
#define wpi_setStaticWPIError(object, error) \
wpi_setStaticWPIErrorWithContext(object, error, "")
#define wpi_setGlobalWPIErrorWithContext(error, context) \
- ::frc::ErrorBase::SetGlobalWPIError((wpi_error_s_##error), (context), \
+ ::frc::ErrorBase::SetGlobalWPIError(wpi_error_s_##error(), (context), \
__FILE__, __FUNCTION__, __LINE__)
#define wpi_setGlobalWPIError(error) wpi_setGlobalWPIErrorWithContext(error, "")
diff --git a/frc971/wpilib/ahal/InterruptableSensorBase.cc b/frc971/wpilib/ahal/InterruptableSensorBase.cc
index 4becafc..d17f2dd 100644
--- a/frc971/wpilib/ahal/InterruptableSensorBase.cc
+++ b/frc971/wpilib/ahal/InterruptableSensorBase.cc
@@ -13,42 +13,6 @@
using namespace frc;
-namespace {
-
-// Converts a freestanding lower half to a 64 bit FPGA timestamp
-//
-// Note: This is making the assumption that the timestamp being converted is
-// always in the past. If you call this with a future timestamp, it probably
-// will make it in the past. If you wait over 70 minutes between capturing the
-// bottom 32 bits of the timestamp and expanding it, you will be off by
-// multiples of 1<<32 microseconds.
-//
-// @return The current time in microseconds according to the FPGA (since FPGA
-// reset) as a 64 bit number.
-uint64_t HAL_ExpandFPGATime(uint32_t unexpanded_lower, int32_t* status) {
- // Capture the current FPGA time. This will give us the upper half of the
- // clock.
- uint64_t fpga_time = HAL_GetFPGATime(status);
- if (*status != 0) return 0;
-
- // Now, we need to detect the case where the lower bits rolled over after we
- // sampled. In that case, the upper bits will be 1 bigger than they should
- // be.
-
- // Break it into lower and upper portions.
- uint32_t lower = fpga_time & ((uint64_t)0xffffffff);
- uint64_t upper = (fpga_time >> 32) & 0xffffffff;
-
- // The time was sampled *before* the current time, so roll it back.
- if (lower < unexpanded_lower) {
- --upper;
- }
-
- return (upper << 32) + static_cast<uint64_t>(unexpanded_lower);
-}
-
-} // namespace
-
InterruptableSensorBase::InterruptableSensorBase() {}
void InterruptableSensorBase::RequestInterrupts() {