Add DMAPulseSeparationReader for shooter tuning
Signed-off-by: Ravago Jones <ravagojones@gmail.com>
Change-Id: Iaa7aa2e84d463c037ce26187864253379860c542
diff --git a/frc971/wpilib/dma_edge_counting.cc b/frc971/wpilib/dma_edge_counting.cc
index 1b7bee9..ca7e095 100644
--- a/frc971/wpilib/dma_edge_counting.cc
+++ b/frc971/wpilib/dma_edge_counting.cc
@@ -32,6 +32,25 @@
prev_sample_ = sample;
}
+void DMAPulseSeparationReader::UpdateFromSample(const DMASample &sample) {
+ // save the time of the rising edge of the input one
+ if (have_prev_sample_ && sample.Get(input_one_) &&
+ !prev_sample_.Get(input_one_)) {
+ input_one_time_ = sample.GetTimestamp();
+ }
+
+ have_prev_sample_ = true;
+ prev_sample_ = sample;
+
+ // take the difference in time between the rising edge of the input one and
+ // the rising edge of the input two
+ if (sample.Get(input_two_) && input_one_time_.has_value()) {
+ last_width_ = sample.GetTimestamp() - input_one_time_.value();
+ pulses_detected_++;
+ input_one_time_.reset();
+ }
+}
+
void DMASynchronizer::CheckDMA() {
DMASample current_sample;
diff --git a/frc971/wpilib/dma_edge_counting.h b/frc971/wpilib/dma_edge_counting.h
index a6f2da4..03ceeb0 100644
--- a/frc971/wpilib/dma_edge_counting.h
+++ b/frc971/wpilib/dma_edge_counting.h
@@ -2,6 +2,7 @@
#define FRC971_WPILIB_DMA_EDGE_COUNTING_H_
#include <memory>
+#include <optional>
#include <vector>
#include "aos/macros.h"
@@ -69,6 +70,53 @@
DISALLOW_COPY_AND_ASSIGN(DMAPulseWidthReader);
};
+// Takes two digital inputs and times the difference between the first one going
+// high and the second one going high.
+class DMAPulseSeparationReader : public DMASampleHandlerInterface {
+ public:
+ DMAPulseSeparationReader(frc::DigitalInput *input_one,
+ frc::DigitalInput *input_two)
+ : input_one_(input_one), input_two_(input_two) {}
+ DMAPulseSeparationReader() = default;
+
+ void set_input_one(frc::DigitalInput *input) { input_one_ = input; }
+ void set_input_two(frc::DigitalInput *input) { input_two_ = input; }
+
+ double last_width() const { return last_width_; }
+ double pulses_detected() const { return pulses_detected_; }
+
+ private:
+ void UpdateFromSample(const DMASample & /*sample*/) override;
+ void UpdatePolledValue() override {}
+
+ void PollFromSample(const DMASample & /*sample*/) override {}
+ void AddToDMA(DMA *dma) override {
+ dma->Add(input_one_);
+ dma->SetExternalTrigger(input_one_, true, true);
+ dma->Add(input_two_);
+ dma->SetExternalTrigger(input_two_, true, false);
+ }
+
+ static constexpr double kSampleTimeoutSeconds = 0.1;
+
+ frc::DigitalInput *input_one_ = nullptr;
+ frc::DigitalInput *input_two_ = nullptr;
+
+ // The last DMA reading we got.
+ DMASample prev_sample_;
+ // Whether or not we actually have anything in prev_sample_.
+ bool have_prev_sample_ = false;
+
+ // the time when the input one went high.
+ std::optional<double> input_one_time_;
+
+ int pulses_detected_ = 0;
+
+ double last_width_ = ::std::numeric_limits<double>::quiet_NaN();
+
+ DISALLOW_COPY_AND_ASSIGN(DMAPulseSeparationReader);
+};
+
// Counts edges on a sensor using DMA data and latches encoder values
// corresponding to those edges.
class DMAEdgeCounter : public DMASampleHandlerInterface {