Add DMA lidar lite reader.
Change-Id: I8195ff2331029adcdb961b9925f43471cc6b08f9
diff --git a/frc971/wpilib/dma_edge_counting.cc b/frc971/wpilib/dma_edge_counting.cc
index 4cf4440..8ea3071 100644
--- a/frc971/wpilib/dma_edge_counting.cc
+++ b/frc971/wpilib/dma_edge_counting.cc
@@ -24,6 +24,14 @@
}
}
+void DMAPulseWidthReader::UpdateFromSample(const DMASample &sample) {
+ if (have_prev_sample_ && prev_sample_.Get(input_) && !sample.Get(input_)) {
+ last_width_ = sample.GetTimestamp() - prev_sample_.GetTimestamp();
+ }
+ have_prev_sample_ = true;
+ prev_sample_ = sample;
+}
+
void DMASynchronizer::CheckDMA() {
DMASample current_sample;
@@ -45,6 +53,7 @@
}
return;
}
+ break;
case DMA::STATUS_TIMEOUT:
return;
case DMA::STATUS_ERROR:
diff --git a/frc971/wpilib/dma_edge_counting.h b/frc971/wpilib/dma_edge_counting.h
index 813f59c..c8a00ea 100644
--- a/frc971/wpilib/dma_edge_counting.h
+++ b/frc971/wpilib/dma_edge_counting.h
@@ -37,6 +37,38 @@
virtual void AddToDMA(DMA *dma) = 0;
};
+// TODO(brian): Timeout old data.
+class DMAPulseWidthReader : public DMASampleHandlerInterface {
+ public:
+ DMAPulseWidthReader(DigitalInput *input) : input_(input) {}
+ DMAPulseWidthReader() = default;
+
+ void set_input(DigitalInput *input) { input_ = input; }
+
+ double last_width() const { return last_width_; }
+
+ private:
+ void UpdateFromSample(const DMASample & /*sample*/) override;
+ void UpdatePolledValue() override {}
+
+ void PollFromSample(const DMASample & /*sample*/) override {}
+ void AddToDMA(DMA *dma) override {
+ dma->Add(input_);
+ dma->SetExternalTrigger(input_, true, true);
+ }
+
+ DigitalInput *input_ = 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;
+
+ double last_width_ = ::std::numeric_limits<double>::quiet_NaN();
+
+ DISALLOW_COPY_AND_ASSIGN(DMAPulseWidthReader);
+};
+
// Counts edges on a sensor using DMA data and latches encoder values
// corresponding to those edges.
class DMAEdgeCounter : public DMASampleHandlerInterface {
diff --git a/y2018/control_loops/superstructure/superstructure.q b/y2018/control_loops/superstructure/superstructure.q
index 0da9d17..9bdabed 100644
--- a/y2018/control_loops/superstructure/superstructure.q
+++ b/y2018/control_loops/superstructure/superstructure.q
@@ -171,6 +171,9 @@
// Value of the beambreak sensor detecting when the box has hit the frame
// cutout.
bool box_back_beambreak_triggered;
+
+ // Distance to the box in meters.
+ double box_distance;
};
message Output {
diff --git a/y2018/wpilib_interface.cc b/y2018/wpilib_interface.cc
index cf19b26..6fba1fe 100644
--- a/y2018/wpilib_interface.cc
+++ b/y2018/wpilib_interface.cc
@@ -287,11 +287,17 @@
pwm_trigger_ = ::std::move(pwm_trigger);
}
+ void set_lidar_lite_input(::std::unique_ptr<DigitalInput> lidar_lite_input) {
+ lidar_lite_input_ = ::std::move(lidar_lite_input);
+ lidar_lite_.set_input(lidar_lite_input_.get());
+ }
+
// All of the DMA-related set_* calls must be made before this, and it
// doesn't hurt to do all of them.
void set_dma(::std::unique_ptr<DMA> dma) {
dma_synchronizer_.reset(
new ::frc971::wpilib::DMASynchronizer(::std::move(dma)));
+ dma_synchronizer_->Add(&lidar_lite_);
}
void RunPWMDetecter() {
@@ -479,6 +485,9 @@
superstructure_message->box_back_beambreak_triggered =
!box_back_beambreak_->Get();
+ superstructure_message->box_distance =
+ lidar_lite_.last_width() / 0.00001 / 100.0 / 2;
+
superstructure_message.Send();
}
@@ -562,6 +571,9 @@
::std::array<::std::unique_ptr<DigitalInput>, 4> autonomous_modes_;
+ ::std::unique_ptr<DigitalInput> lidar_lite_input_;
+ ::frc971::wpilib::DMAPulseWidthReader lidar_lite_;
+
::std::atomic<bool> run_{true};
};
@@ -853,6 +865,8 @@
reader.set_pwm_trigger(make_unique<DigitalInput>(25));
+ reader.set_lidar_lite_input(make_unique<DigitalInput>(22));
+
reader.set_dma(make_unique<DMA>());
::std::thread reader_thread(::std::ref(reader));