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/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