split out the interrupt-based edge-counting code
Change-Id: I57f7b451e1ce16e092c9c86f44b6b45e71bbc80d
diff --git a/frc971/wpilib/wpilib_interface.cc b/frc971/wpilib/wpilib_interface.cc
index 207c481..3d5958b 100644
--- a/frc971/wpilib/wpilib_interface.cc
+++ b/frc971/wpilib/wpilib_interface.cc
@@ -29,6 +29,7 @@
#include "frc971/wpilib/buffered_solenoid.h"
#include "frc971/wpilib/buffered_pcm.h"
#include "frc971/wpilib/gyro_sender.h"
+#include "frc971/wpilib/interrupt_edge_counting.h"
#include "Encoder.h"
#include "Talon.h"
@@ -48,254 +49,6 @@
namespace frc971 {
namespace wpilib {
-// TODO(brian): Split this out into a separate file once DMA is in.
-class EdgeCounter {
- public:
- EdgeCounter(int priority, Encoder *encoder, HallEffect *input,
- ::aos::stl_mutex *mutex)
- : priority_(priority),
- encoder_(encoder),
- input_(input),
- mutex_(mutex),
- run_(true),
- any_interrupt_count_(0) {
- thread_.reset(new ::std::thread(::std::ref(*this)));
- }
-
- // Waits for interrupts, locks the mutex, and updates the internal state.
- // Updates the any_interrupt_count count when the interrupt comes in without
- // the lock.
- void operator()() {
- ::aos::SetCurrentThreadName("EdgeCounter_" +
- ::std::to_string(input_->GetChannel()));
-
- input_->RequestInterrupts();
- input_->SetUpSourceEdge(true, true);
-
- {
- ::std::unique_lock<::aos::stl_mutex> mutex_guard(*mutex_);
- current_value_ = input_->GetHall();
- }
-
- ::aos::SetCurrentThreadRealtimePriority(priority_);
- InterruptableSensorBase::WaitResult result = InterruptableSensorBase::kBoth;
- while (run_) {
- result = input_->WaitForInterrupt(
- 0.1, result != InterruptableSensorBase::kTimeout);
- if (result == InterruptableSensorBase::kTimeout) {
- continue;
- }
- ++any_interrupt_count_;
-
- ::std::unique_lock<::aos::stl_mutex> mutex_guard(*mutex_);
- int32_t encoder_value = encoder_->GetRaw();
- bool hall_value = input_->GetHall();
- if (current_value_ != hall_value) {
- if (hall_value) {
- ++positive_interrupt_count_;
- last_positive_encoder_value_ = encoder_value;
- } else {
- ++negative_interrupt_count_;
- last_negative_encoder_value_ = encoder_value;
- }
- } else {
- LOG(WARNING, "Detected spurious edge on %d. Dropping it.\n",
- input_->GetChannel());
- }
-
- current_value_ = hall_value;
- }
- }
-
- // Updates the internal hall effect value given this new observation.
- // The mutex provided at construction time must be held during this operation.
- void set_polled_value(bool value) {
- polled_value_ = value;
- bool miss_match = (value != current_value_);
- if (miss_match && last_miss_match_) {
- current_value_ = value;
- last_miss_match_ = false;
- } else {
- last_miss_match_ = miss_match;
- }
- }
-
- // Signals the thread to quit next time it gets an interrupt.
- void Quit() {
- run_ = false;
- thread_->join();
- }
-
- // Returns the total number of interrupts since construction time. This
- // should be done without the mutex held.
- int any_interrupt_count() const { return any_interrupt_count_; }
- // Returns the current interrupt edge counts and encoder values.
- // The mutex provided at construction time must be held during this operation.
- int positive_interrupt_count() const { return positive_interrupt_count_; }
- int negative_interrupt_count() const { return negative_interrupt_count_; }
- int32_t last_positive_encoder_value() const {
- return last_positive_encoder_value_;
- }
- int32_t last_negative_encoder_value() const {
- return last_negative_encoder_value_;
- }
- // Returns the current polled value.
- bool polled_value() const { return polled_value_; }
-
- private:
- int priority_;
- Encoder *encoder_;
- HallEffect *input_;
- ::aos::stl_mutex *mutex_;
- ::std::atomic<bool> run_;
-
- ::std::atomic<int> any_interrupt_count_;
-
- // The following variables represent the current state. They must be
- // synchronized by mutex_;
- bool current_value_ = false;
- bool polled_value_ = false;
- bool last_miss_match_ = true;
- int positive_interrupt_count_ = 0;
- int negative_interrupt_count_ = 0;
- int32_t last_positive_encoder_value_ = 0;
- int32_t last_negative_encoder_value_ = 0;
-
- ::std::unique_ptr<::std::thread> thread_;
-};
-
-// This class will synchronize sampling edges on a bunch of HallEffects with
-// the periodic poll.
-//
-// The data is provided to subclasses by calling SaveState when the state is
-// consistent and ready.
-//
-// TODO(brian): Split this out into a separate file once DMA is in.
-template <int num_sensors>
-class PeriodicHallSynchronizer {
- public:
- PeriodicHallSynchronizer(
- const char *name, int priority, int interrupt_priority,
- ::std::unique_ptr<Encoder> encoder,
- ::std::array<::std::unique_ptr<HallEffect>, num_sensors> *sensors)
- : name_(name),
- priority_(priority),
- encoder_(::std::move(encoder)),
- run_(true) {
- for (int i = 0; i < num_sensors; ++i) {
- sensors_[i] = ::std::move((*sensors)[i]);
- edge_counters_[i] = ::std::unique_ptr<EdgeCounter>(new EdgeCounter(
- interrupt_priority, encoder_.get(), sensors_[i].get(), &mutex_));
- }
- }
-
- const char *name() const { return name_.c_str(); }
-
- void StartThread() { thread_.reset(new ::std::thread(::std::ref(*this))); }
-
- // Called when the state is consistent and up to date.
- virtual void SaveState() = 0;
-
- // Starts a sampling iteration. See RunIteration for usage.
- void StartIteration() {
- // Start by capturing the current interrupt counts.
- for (int i = 0; i < num_sensors; ++i) {
- interrupt_counts_[i] = edge_counters_[i]->any_interrupt_count();
- }
-
- {
- // Now, update the encoder and sensor values.
- ::std::unique_lock<::aos::stl_mutex> mutex_guard(mutex_);
- encoder_value_ = encoder_->GetRaw();
- for (int i = 0; i < num_sensors; ++i) {
- edge_counters_[i]->set_polled_value(sensors_[i]->GetHall());
- }
- }
- }
-
- // Attempts to finish a sampling iteration. See RunIteration for usage.
- // Returns true if the iteration succeeded, and false otherwise.
- bool TryFinishingIteration() {
- // Make sure no interrupts have occurred while we were waiting. If they
- // have, we are in an inconsistent state and need to try again.
- ::std::unique_lock<::aos::stl_mutex> mutex_guard(mutex_);
- bool retry = false;
- for (int i = 0; i < num_sensors; ++i) {
- retry = retry || (interrupt_counts_[i] !=
- edge_counters_[i]->any_interrupt_count());
- }
- if (!retry) {
- SaveState();
- return true;
- }
- LOG(WARNING, "Got an interrupt while sampling encoder %s, retrying\n",
- name());
- return false;
- }
-
- void RunIteration() {
- while (true) {
- StartIteration();
-
- // Wait more than the amount of time it takes for a digital input change
- // to go from visible to software to having triggered an interrupt.
- ::aos::time::SleepFor(::aos::time::Time::InUS(120));
-
- if (TryFinishingIteration()) {
- return;
- }
- }
- }
-
- void operator()() {
- ::aos::SetCurrentThreadName("HallSync" + ::std::to_string(num_sensors));
- ::aos::SetCurrentThreadRealtimePriority(priority_);
- while (run_) {
- ::aos::time::PhasedLoopXMS(10, 9000);
- RunIteration();
- }
- }
-
- void Quit() {
- run_ = false;
- for (int i = 0; i < num_sensors; ++i) {
- edge_counters_[i]->Quit();
- }
- if (thread_) {
- thread_->join();
- }
- }
-
- protected:
- // These values are only safe to fetch from inside SaveState()
- int32_t encoder_value() const { return encoder_value_; }
- ::std::array<::std::unique_ptr<EdgeCounter>, num_sensors> &edge_counters() {
- return edge_counters_;
- }
-
- private:
- // A descriptive name for error messages.
- ::std::string name_;
- // The priority of the polling thread.
- int priority_;
- // The Encoder to sample.
- ::std::unique_ptr<Encoder> encoder_;
- // A list of all the digital inputs.
- ::std::array<::std::unique_ptr<HallEffect>, num_sensors> sensors_;
- // The mutex used to synchronize all the state.
- ::aos::stl_mutex mutex_;
- ::std::atomic<bool> run_;
-
- // The state.
- // The current encoder value.
- int32_t encoder_value_ = 0;
- // The current edge counters.
- ::std::array<::std::unique_ptr<EdgeCounter>, num_sensors> edge_counters_;
-
- ::std::unique_ptr<::std::thread> thread_;
- ::std::array<int, num_sensors> interrupt_counts_;
-};
-
double drivetrain_translate(int32_t in) {
return static_cast<double>(in) /
(256.0 /*cpr*/ * 2.0 /*2x. Stupid WPILib*/) *