Added the operator to frc971 SensorReader.
Took the operator fuction out of the year specific SensorReader, needed
to make a empty RunInteration in order for it to work.
Change-Id: I0ec02ab293c1f8eba56d6365383105e860f69cdd
diff --git a/frc971/wpilib/BUILD b/frc971/wpilib/BUILD
index 00cd77d..d0f6bce 100644
--- a/frc971/wpilib/BUILD
+++ b/frc971/wpilib/BUILD
@@ -289,6 +289,7 @@
deps = [
"//aos/stl_mutex",
"//aos/time:time",
+ "//aos/util:phased_loop",
"//aos:init",
"//third_party:wpilib",
":dma",
diff --git a/frc971/wpilib/sensor_reader.cc b/frc971/wpilib/sensor_reader.cc
index 24d33f8..2f50c4a 100644
--- a/frc971/wpilib/sensor_reader.cc
+++ b/frc971/wpilib/sensor_reader.cc
@@ -1,7 +1,11 @@
#include "frc971/wpilib/sensor_reader.h"
+#include <inttypes.h>
+#include <unistd.h>
+
#include "aos/init.h"
#include "aos/util/compiler_memory_barrier.h"
+#include "aos/util/phased_loop.h"
#include "frc971/wpilib/ahal/DigitalInput.h"
#include "frc971/wpilib/ahal/Utility.h"
@@ -102,5 +106,51 @@
pwm_trigger_->CancelInterrupts();
}
+void SensorReader::operator()() {
+ ::aos::SetCurrentThreadName("SensorReader");
+
+ my_pid_ = getpid();
+
+ dma_synchronizer_->Start();
+
+ ::aos::time::PhasedLoop phased_loop(last_period_,
+ ::std::chrono::milliseconds(3));
+ chrono::nanoseconds filtered_period = last_period_;
+
+ ::std::thread pwm_detecter_thread(
+ ::std::bind(&SensorReader::RunPWMDetecter, this));
+
+ ::aos::SetCurrentThreadRealtimePriority(40);
+ while (run_) {
+ {
+ const int iterations = phased_loop.SleepUntilNext();
+ if (iterations != 1) {
+ LOG(WARNING, "SensorReader skipped %d iterations\n", iterations - 1);
+ }
+ }
+ RunIteration();
+
+ monotonic_clock::time_point last_tick_timepoint;
+ chrono::nanoseconds period;
+ {
+ ::std::unique_lock<::aos::stl_mutex> locker(tick_time_mutex_);
+ last_tick_timepoint = last_tick_time_monotonic_timepoint_;
+ period = last_period_;
+ }
+
+ if (last_tick_timepoint == monotonic_clock::min_time) {
+ continue;
+ }
+ chrono::nanoseconds new_offset = phased_loop.OffsetFromIntervalAndTime(
+ period, last_tick_timepoint + chrono::microseconds(2050));
+
+ // TODO(austin): If this is the first edge in a while, skip to it (plus
+ // an offset). Otherwise, slowly drift time to line up.
+
+ phased_loop.set_interval_and_offset(period, new_offset);
+ }
+ pwm_detecter_thread.join();
+}
+
} // namespace wpilib
} // namespace frc971
diff --git a/frc971/wpilib/sensor_reader.h b/frc971/wpilib/sensor_reader.h
index 3abe1b0..b3ced4c 100644
--- a/frc971/wpilib/sensor_reader.h
+++ b/frc971/wpilib/sensor_reader.h
@@ -36,11 +36,17 @@
// Stops the pwm trigger on the next iteration.
void Quit() { run_ = false; }
+ virtual void RunIteration() = 0;
+
+ void operator()();
+
protected:
// Uses the pwm trigger to find the pwm cycle width and offset for that
// iteration.
void RunPWMDetecter();
+ int32_t my_pid_;
+
::std::unique_ptr<frc::DigitalInput> pwm_trigger_;
frc::DigitalGlitchFilter fast_encoder_filter_, medium_encoder_filter_,
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index dcdf00b..9395489 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -116,53 +116,7 @@
hall_filter_.SetPeriodNanoSeconds(100000);
}
- void operator()() {
- ::aos::SetCurrentThreadName("SensorReader");
-
- my_pid_ = getpid();
-
- dma_synchronizer_->Start();
-
- ::aos::time::PhasedLoop phased_loop(last_period_,
- ::std::chrono::milliseconds(3));
- chrono::nanoseconds filtered_period = last_period_;
-
- ::std::thread pwm_detecter_thread(
- ::std::bind(&SensorReader::RunPWMDetecter, this));
-
- ::aos::SetCurrentThreadRealtimePriority(40);
- while (run_) {
- {
- const int iterations = phased_loop.SleepUntilNext();
- if (iterations != 1) {
- LOG(WARNING, "SensorReader skipped %d iterations\n", iterations - 1);
- }
- }
- RunIteration();
-
- monotonic_clock::time_point last_tick_timepoint;
- chrono::nanoseconds period;
- {
- ::std::unique_lock<::aos::stl_mutex> locker(tick_time_mutex_);
- last_tick_timepoint = last_tick_time_monotonic_timepoint_;
- period = last_period_;
- }
-
- if (last_tick_timepoint == monotonic_clock::min_time) {
- continue;
- }
- chrono::nanoseconds new_offset = phased_loop.OffsetFromIntervalAndTime(
- period, last_tick_timepoint + chrono::microseconds(2050));
-
- // TODO(austin): If this is the first edge in a while, skip to it (plus
- // an offset). Otherwise, slowly drift time to line up.
-
- phased_loop.set_interval_and_offset(period, new_offset);
- }
- pwm_detecter_thread.join();
- }
-
- void RunIteration() {
+ void RunIteration() override {
::frc971::wpilib::SendRobotState(my_pid_);
{
@@ -182,13 +136,6 @@
dma_synchronizer_->RunIteration();
}
-
-
- private:
- int32_t my_pid_;
-
- ::std::unique_ptr<frc::Encoder> drivetrain_left_encoder_,
- drivetrain_right_encoder_;
};
class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler {