Move y2012 over to new SensorReader class
While we are here, shove the RobotState sender and DMA tick code into
the base class.
Change-Id: I49e6b3902c1ee46d396ccc77c2b8dce825e891d8
diff --git a/frc971/wpilib/BUILD b/frc971/wpilib/BUILD
index d0f6bce..27f9190 100644
--- a/frc971/wpilib/BUILD
+++ b/frc971/wpilib/BUILD
@@ -21,9 +21,9 @@
deps = [
":dma",
":dma_edge_counting",
- "//aos/mutex:mutex",
- "//aos/logging",
"//aos:init",
+ "//aos/logging",
+ "//aos/mutex",
"//third_party:wpilib",
],
)
@@ -54,10 +54,10 @@
],
restricted_to = ["//tools:roborio"],
deps = [
- "//aos/stl_mutex:stl_mutex",
- "//aos/time:time",
- "//aos/logging",
"//aos:init",
+ "//aos/logging",
+ "//aos/stl_mutex",
+ "//aos/time",
"//third_party:wpilib",
],
)
@@ -89,8 +89,8 @@
],
restricted_to = ["//tools:roborio"],
deps = [
- "//aos/time:time",
"//aos/logging",
+ "//aos/time",
"//third_party:wpilib",
],
)
@@ -106,12 +106,12 @@
restricted_to = ["//tools:roborio"],
deps = [
":gyro_interface",
- "//aos/time:time",
+ "//aos:init",
"//aos/logging",
"//aos/logging:queue_logging",
- "//aos/robot_state:robot_state",
+ "//aos/robot_state",
+ "//aos/time",
"//aos/util:phased_loop",
- "//aos:init",
"//frc971/queues:gyro",
"//frc971/zeroing:averager",
],
@@ -127,8 +127,8 @@
],
restricted_to = ["//tools:roborio"],
deps = [
- "//aos/mutex:mutex",
- "//aos/time:time",
+ "//aos/mutex",
+ "//aos/time",
"//frc971/queues:gyro",
"//third_party:wpilib",
],
@@ -143,11 +143,11 @@
"loop_output_handler.h",
],
deps = [
- "//aos/scoped:scoped_fd",
- "//aos/time:time",
- "//aos/robot_state:robot_state",
- "//aos/util:log_interval",
"//aos:init",
+ "//aos/robot_state",
+ "//aos/scoped:scoped_fd",
+ "//aos/time",
+ "//aos/util:log_interval",
],
)
@@ -161,10 +161,10 @@
],
restricted_to = ["//tools:roborio"],
deps = [
- "//aos/logging:queue_logging",
- "//aos/robot_state:robot_state",
- "//aos/network:team_number",
"//aos:init",
+ "//aos/logging:queue_logging",
+ "//aos/network:team_number",
+ "//aos/robot_state",
"//third_party:wpilib",
],
)
@@ -180,7 +180,7 @@
restricted_to = ["//tools:roborio"],
deps = [
"//aos/logging:queue_logging",
- "//aos/robot_state:robot_state",
+ "//aos/robot_state",
"//third_party:wpilib",
],
)
@@ -203,9 +203,9 @@
restricted_to = ["//tools:roborio"],
deps = [
":pdp_values",
+ "//aos:init",
"//aos/logging:queue_logging",
"//aos/util:phased_loop",
- "//aos:init",
"//third_party:wpilib",
],
)
@@ -240,11 +240,11 @@
deps = [
":imu_queue",
":spi_rx_clearer",
- "//aos/time:time",
+ "//aos:init",
"//aos/logging",
"//aos/logging:queue_logging",
- "//aos/robot_state:robot_state",
- "//aos:init",
+ "//aos/robot_state",
+ "//aos/time",
"//frc971/zeroing:averager",
"//third_party:wpilib",
],
@@ -283,16 +283,17 @@
"sensor_reader.cc",
],
hdrs = [
- "sensor_reader.h"
+ "sensor_reader.h",
],
restricted_to = ["//tools:roborio"],
deps = [
- "//aos/stl_mutex",
- "//aos/time:time",
- "//aos/util:phased_loop",
- "//aos:init",
- "//third_party:wpilib",
":dma",
":dma_edge_counting",
+ ":wpilib_interface",
+ "//aos:init",
+ "//aos/stl_mutex",
+ "//aos/time",
+ "//aos/util:phased_loop",
+ "//third_party:wpilib",
],
)
diff --git a/frc971/wpilib/sensor_reader.cc b/frc971/wpilib/sensor_reader.cc
index 2f50c4a..6ce1995 100644
--- a/frc971/wpilib/sensor_reader.cc
+++ b/frc971/wpilib/sensor_reader.cc
@@ -8,26 +8,44 @@
#include "aos/util/phased_loop.h"
#include "frc971/wpilib/ahal/DigitalInput.h"
#include "frc971/wpilib/ahal/Utility.h"
+#include "frc971/wpilib/wpilib_interface.h"
namespace frc971 {
namespace wpilib {
-SensorReader::SensorReader() {}
+SensorReader::SensorReader() {
+ // Set some defaults. We don't tend to exceed these, so old robots should
+ // just work with them.
+ UpdateFastEncoderFilterHz(500000);
+ UpdateMediumEncoderFilterHz(100000);
+}
+
+void SensorReader::UpdateFastEncoderFilterHz(int hz) {
+ fast_encoder_filter_.SetPeriodHz(::std::max(hz, 100000));
+}
+
+void SensorReader::UpdateMediumEncoderFilterHz(int hz) {
+ medium_encoder_filter_.SetPeriodHz(::std::max(hz, 50000));
+}
void SensorReader::set_drivetrain_left_encoder(
::std::unique_ptr<frc::Encoder> encoder) {
fast_encoder_filter_.Add(encoder.get());
drivetrain_left_encoder_ = ::std::move(encoder);
+ drivetrain_left_encoder_->SetMaxPeriod(0.005);
}
void SensorReader::set_drivetrain_right_encoder(
::std::unique_ptr<frc::Encoder> encoder) {
fast_encoder_filter_.Add(encoder.get());
drivetrain_right_encoder_ = ::std::move(encoder);
+ drivetrain_right_encoder_->SetMaxPeriod(0.005);
}
// All of the DMA-related set_* calls must be made before this, and it
// doesn't hurt to do all of them.
+// TODO(austin): Does anyone actually do anything other than set this? Or can
+// we just take care of that automatically?
void SensorReader::set_dma(::std::unique_ptr<DMA> dma) {
dma_synchronizer_.reset(
new ::frc971::wpilib::DMASynchronizer(::std::move(dma)));
@@ -109,16 +127,25 @@
void SensorReader::operator()() {
::aos::SetCurrentThreadName("SensorReader");
- my_pid_ = getpid();
+ int32_t my_pid = getpid();
dma_synchronizer_->Start();
+ if (pwm_trigger_) {
+ last_period_ = chrono::microseconds(5050);
+ LOG(INFO, "Using PWM trigger and a 5.05 ms period\n");
+ } else {
+ LOG(INFO, "Defaulting to open loop pwm synchronization\n");
+ last_period_ = chrono::microseconds(5000);
+ }
+ ::aos::time::PhasedLoop phased_loop(
+ last_period_,
+ pwm_trigger_ ? ::std::chrono::milliseconds(3) : chrono::milliseconds(4));
- ::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));
+ ::std::thread pwm_detecter_thread;
+ if (pwm_trigger_) {
+ pwm_detecter_thread =
+ ::std::thread(::std::bind(&SensorReader::RunPWMDetecter, this));
+ }
::aos::SetCurrentThreadRealtimePriority(40);
while (run_) {
@@ -128,28 +155,35 @@
LOG(WARNING, "SensorReader skipped %d iterations\n", iterations - 1);
}
}
+
+ ::frc971::wpilib::SendRobotState(my_pid);
RunIteration();
+ dma_synchronizer_->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 (pwm_trigger_) {
+ 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);
}
-
- 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();
+ if (pwm_trigger_) {
+ pwm_detecter_thread.join();
+ }
}
} // namespace wpilib
diff --git a/frc971/wpilib/sensor_reader.h b/frc971/wpilib/sensor_reader.h
index b3ced4c..0ce203c 100644
--- a/frc971/wpilib/sensor_reader.h
+++ b/frc971/wpilib/sensor_reader.h
@@ -21,6 +21,10 @@
public:
SensorReader();
+ // Updates the fast and medium encoder filter frequencies.
+ void UpdateFastEncoderFilterHz(int hz);
+ void UpdateMediumEncoderFilterHz(int hz);
+
// Sets the left drivetrain encoder.
void set_drivetrain_left_encoder(::std::unique_ptr<frc::Encoder> encoder);
@@ -41,29 +45,27 @@
void operator()();
protected:
+ frc::DigitalGlitchFilter fast_encoder_filter_, medium_encoder_filter_;
+
+ ::std::unique_ptr<frc::Encoder> drivetrain_left_encoder_,
+ drivetrain_right_encoder_;
+
+ private:
// 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_,
- hall_filter_;
-
// Mutex to manage access to the period and tick time variables.
::aos::stl_mutex tick_time_mutex_;
monotonic_clock::time_point last_tick_time_monotonic_timepoint_ =
monotonic_clock::min_time;
- chrono::nanoseconds last_period_ = chrono::microseconds(5050);
+ chrono::nanoseconds last_period_;
::std::unique_ptr<::frc971::wpilib::DMASynchronizer> dma_synchronizer_;
::std::atomic<bool> run_{true};
-
- ::std::unique_ptr<frc::Encoder> drivetrain_left_encoder_,
- drivetrain_right_encoder_;
};
} // namespace wpilib
diff --git a/y2012/BUILD b/y2012/BUILD
index 5e85c3d..159632b 100644
--- a/y2012/BUILD
+++ b/y2012/BUILD
@@ -57,6 +57,7 @@
"//frc971/wpilib:joystick_sender",
"//frc971/wpilib:logging_queue",
"//frc971/wpilib:loop_output_handler",
+ "//frc971/wpilib:sensor_reader",
"//frc971/wpilib:wpilib_interface",
"//frc971/wpilib:wpilib_robot_base",
"//third_party:wpilib",
diff --git a/y2012/wpilib_interface.cc b/y2012/wpilib_interface.cc
index b5be8f4..27e8b77 100644
--- a/y2012/wpilib_interface.cc
+++ b/y2012/wpilib_interface.cc
@@ -29,7 +29,6 @@
#include "aos/util/log_interval.h"
#include "aos/util/phased_loop.h"
#include "aos/util/wrapping_counter.h"
-
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/wpilib/buffered_pcm.h"
#include "frc971/wpilib/buffered_solenoid.h"
@@ -41,7 +40,7 @@
#include "frc971/wpilib/joystick_sender.h"
#include "frc971/wpilib/logging.q.h"
#include "frc971/wpilib/loop_output_handler.h"
-#include "frc971/wpilib/wpilib_interface.h"
+#include "frc971/wpilib/sensor_reader.h"
#include "y2012/control_loops/accessories/accessories.q.h"
#ifndef M_PI
@@ -69,43 +68,11 @@
(3.5 /*wheel diameter*/ * 2.54 / 100.0 * M_PI) * 2.0 / 2.0;
}
-class SensorReader {
+class SensorReader : public ::frc971::wpilib::SensorReader {
public:
SensorReader() {}
- void set_drivetrain_left_encoder(::std::unique_ptr<Encoder> encoder) {
- drivetrain_left_encoder_ = ::std::move(encoder);
- drivetrain_left_encoder_->SetMaxPeriod(0.005);
- }
-
- void set_drivetrain_right_encoder(::std::unique_ptr<Encoder> encoder) {
- drivetrain_right_encoder_ = ::std::move(encoder);
- drivetrain_right_encoder_->SetMaxPeriod(0.005);
- }
-
- void operator()() {
- ::aos::SetCurrentThreadName("SensorReader");
-
- my_pid_ = getpid();
-
- ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
- ::std::chrono::milliseconds(4));
-
- ::aos::SetCurrentThreadRealtimePriority(40);
- while (run_) {
- {
- const int iterations = phased_loop.SleepUntilNext();
- if (iterations != 1) {
- LOG(WARNING, "SensorReader skipped %d iterations\n", iterations - 1);
- }
- }
- RunIteration();
- }
- }
-
void RunIteration() {
- ::frc971::wpilib::SendRobotState(my_pid_);
-
{
auto drivetrain_message = drivetrain_queue.position.MakeMessage();
drivetrain_message->right_encoder =
@@ -122,15 +89,6 @@
accessories_queue.position.MakeMessage().Send();
}
-
- void Quit() { run_ = false; }
-
- private:
- int32_t my_pid_;
-
- ::std::unique_ptr<Encoder> drivetrain_left_encoder_;
- ::std::unique_ptr<Encoder> drivetrain_right_encoder_;
- ::std::atomic<bool> run_{true};
};
class SolenoidWriter {
@@ -301,6 +259,7 @@
::std::thread joystick_thread(::std::ref(joystick_sender));
SensorReader reader;
+ reader.set_dma(make_unique<DMA>());
reader.set_drivetrain_left_encoder(make_encoder(0));
reader.set_drivetrain_right_encoder(make_encoder(1));
diff --git a/y2018/wpilib_interface.cc b/y2018/wpilib_interface.cc
index dea6a7b..6ddcd41 100644
--- a/y2018/wpilib_interface.cc
+++ b/y2018/wpilib_interface.cc
@@ -380,7 +380,6 @@
::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));
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index ff5839f..7a6a360 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -38,7 +38,6 @@
#include "frc971/wpilib/loop_output_handler.h"
#include "frc971/wpilib/pdp_fetcher.h"
#include "frc971/wpilib/sensor_reader.h"
-#include "frc971/wpilib/wpilib_interface.h"
#include "frc971/wpilib/wpilib_robot_base.h"
#include "y2019/constants.h"
@@ -107,20 +106,11 @@
SensorReader() {
// Set to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
- fast_encoder_filter_.SetPeriodNanoSeconds(
- static_cast<int>(1 / 4.0 /* built-in tolerance */ /
- kMaxFastEncoderPulsesPerSecond * 1e9 +
- 0.5));
- medium_encoder_filter_.SetPeriodNanoSeconds(
- static_cast<int>(1 / 4.0 /* built-in tolerance */ /
- kMaxMediumEncoderPulsesPerSecond * 1e9 +
- 0.5));
- hall_filter_.SetPeriodNanoSeconds(100000);
+ UpdateFastEncoderFilterHz(kMaxFastEncoderPulsesPerSecond);
+ UpdateMediumEncoderFilterHz(kMaxMediumEncoderPulsesPerSecond);
}
void RunIteration() override {
- ::frc971::wpilib::SendRobotState(my_pid_);
-
{
auto drivetrain_message = drivetrain_queue.position.MakeMessage();
drivetrain_message->left_encoder =
@@ -135,8 +125,6 @@
drivetrain_message.Send();
}
-
- dma_synchronizer_->RunIteration();
}
};