add the dma interface code
This is mostly copied from what Austin wrote.
Change-Id: I06a5f2323ec2d39ca6f1a3eaac6f4d129de84a3c
diff --git a/aos/build/externals.gyp b/aos/build/externals.gyp
index d4bce22..35d271d 100644
--- a/aos/build/externals.gyp
+++ b/aos/build/externals.gyp
@@ -18,6 +18,7 @@
'stm32flash_commit': '8399fbe1baf2b7d097746786458021d92895d71b',
'allwpilib': '<(AOS)/externals/allwpilib',
+ 'forwpilib': '<(AOS)/externals/forwpilib',
},
'targets': [
{
@@ -25,10 +26,12 @@
'type': 'static_library',
'variables': {
'header_dirs': [
+ '<(forwpilib)',
'<(allwpilib)/wpilibc/wpilibC++/include',
'<(allwpilib)/wpilibc/wpilibC++Devices/include',
'<(allwpilib)/hal/include',
'<(allwpilib)/hal/lib/Athena/FRC_FPGA_ChipObject',
+ '<(allwpilib)/hal/lib/Athena',
],
},
'include_dirs': [
@@ -44,6 +47,7 @@
'<!@(ls <(allwpilib)/wpilibc/wpilibC++Devices/src/Internal/*.cpp)',
'<!@(ls <(allwpilib)/hal/lib/Athena/*.cpp)',
'<!@(ls <(allwpilib)/hal/lib/Athena/ctre/*.cpp)',
+ '<(forwpilib)/dma.cc',
],
'link_settings': {
'library_dirs': [
diff --git a/aos/externals/forwpilib/README b/aos/externals/forwpilib/README
new file mode 100644
index 0000000..d7ecb6c
--- /dev/null
+++ b/aos/externals/forwpilib/README
@@ -0,0 +1,2 @@
+This directory contains files that we plan to eventually push to upstream
+WPILib but haven't yet.
diff --git a/aos/externals/forwpilib/dma.cc b/aos/externals/forwpilib/dma.cc
new file mode 100644
index 0000000..279929f
--- /dev/null
+++ b/aos/externals/forwpilib/dma.cc
@@ -0,0 +1,348 @@
+#include "dma.h"
+
+#include <algorithm>
+
+// Like tEncoder::tOutput with the bitfields reversed.
+typedef union {
+ struct {
+ unsigned Direction: 1;
+ signed Value: 31;
+ };
+ struct {
+ unsigned value: 32;
+ };
+} t1Output;
+
+static const uint32_t kNumHeaders = 10;
+
+static constexpr ssize_t kChannelSize[18] = {2, 2, 4, 4, 2, 2, 4, 4, 3,
+ 3, 2, 1, 4, 4, 4, 4, 4, 4};
+
+enum DMAOffsetConstants {
+ kEnable_AI0_Low = 0,
+ kEnable_AI0_High = 1,
+ kEnable_AIAveraged0_Low = 2,
+ kEnable_AIAveraged0_High = 3,
+ kEnable_AI1_Low = 4,
+ kEnable_AI1_High = 5,
+ kEnable_AIAveraged1_Low = 6,
+ kEnable_AIAveraged1_High = 7,
+ kEnable_Accumulator0 = 8,
+ kEnable_Accumulator1 = 9,
+ kEnable_DI = 10,
+ kEnable_AnalogTriggers = 11,
+ kEnable_Counters_Low = 12,
+ kEnable_Counters_High = 13,
+ kEnable_CounterTimers_Low = 14,
+ kEnable_CounterTimers_High = 15,
+ kEnable_Encoders = 16,
+ kEnable_EncoderTimers = 17,
+};
+
+DMA::DMA() {
+ tRioStatusCode status = 0;
+ tdma_config_ = tDMA::create(&status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ if (status != 0) {
+ return;
+ }
+ SetRate(1);
+ SetPause(false);
+}
+
+DMA::~DMA() {
+ tRioStatusCode status = 0;
+
+ manager_->stop(&status);
+ delete tdma_config_;
+}
+
+void DMA::SetPause(bool pause) {
+ tRioStatusCode status = 0;
+ tdma_config_->writeConfig_Pause(pause, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+void DMA::SetRate(uint32_t cycles) {
+ if (cycles < 1) {
+ cycles = 1;
+ }
+ tRioStatusCode status = 0;
+ tdma_config_->writeRate(cycles, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+void DMA::Add(Encoder * /*encoder*/) {
+ tRioStatusCode status = 0;
+
+ if (manager_) {
+ wpi_setErrorWithContext(NiFpga_Status_InvalidParameter,
+ "DMA::Add() only works before DMA::Start()");
+ return;
+ }
+
+ fprintf(stderr, "DMA::Add(Encoder*) needs re-testing. aborting\n");
+ abort();
+
+ // TODO(austin): Encoder uses a Counter for 1x or 2x; quad for 4x...
+ tdma_config_->writeConfig_Enable_Encoders(true, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+void DMA::Add(DigitalSource * /*input*/) {
+ tRioStatusCode status = 0;
+
+ if (manager_) {
+ wpi_setErrorWithContext(NiFpga_Status_InvalidParameter,
+ "DMA::Add() only works before DMA::Start()");
+ return;
+ }
+
+ tdma_config_->writeConfig_Enable_DI(true, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+void DMA::SetExternalTrigger(DigitalSource *input, bool rising, bool falling) {
+ tRioStatusCode status = 0;
+
+ if (manager_) {
+ wpi_setErrorWithContext(NiFpga_Status_InvalidParameter,
+ "DMA::SetExternalTrigger() only works before DMA::Start()");
+ return;
+ }
+
+ auto index =
+ ::std::find(trigger_channels_.begin(), trigger_channels_.end(), false);
+ if (index == trigger_channels_.end()) {
+ wpi_setErrorWithContext(NiFpga_Status_InvalidParameter,
+ "DMA: No channels left");
+ return;
+ }
+ *index = true;
+
+ const int channel_index = index - trigger_channels_.begin();
+
+ tdma_config_->writeConfig_ExternalClock(true, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ if (status != 0) {
+ return;
+ }
+
+ // Configures the trigger to be external, not off the FPGA clock.
+ tdma_config_->writeExternalTriggers_ExternalClockSource_Channel(
+ channel_index, input->GetChannelForRouting(), &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ if (status != 0) {
+ return;
+ }
+
+ tdma_config_->writeExternalTriggers_ExternalClockSource_Module(
+ channel_index, input->GetModuleForRouting(), &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ if (status != 0) {
+ return;
+ }
+ tdma_config_->writeExternalTriggers_ExternalClockSource_AnalogTrigger(
+ channel_index, input->GetAnalogTriggerForRouting(), &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ if (status != 0) {
+ return;
+ }
+ tdma_config_->writeExternalTriggers_RisingEdge(channel_index, rising,
+ &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ if (status != 0) {
+ return;
+ }
+ tdma_config_->writeExternalTriggers_FallingEdge(channel_index, falling,
+ &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ if (status != 0) {
+ return;
+ }
+}
+
+DMA::ReadStatus DMA::Read(DMASample *sample, uint32_t timeout_ms,
+ size_t *remaining) {
+ tRioStatusCode status = 0;
+ size_t remainingBytes = 0;
+ *remaining = 0;
+
+ if (!manager_.get()) {
+ wpi_setErrorWithContext(NiFpga_Status_InvalidParameter,
+ "DMA::Read() only works after DMA::Start()");
+ return STATUS_ERROR;
+ }
+
+ // memset(&sample->read_buffer_, 0, sizeof(read_buffer_));
+ manager_->read(sample->read_buffer_, capture_size_, timeout_ms,
+ &remainingBytes, &status);
+
+ if (0) { // DEBUG
+ printf("buf[] = ");
+ for (size_t i = 0;
+ i < sizeof(sample->read_buffer_) / sizeof(sample->read_buffer_[0]);
+ ++i) {
+ if (i != 0) {
+ printf(" ");
+ }
+ printf("0x%.8x", sample->read_buffer_[i]);
+ }
+ printf("\n");
+ }
+
+ // TODO(jerry): Do this only if status == 0?
+ *remaining = remainingBytes / capture_size_;
+ sample->dma_ = this;
+
+ if (0) { // DEBUG
+ printf("Remaining samples = %d\n", *remaining);
+ }
+
+ // TODO(austin): Check that *remainingBytes % capture_size_ == 0 and deal
+ // with it if it isn't. Probably meant that we overflowed?
+ if (status == 0) {
+ return STATUS_OK;
+ } else if (status == NiFpga_Status_FifoTimeout) {
+ return STATUS_TIMEOUT;
+ } else {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return STATUS_ERROR;
+ }
+}
+
+void DMA::Start(size_t queue_depth) {
+ tRioStatusCode status = 0;
+ tconfig_ = tdma_config_->readConfig(&status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ if (status != 0) {
+ return;
+ }
+
+ {
+ size_t accum_size = 0;
+#define SET_SIZE(bit) \
+ if (tconfig_.bit) { \
+ channel_offsets_[k##bit] = accum_size; \
+ accum_size += kChannelSize[k##bit]; \
+ } else { \
+ channel_offsets_[k##bit] = -1; \
+ }
+
+ SET_SIZE(Enable_AI0_Low);
+ SET_SIZE(Enable_AI0_High);
+ SET_SIZE(Enable_AIAveraged0_Low);
+ SET_SIZE(Enable_AIAveraged0_High);
+ SET_SIZE(Enable_AI1_Low);
+ SET_SIZE(Enable_AI1_High);
+ SET_SIZE(Enable_AIAveraged1_Low);
+ SET_SIZE(Enable_AIAveraged1_High);
+ SET_SIZE(Enable_Accumulator0);
+ SET_SIZE(Enable_Accumulator1);
+ SET_SIZE(Enable_DI);
+ SET_SIZE(Enable_AnalogTriggers);
+ SET_SIZE(Enable_Counters_Low);
+ SET_SIZE(Enable_Counters_High);
+ SET_SIZE(Enable_CounterTimers_Low);
+ SET_SIZE(Enable_CounterTimers_High);
+ SET_SIZE(Enable_Encoders);
+ SET_SIZE(Enable_EncoderTimers);
+#undef SET_SIZE
+ capture_size_ = accum_size + 1;
+ }
+
+ manager_.reset(new nFPGA::tDMAManager(0, queue_depth * capture_size_, &status));
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ if (status != 0) {
+ return;
+ }
+ // Start, stop, start to clear the buffer.
+ manager_->start(&status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ if (status != 0) {
+ return;
+ }
+ manager_->stop(&status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ if (status != 0) {
+ return;
+ }
+ manager_->start(&status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ if (status != 0) {
+ return;
+ }
+}
+
+ssize_t DMASample::offset(int index) const { return dma_->channel_offsets_[index]; }
+
+double DMASample::GetTimestamp() const {
+ return static_cast<double>(read_buffer_[dma_->capture_size_ - 1]) * 0.000001;
+}
+
+bool DMASample::Get(DigitalSource *input) const {
+ if (offset(kEnable_DI) == -1) {
+ wpi_setStaticErrorWithContext(dma_,
+ NiFpga_Status_ResourceNotFound,
+ getHALErrorMessage(NiFpga_Status_ResourceNotFound));
+ return false;
+ }
+ if (input->GetChannelForRouting() < kNumHeaders) {
+ return (read_buffer_[offset(kEnable_DI)] >>
+ input->GetChannelForRouting()) &
+ 0x1;
+ } else {
+ return (read_buffer_[offset(kEnable_DI)] >>
+ (input->GetChannelForRouting() + 6)) &
+ 0x1;
+ }
+}
+
+int32_t DMASample::GetRaw(Encoder *input) const {
+ if (offset(kEnable_Encoders) == -1) {
+ wpi_setStaticErrorWithContext(dma_,
+ NiFpga_Status_ResourceNotFound,
+ getHALErrorMessage(NiFpga_Status_ResourceNotFound));
+ return -1;
+ }
+
+ uint32_t dmaWord =
+ read_buffer_[offset(kEnable_Encoders) + input->GetFPGAIndex()];
+ int32_t result = 0;
+
+ if (1) {
+ // Extract the 31-bit signed tEncoder::tOutput Value using a struct with the
+ // reverse packed field order of tOutput. This gets Value from the high
+ // order 31 bits of output on little-endian ARM using gcc. This works
+ // even though C/C++ doesn't guarantee bitfield order.
+ t1Output output;
+
+ output.value = dmaWord;
+ result = output.Value;
+ } else if (1) {
+ // Extract the 31-bit signed tEncoder::tOutput Value using right-shift.
+ // This works even though C/C++ doesn't guarantee whether signed >> does
+ // arithmetic or logical shift. (dmaWord / 2) is not a great alternative
+ // since it rounds.
+ result = static_cast<int32_t>(dmaWord) >> 1;
+ }
+#if 0 // This approach was recommended but it doesn't return the right value.
+ else {
+ // Byte-reverse the DMA word (big-endian value from the FPGA) then extract
+ // the 31-bit tEncoder::tOutput. This does not return the right Value.
+ tEncoder::tOutput encoderData;
+
+ encoderData.value = __builtin_bswap32(dmaWord);
+ result = encoderData.Value;
+ }
+#endif
+
+ return result;
+}
+
+int32_t DMASample::Get(Encoder *input) const {
+ int32_t raw = GetRaw(input);
+
+ // TODO(austin): Really bad... DecodingScaleFactor?
+ return raw / 4.0;
+}
diff --git a/aos/externals/forwpilib/dma.h b/aos/externals/forwpilib/dma.h
new file mode 100644
index 0000000..0baf473
--- /dev/null
+++ b/aos/externals/forwpilib/dma.h
@@ -0,0 +1,101 @@
+#ifndef _DMA_H_
+#define _DMA_H_
+
+#include <stdint.h>
+
+#include <array>
+#include <memory>
+
+#include "ChipObject.h"
+#include "DigitalSource.h"
+#include "Encoder.h"
+
+class DMA;
+
+class DMASample {
+ public:
+ DMASample() {}
+
+ // Returns the FPGA timestamp of the sample.
+ double GetTimestamp() const;
+
+ // All Get methods either return the requested value, or set the Error.
+
+ // Returns the value of the digital input in the sample.
+ bool Get(DigitalSource *input) const;
+ // Returns the raw value of the encoder in the sample.
+ int32_t GetRaw(Encoder *input) const;
+ // Returns the {1, 2, or 4} X scaled value of the encoder in the sample.
+ int32_t Get(Encoder *input) const;
+
+ private:
+ friend DMA;
+
+ // Returns the offset of the sample type in the buffer, or -1 if it isn't in
+ // the sample.
+ ssize_t offset(int index) const;
+
+ // TODO(austin): This should be re-used from WPILib... Once I merge this back
+ // into WPILib.
+
+ DMA *dma_;
+ uint32_t read_buffer_[64];
+};
+
+class DMA : public ErrorBase {
+ public:
+ DMA();
+ virtual ~DMA();
+
+ // Sets whether or not DMA is paused.
+ void SetPause(bool pause);
+
+ // Sets the number of triggers that need to occur before a sample is saved.
+ void SetRate(uint32_t cycles);
+
+ // Adds the input signal to the state to snapshot on the trigger event.
+ // Call Add() and SetExternalTrigger() before Start().
+ void Add(Encoder *encoder);
+ void Add(DigitalSource *input);
+
+ // Configures DMA to trigger on an external trigger. There can only be 4
+ // external triggers.
+ // Call Add() and SetExternalTrigger() before Start().
+ void SetExternalTrigger(DigitalSource *input, bool rising, bool falling);
+
+ // Starts reading samples into the buffer. Clears all previous samples before
+ // starting.
+ // Call Start() before Read().
+ void Start(size_t queue_depth);
+
+ enum ReadStatus {
+ STATUS_OK = 0,
+ STATUS_TIMEOUT = 1,
+ STATUS_ERROR = 2,
+ };
+
+ // Reads a sample from the DMA buffer, waiting up to timeout_ms for it.
+ // Returns a status code indicating whether the read worked, timed out, or
+ // failed.
+ // Call Add() and SetExternalTrigger() then Start() before Read().
+ // The sample is only usable while this DMA object is left started.
+ ReadStatus Read(DMASample *sample, uint32_t timeout_ms, size_t *remaining);
+
+ private:
+ ::std::unique_ptr<nFPGA::tDMAManager> manager_; // set by Start()
+ typedef nFPGA::nRoboRIO_FPGANamespace::tDMA tDMA;
+ friend DMASample;
+
+ // The offsets into the sample structure for each DMA type, or -1 if it isn't
+ // in the set of values.
+ ssize_t channel_offsets_[18];
+
+ // The size of the data to read to get a sample.
+ size_t capture_size_ = 0;
+ tDMA::tConfig tconfig_;
+ tDMA *tdma_config_;
+
+ ::std::array<bool, 4> trigger_channels_ = {{false, false, false, false}};
+};
+
+#endif // _DMA_H_
diff --git a/aos/externals/forwpilib/dma_test.cc b/aos/externals/forwpilib/dma_test.cc
new file mode 100644
index 0000000..7ef7ab1
--- /dev/null
+++ b/aos/externals/forwpilib/dma_test.cc
@@ -0,0 +1,222 @@
+#include <memory>
+#include <thread>
+#define __STDC_FORMAT_MACROS
+#include <inttypes.h>
+#include <atomic>
+#include <mutex>
+#include <sched.h>
+#include <assert.h>
+#include <WPILib.h>
+#include "dma.h"
+#include <signal.h>
+
+::std::atomic<double> last_time;
+
+class priority_mutex {
+ public:
+ typedef pthread_mutex_t *native_handle_type;
+
+ // TODO(austin): Write a test case for the mutex, and make the constructor
+ // constexpr.
+ priority_mutex() {
+ pthread_mutexattr_t attr;
+ // Turn on priority inheritance.
+ assert_perror(pthread_mutexattr_init(&attr));
+ assert_perror(pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_NORMAL));
+ assert_perror(pthread_mutexattr_setprotocol(&attr, PTHREAD_PRIO_INHERIT));
+
+ assert_perror(pthread_mutex_init(native_handle(), &attr));
+
+ assert_perror(pthread_mutexattr_destroy(&attr));
+ }
+
+ ~priority_mutex() { pthread_mutex_destroy(&handle_); }
+
+ void lock() { assert_perror(pthread_mutex_lock(&handle_)); }
+ bool try_lock() {
+ int ret = pthread_mutex_trylock(&handle_);
+ if (ret == 0) {
+ return true;
+ } else if (ret == EBUSY) {
+ return false;
+ } else {
+ assert_perror(ret);
+ }
+ }
+ void unlock() { assert_perror(pthread_mutex_unlock(&handle_)); }
+
+ native_handle_type native_handle() { return &handle_; }
+
+ private:
+ DISALLOW_COPY_AND_ASSIGN(priority_mutex);
+ pthread_mutex_t handle_;
+};
+
+class EdgePrinter {
+ public:
+ EdgePrinter(DigitalInput *sensor)
+ : quit_(false),
+ sensor_(sensor),
+ interrupt_count_(0) {
+ }
+
+ void Start() {
+ printf("Creating thread %d\n", sensor_->GetChannel());
+ thread_.reset(new ::std::thread(::std::ref(*this)));
+ }
+
+ void operator ()() {
+ struct sched_param param;
+ param.sched_priority = 55;
+ if (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) {
+ perror("sched_setscheduler failed");
+ exit(-1);
+ }
+
+ printf("Started thread %d\n", sensor_->GetChannel());
+
+ sensor_->RequestInterrupts();
+ sensor_->SetUpSourceEdge(true, false);
+
+ InterruptableSensorBase::WaitResult result = InterruptableSensorBase::kBoth;
+ while (!quit_) {
+ result = sensor_->WaitForInterrupt(
+ 0.1, result != InterruptableSensorBase::kTimeout);
+ if (result != InterruptableSensorBase::kTimeout) {
+ ++interrupt_count_;
+ printf("Got %d edges on %d\n", interrupt_count_.load(),
+ sensor_->GetChannel());
+ }
+ }
+ }
+
+ int interrupt_count() const { return interrupt_count_; }
+
+ void quit() {
+ quit_ = true;
+ thread_->join();
+ }
+
+ DigitalInput *sensor() { return sensor_.get(); }
+
+ private:
+ ::std::atomic<bool> quit_;
+ ::std::unique_ptr<DigitalInput> sensor_;
+ ::std::atomic<int> interrupt_count_;
+ ::std::unique_ptr<::std::thread> thread_;
+};
+
+class TestRobot;
+static TestRobot *my_robot;
+
+class TestRobot : public RobotBase {
+ public:
+ static void HandleSigIntStatic(int signal) { my_robot->HandleSigInt(signal); }
+ void HandleSigInt(int /*signal*/) { quit_ = true; }
+
+ ::std::unique_ptr<Encoder> MakeEncoder(int index) {
+ return ::std::unique_ptr<Encoder>(
+ new Encoder(sensor(10 + 2 * index), sensor(11 + 2 * index)));
+ }
+
+ ::std::vector<::std::unique_ptr<EdgePrinter>> printers;
+ ::std::vector<::std::unique_ptr<DigitalInput>> dio;
+
+ DigitalInput *sensor(int i) {
+ if (i < 8) {
+ return printers[i]->sensor();
+ } else {
+ return dio[i - 8].get();
+ }
+ }
+
+ void AllEdgeTests() {
+ my_robot = this;
+ quit_ = false;
+ struct sigaction sa;
+
+ memset(&sa, 0, sizeof(sa));
+ // Setup the sighub handler
+ sa.sa_handler = &TestRobot::HandleSigIntStatic;
+
+ // Restart the system call, if at all possible
+ sa.sa_flags = SA_RESTART;
+
+ // Block every signal during the handler
+ sigfillset(&sa.sa_mask);
+
+ for (int i = 0; i < 8; ++i) {
+ printers.emplace_back(new EdgePrinter(new DigitalInput(i)));
+ }
+ printf("Created all objects\n");
+ for (auto &printer : printers) {
+ printer->Start();
+ }
+
+ for (int i = 8; i < 26; ++i) {
+ dio.emplace_back(new DigitalInput(i));
+ }
+
+ ::std::unique_ptr<Encoder> e0 = MakeEncoder(0);
+ ::std::unique_ptr<Encoder> e1 = MakeEncoder(1);
+ ::std::unique_ptr<Encoder> e2 = MakeEncoder(2);
+ ::std::unique_ptr<Encoder> e3 = MakeEncoder(3);
+
+ DMA dma;
+
+ dma.Add(sensor(6));
+ dma.Add(e0.get());
+ dma.SetExternalTrigger(sensor(6), true, true);
+ dma.Start();
+ while (!quit_) {
+ printf("Battery voltage %f\n",
+ DriverStation::GetInstance()->GetBatteryVoltage());
+
+ DMASample dma_sample;
+ size_t left;
+ DMA::ReadStatus dma_read_return = dma.Read(&dma_sample, 1000, &left);
+ printf("dma_read %d, items left %d\n", dma_read_return, left);
+
+ if (left >= 0) {
+ uint32_t sensor_value = 0;
+ uint32_t dma_sensor_value = 0;
+ for (int i = 0; i < 26; ++i) {
+ int j = i;
+ if (j >= 10) j += 6;
+ sensor_value |= (static_cast<uint32_t>(sensor(i)->Get()) << j);
+ dma_sensor_value |= (static_cast<uint32_t>(dma_sample.Get(sensor(i)) << j));
+ }
+
+ printf("dio 0x%x\n", sensor_value);
+ printf("dma 0x%x\n", dma_sensor_value);
+ printf("e0 %d, e0_dma %d\n", e0->GetRaw(), dma_sample.GetRaw(e0.get()));
+ printf("e1 %d, e1_dma %d\n", e1->GetRaw(), dma_sample.GetRaw(e1.get()));
+ printf("e2 %d, e2_dma %d\n", e2->GetRaw(), dma_sample.GetRaw(e2.get()));
+ printf("e3 %d, e3_dma %d\n", e3->GetRaw(), dma_sample.GetRaw(e3.get()));
+ printf("timestamp %f %f\n", dma_sample.GetTimestamp(),
+ static_cast<double>(GetFPGATime()) * 0.000001);
+
+ printf("Remaining is %d\n", left);
+ }
+ }
+ // Wait(0.5);
+ for (auto &printer : printers) {
+ printer->quit();
+ }
+ }
+
+ virtual void StartCompetition() {
+ AllEdgeTests();
+ }
+
+ private:
+ ::std::unique_ptr<Encoder> encoder_;
+ ::std::unique_ptr<Encoder> test_encoder_;
+ ::std::unique_ptr<Talon> talon_;
+ ::std::unique_ptr<DigitalInput> hall_;
+
+ ::std::atomic<bool> quit_;
+ ::std::mutex encoder_mutex_;
+};
+
+START_ROBOT_CLASS(TestRobot);