add support for reading encoder+pot pairs with dma and interrupts
Change-Id: I6c091d649ac93f445265b645e8c4720687f663ae
diff --git a/frc971/wpilib/encoder_and_potentiometer.cc b/frc971/wpilib/encoder_and_potentiometer.cc
new file mode 100644
index 0000000..800c49b
--- /dev/null
+++ b/frc971/wpilib/encoder_and_potentiometer.cc
@@ -0,0 +1,57 @@
+#include "frc971/wpilib/encoder_and_potentiometer.h"
+
+#include "aos/linux_code/init.h"
+#include "aos/common/logging/logging.h"
+
+namespace frc971 {
+namespace wpilib {
+
+void DMAEncoderAndPotentiometer::UpdateFromSample(const DMASample &sample) {
+ if (index_last_value_) {
+ // It was already true last time, so check if it's reset back to false yet.
+ index_last_value_ = sample.Get(index_.get());
+ } else if (sample.Get(index_.get())) {
+ // This sample is posedge, so record all the values.
+ index_last_value_ = true;
+ ++index_posedge_count_;
+ last_encoder_value_ = sample.GetRaw(encoder_.get());
+ last_potentiometer_voltage_ = sample.GetVoltage(potentiometer_.get());
+ }
+}
+
+void InterruptEncoderAndPotentiometer::Start() {
+ CHECK_NE(nullptr, encoder_);
+ CHECK_NE(nullptr, index_);
+ CHECK_NE(nullptr, potentiometer_);
+ CHECK_NE(0, priority_);
+ thread_ = ::std::thread(::std::ref(*this));
+}
+
+void InterruptEncoderAndPotentiometer::operator()() {
+ ::aos::SetCurrentThreadName("IntEncPot_" +
+ ::std::to_string(potentiometer_->GetChannel()));
+
+ index_->RequestInterrupts();
+ index_->SetUpSourceEdge(true, false);
+
+ ::aos::SetCurrentThreadRealtimePriority(priority_);
+
+ InterruptableSensorBase::WaitResult result = InterruptableSensorBase::kBoth;
+ while (run_) {
+ result = index_->WaitForInterrupt(
+ 0.1, result != InterruptableSensorBase::kTimeout);
+ if (result == InterruptableSensorBase::kTimeout) {
+ continue;
+ }
+
+ {
+ ::aos::MutexLocker locker(&mutex_);
+ last_potentiometer_voltage_ = potentiometer_->GetVoltage();
+ last_encoder_value_ = encoder_->GetRaw();
+ ++index_posedge_count_;
+ }
+ }
+}
+
+} // namespace wpilib
+} // namespace frc971
diff --git a/frc971/wpilib/encoder_and_potentiometer.h b/frc971/wpilib/encoder_and_potentiometer.h
new file mode 100644
index 0000000..82fa9a8
--- /dev/null
+++ b/frc971/wpilib/encoder_and_potentiometer.h
@@ -0,0 +1,169 @@
+#ifndef FRC971_ENCODER_AND_POTENTIOMETER_H_
+#define FRC971_ENCODER_AND_POTENTIOMETER_H_
+
+#include <atomic>
+#include <thread>
+
+#include "aos/common/macros.h"
+#include "aos/common/mutex.h"
+
+#include "Encoder.h"
+#include "DigitalSource.h"
+#include "AnalogInput.h"
+#include "dma.h"
+
+#include "frc971/wpilib/dma_edge_counting.h"
+
+namespace frc971 {
+namespace wpilib {
+
+// Latches values from an encoder and potentiometer on positive edges from
+// another input using an interrupt.
+class InterruptEncoderAndPotentiometer {
+ public:
+ // priority is the priority the thread will run at.
+ InterruptEncoderAndPotentiometer(int priority) : priority_(priority) {}
+
+ // Starts the thread running so it can receive interrupts.
+ void Start();
+
+ // Tells the thread to stop running and then waits for it to finish.
+ void Stop() {
+ run_ = false;
+ thread_.join();
+ }
+
+ // Loops until Stop() is called, reading interrupts.
+ // Designed to be called by ::std::thread internally.
+ void operator()();
+
+ // Returns the mutex which must be held while calling index_posedge_count(),
+ // last_encoder_value(), and last_potentiometer_voltage().
+ // Holding this mutex will increase the handling latency.
+ ::aos::Mutex *mutex() { return &mutex_; }
+
+ void set_encoder(::std::unique_ptr<Encoder> encoder) {
+ encoder_ = ::std::move(encoder);
+ }
+ Encoder *encoder() const { return encoder_.get(); }
+
+ void set_index(::std::unique_ptr<DigitalSource> index) {
+ index_ = ::std::move(index);
+ }
+ DigitalSource *index() const { return index_.get(); }
+
+ void set_potentiometer(::std::unique_ptr<AnalogInput> potentiometer) {
+ potentiometer_ = ::std::move(potentiometer);
+ }
+ AnalogInput *potentiometer() const { return potentiometer_.get(); }
+
+ // Returns the number of poseges that have happened on the index input.
+ // mutex() must be held while calling this.
+ uint32_t index_posedge_count() const { return index_posedge_count_; }
+ // Returns the value of the encoder at the last index posedge.
+ // mutex() must be held while calling this.
+ int32_t last_encoder_value() const { return last_encoder_value_; }
+ // Returns the voltage of the potentiometer at the last index posedge.
+ // mutex() must be held while calling this.
+ float last_potentiometer_voltage() const {
+ return last_potentiometer_voltage_;
+ }
+
+ private:
+ ::std::unique_ptr<Encoder> encoder_;
+ ::std::unique_ptr<DigitalSource> index_;
+ ::std::unique_ptr<AnalogInput> potentiometer_;
+
+ int32_t last_encoder_value_{0};
+ float last_potentiometer_voltage_{0.0f};
+ uint32_t index_posedge_count_{0};
+
+ ::aos::Mutex mutex_;
+
+ const int priority_;
+
+ ::std::atomic<bool> run_{true};
+ ::std::thread thread_;
+
+ DISALLOW_COPY_AND_ASSIGN(InterruptEncoderAndPotentiometer);
+};
+
+// Latches values from an encoder and potentiometer on positive edges from
+// another input using DMA.
+class DMAEncoderAndPotentiometer : public DMASampleHandlerInterface {
+ public:
+ DMAEncoderAndPotentiometer() {}
+
+ void set_encoder(::std::unique_ptr<Encoder> encoder) {
+ encoder_ = ::std::move(encoder);
+ }
+ Encoder *encoder() const { return encoder_.get(); }
+
+ void set_index(::std::unique_ptr<DigitalSource> index) {
+ index_ = ::std::move(index);
+ }
+ DigitalSource *index() const { return index_.get(); }
+
+ void set_potentiometer(::std::unique_ptr<AnalogInput> potentiometer) {
+ potentiometer_ = ::std::move(potentiometer);
+ }
+ AnalogInput *potentiometer() const { return potentiometer_.get(); }
+
+ // Returns the most recent polled value of the encoder.
+ uint32_t polled_encoder_value() const { return polled_encoder_value_; }
+ // Returns the most recent polled voltage of the potentiometer.
+ float polled_potentiometer_voltage() const {
+ return polled_potentiometer_voltage_;
+ }
+
+ // Returns the number of poseges that have happened on the index input.
+ uint32_t index_posedge_count() const { return index_posedge_count_; }
+ // Returns the value of the encoder at the last index posedge.
+ int32_t last_encoder_value() const { return last_encoder_value_; }
+ // Returns the voltage of the potentiometer at the last index posedge.
+ float last_potentiometer_voltage() const {
+ return last_potentiometer_voltage_;
+ }
+
+ virtual void UpdateFromSample(const DMASample &sample) override;
+
+ virtual void PollFromSample(const DMASample &sample) override {
+ polled_encoder_value_ = sample.GetRaw(encoder_.get());
+ polled_potentiometer_voltage_ = sample.GetVoltage(potentiometer_.get());
+ }
+
+ virtual void UpdatePolledValue() override {
+ polled_encoder_value_ = encoder_->GetRaw();
+ polled_potentiometer_voltage_ = potentiometer_->GetVoltage();
+ }
+
+ virtual void AddToDMA(DMA *dma) override {
+ dma->Add(encoder_.get());
+ dma->Add(index_.get());
+ dma->Add(potentiometer_.get());
+ dma->SetExternalTrigger(index_.get(), true, false);
+ }
+
+ private:
+ ::std::unique_ptr<Encoder> encoder_;
+ ::std::unique_ptr<DigitalSource> index_;
+ ::std::unique_ptr<AnalogInput> potentiometer_;
+
+ int32_t polled_encoder_value_ = 0;
+ float polled_potentiometer_voltage_ = 0.0f;
+
+ int32_t last_encoder_value_ = 0;
+ float last_potentiometer_voltage_ = 0.0f;
+
+ uint32_t index_posedge_count_ = 0;
+
+ // Whether or not it was triggered in the last sample.
+ bool index_last_value_ = false;
+
+ DISALLOW_COPY_AND_ASSIGN(DMAEncoderAndPotentiometer);
+};
+
+} // namespace wpilib
+} // namespace frc971
+
+#endif // FRC971_ENCODER_AND_POTENTIOMETER_H_
diff --git a/frc971/wpilib/wpilib.gyp b/frc971/wpilib/wpilib.gyp
index 14da17a..a238018 100644
--- a/frc971/wpilib/wpilib.gyp
+++ b/frc971/wpilib/wpilib.gyp
@@ -30,6 +30,26 @@
'gyro_sender',
'dma_edge_counting',
'interrupt_edge_counting',
+ 'encoder_and_potentiometer',
+ ],
+ },
+ {
+ 'target_name': 'encoder_and_potentiometer',
+ 'type': 'static_library',
+ 'sources': [
+ 'encoder_and_potentiometer.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):WPILib',
+ 'dma_edge_counting',
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/common.gyp:mutex',
+ ],
+ 'export_dependent_settings': [
+ '<(EXTERNALS):WPILib',
+ 'dma_edge_counting',
+ '<(AOS)/common/common.gyp:mutex',
],
},
{
diff --git a/frc971/wpilib/wpilib_interface.cc b/frc971/wpilib/wpilib_interface.cc
index 90c1c3a..6d924e5 100644
--- a/frc971/wpilib/wpilib_interface.cc
+++ b/frc971/wpilib/wpilib_interface.cc
@@ -33,6 +33,7 @@
#include "frc971/wpilib/gyro_sender.h"
#include "frc971/wpilib/dma_edge_counting.h"
#include "frc971/wpilib/interrupt_edge_counting.h"
+#include "frc971/wpilib/encoder_and_potentiometer.h"
#include "Encoder.h"
#include "Talon.h"